diff options
Diffstat (limited to 'apps')
440 files changed, 76907 insertions, 0 deletions
diff --git a/apps/ChangeLog.txt b/apps/ChangeLog.txt new file mode 100755 index 000000000..8cd30ae83 --- /dev/null +++ b/apps/ChangeLog.txt @@ -0,0 +1,272 @@ +5.19 2011-03-12 Gregory Nutt <gnutt@nuttx.org> + + * Initial version of the apps/ directory was released as contributed by + Uros Platise. + +6.0 2011-03-21 Gregory Nutt <gnutt@nuttx.org> + + * README.txt -- README cosmetics + * hello/ -- hello world minor changes + * Makefile -- Makefile cosmetics (I am slowly adding the Darjeeling JVM) + * Make.defs -- New file adds common make definitions for applications. + * hello/Makefile -- Now uses new Make.defs definitions. Added README.txt. + * apps/poweroff -- New application to turn off board power. + * Moved NSH library, netutils, and examples from the nuttx/ directory to + the apps/ directory + * Moved exec_nuttapp machinery into the nuttapp/ directory. + +6.1 2011-04-10 Gregory Nutt <gnutt@nuttx.org> + + * Creation of auto-generated header files now occurs during the context + build phase. + * Added sdcard insert and eject, nsh command '?' and some code remarks + * Renamed nuttapp to namedapp + * namedapp/binfs.c -- Create a tiny filesystem that can be used + to show the internal named apps under /bin. + * Numerous fixes to build system required to support building with native + Windows toolchain. + +6.2 2011-05-06 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/nxffs: Add a test a a configuration that will be used to + verify NXFFS. + +6.3 2011-05-15 Gregory Nutt <gnutt@nuttx.org> + + * 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/. + * apps/netutils/dhcpc, dhcpcd, and tftp. If these directories are included + in the configuration but CONFIG_NET_UDP is disable (which is not very wise), + then a make error occurs because tools/mkdep.sh is called with no files. + * system/free: Move Uros' custom free command from vsn/free + * system/install: Add a new install command submitted by Uros Platise. + * examples/rgmp. Add a placeholder for an RGMP build example. + RGMP is a project for running GPOS and RTOS simultaneously on + multi-processor platforms. See http://rgmp.sourceforge.net/wiki/index.php/Main_Page + for further information about RGMP. NOTE: This is an empty example + on initial check-in. + +6.4 2011-06-06 Gregory Nutt <gnutt@nuttx.org> + + * nshlib/nsh_netcmds.c: If a network device name and IP address are provided + with the ifconfig command, then this command will now set the network address. + (Contributed by Yu Qiang). + * netutils/ftpc: A library to support client-side FTP. + * examples/ftpc: A simple add-on to the NSH. From NSH, you can start + this simple FTP shell to transfer files to/from a remote FTP server. + +6.5 2011-06-21 Gregory Nutt <gnutt@nuttx.org> + + * netutils/ftpc: Simpflication and size reduction. + +6.6 2011-07-11 Gregory Nutt <gnutt@nuttx.org> + + * Make.defs, namedapp/namedapp.c: Several structural changes made to get a + clean compile under the ez80 ZDS-II toolchain (no design changes). + * apps/examples/buttons: Add a test for the new standardized button interfaces + * apps/examples/nxtext: Add another NX graphics test. This one focus on + placing text on the background while pop-up windows occur. Text should + continue to update normally with or without the popup windows present. + +6.7 2011-08-02 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/nx and nxtext: These examples can now be built as NSH + "built-in" commands. + * apps/examples/nxhello: The simplest graphics example: It just says + "Hello, World!" in the center of the display. This example can also be + built as an NSH "built-in" command. + * apps/examples/nx, ntext, and nxhello: All updated to use the new + NuttX font interfaces. + * apps/examples/nximage: Another super simple graphics example: It just puts + the NuttX logo in the center of the display. This example can also be + built as an NSH "built-in" command. + * apps/examples/usbstorage: Can now be built as two NSH "built-in" commands: + 'msconn' will connect the USB mass storage device; 'msdis' will disconnect + the USB storage device. + * apps/examples/nx*: All NX header files moved from nuttx/include/nuttx to + nuttx/include/nuttx/nx. + * apps/examples/usbstorage: Added instrumentation to monitor memory usage + to check for memory leaks in the USB storage driver. + * apps/examples/nxhello/nxhello_bkgd.c: Fix handling of allocated glyph + memory. + +6.8 2011-08-11 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/nxlines: Added a test for NX line drawing capabilities. + +6.9 2011-09-11 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/nxlines: Extend the line drawing text to include drawing + of circles. + * apps/system/i2c: Add an I2C test tool that should help to bring up I2C + devices (when it is fully functional). + * apps/nshlib/nsh_timcmds.c: Add the date command that can be used to + show or set the time (only if CONFIG_RTC is set). + +6.10 2011-10-06 Gregory Nutt <gnutt@nuttx.org> + + * apps/system/i2c: Add repitition and address auto-incrementing so that + and command can be executed numerous times. Add a new verify command + that will write to a register, read from register, and verify that + returned value. + * apps/graphics/tiff: Add a library that can be used to create TIFF files. + * apps/examples/tiff: Add a unit test for the TIFF file creation logic + * apps/examples/lcdrw: Add a test to verify if you can or can or read + data from an LCD correctly. + * apps/examples/usbterm: A USB terminal example.. more of a USB chat or + serial bridge: Data received on local console echoed via USB serial; + data received on USB serial is echoed on the local console. + * apps/examples/touchscreen: Add a simple, generic test for any + touschscreen driver. + * Makefile: The apps/ Makefile now checks for an apps/external directory + or symbolic link. If such a directory/link exists (and has a Makefile), + it will be added to the apps/ build. This allows external directories + to be included into the apps/ build by simply creating a symbolic link. + +6.11 2011-11-12 Gregory Nutt <gnutt@nuttx.org> + + (No major changes from 6.10) + +6.12 2011-12-06 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/buttons: The button test can now be executed as an NSH + built in command. + +6.13 2012-12-26 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/dhcpd: May now be built as an NSH built-in application + by setting CONFIG_NSH_BUILTIN_APPS. + * apps/netutils/dhcpd/dhcpd.c: Fix several problems using host order address + where network addresses expected (and vice versa). + * apps/examples/nettest: May now be built as an NSH built-in application + by setting CONFIG_NSH_BUILTIN_APPS. + * apps/examples/nettest: Correct some build issues with the nettest is + built for performance evaluation. + * apps/examples/adc: Add a very simple test to drive and test an ADC + driver. + * apps/examples/pwm: Add an NSH PWM command to drive and test a PWM + driver. + * apps/examples/can: Add an NSH CAN command to drive and test a CAN + driver in loopback mode. + +6.14 2012-01-15 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/buttons/main.c: The test needs to call up_buttoninit() to + properly configure the button interrupt GPIOs. + * apps/examples/pwm: Add support to test the pulse count option recently + added to the PWM interface. + +6.15 2012-02-12 Gregory Nutt <gnutt@nuttx.org> + + * apps/nshlib/nsh_serial.c and nsh_usbdev.c: If NuttX is configured to use + a USB serial console, then NSH needs to wait until the USB console is + connected and available. + * apps/examples/composite: Add a test of the USB composite device. + * apps/examples/Telnetd: Move the tiny uIP shell example from + netutils/Telnetd to examples/Telnetd. Enhanced the Telnetd daemon so that + it supports Telnetd via a TTY device driver: A new TTY device driver is + created when each new Telnet connection is created. The shell thread + is started with stdin, stdout, and stderr mapped to the TTY device. + * netutils/Telnetd: The old uIP Telnet demo is gone. In its place is a new + Telnet infrastructure. The new Telnet daemon creates sessions that are + "wrapped" as character devices and mapped to stdin, stdout, and stderr. + Now the Telnet session can be inherited by spawned tasks. + * examples/Telnetd: Add a test for the new Telnet daemon. + * examples/Telnetd/telnetd_driver.c: Move the internal socket structure from + the daemon's socket array into the driver's state data so that it will be + independent from the the Telnetd daemon. + * apps/system/readline: Moved the old nuttx/lib/stdio/lib_fgets.c here + and renamed it as readline(). The old fgets was simplied and the overloaded + readline functionality was removed. + * apps/netutils/ftpd: Add an FTPD server (does not even compile on initial + checkin). + * apps/examples/ftpd: Add a test for the FTPD server (untest on initial + check-in). + * apps/nshlib/nsh_fscmds.c: Add support for a 'dmesg' command that will + dump the system log if CONFIG_SYSLOG is selected. + +6.16 2012-03-10 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/qencoder: Add a quadrature driver test. + * apps/examples/ostest/fpu.c: Add a test to verify that FPU registers + are properly saved and restored on context switches. + * apps/system/readline/readline.c: readline() will now treat either a + backspace or a DEL character as a backspace (i.e., deleting the character + to the left of the cursor). This makes NSH less dependent on particular + keyboard mappings of the Backspace key. Submitted by Mike Smith. + * apps/examples/cdcacm: An example that illustrates how the CDC/ACM driver + may to connected and disconnected through software control. + * apps/examples/nsh/nsh_main.c: If available, call up_cxxinitialize() to + initialize all statically defined C++ classes. + * apps/nshlib: Now supports a USB serial device for NSH console I/O. This + allows NSH to be used on boards that have USB but no serial connectors. + +6.17 2012-04-14 Gregory Nutt <gnutt@nuttx.org> + + * apps/examples/can: Add conditional compilation so that the test can be + configured to only send messages or to only receive messages. This will + let the test work in other modes than simple loopback testing. + * apps/examples/hello and apps/examples/ostest: Can now be built as NSH + built-int functions. + * vsn/hello: Removed. The modified apps/examples/hello is enough "Hello, + World!" + * apps/examples/nxconsole: Add a test of the NX console device. + * apps/examples/nxconsole: The NX console example now supports running + the NuttShell (NSH) within an NX window. + * apps/system/readline: Now uses standard definitions from + include/nuttx/ascii.h and vt100.h + * Kconfig, */Kconfig: Added skeleton Kconfig files to all directories that + may need them. + +6.18 2012-05-19 Gregory Nutt <gnutt@nuttx.org> + + * Kconfig: Continued Kconfig file updates (no longer tracking on a per-file + basis in the ChangeLog) + * apps/examples/watchdog: Add a watchdog timer example. + * apps/examples/tiff: Fix wrong path used for temporary file. + * apps/examples/touchscreen: Standardize the board-specific, touchscreen + initialization interfaces. + +6.19 2012-06-15 Gregory Nutt <gnutt@nuttx.org> + + * apps/nshlib/nsh_usbdev.c: Add the capability to use an arbitrary USB + device as the console (not necessarily /dev/console). This is a useful + option because then you can still use the serial console to debug with. + * apps/nshlib/nsh_usbdev.c: User now has to press ENTER 3 times before + USB console will start. Otherwise, the USB console starts before there + is anyone at the other end to listen. + * apps/nshlib/nsh_usbdev.c and nsh_consolemain.c: Add support for the USB + capability when a USB console is used. + * apps/nshlib/nsh_fscmds.c: Add the 'mv' command + +6.20 2012-07-12 Gregory Nutt <gnutt@nuttx.org> + + * namedapp/exec_namedapp.c - Correct an error when round robin scheduling + is enabled. The priority of the new, named application was erroneously + being set to the priority of the parent thread; losing its configured + priority. Reported by Mike Smith. + +6.21 2012-xx-xx Gregory Nutt <gnutt@nuttx.org> + + * apps/include/: Stylistic clean-up of all header files. + * apps/modbus and apps/include/modbus: A port of freemodbus-v1.5.0 + has been added to the NuttX apps/ source tree. + * apps/examples/modbus: A port of the freemodbus-v1.5.0 "demo" + program that will be used to verify the FreeModBus port + * apps/modbus: Don't use strerror(). It is just too big. + * apps/modbus: Add CONFIG_MB_TERMIOS. If the driver doesn't support + termios ioctls, then don't bother trying to configure the baud, parity + etc. + * apps/nslib: If waitpid() is supported, then NSH not catches the + return value from spawned applications (provided by Mike Smith) + * apps/nslib: Lock the schedule while starting built-in applications + in order to eliminate race conditions (also from Mike Smith). + * apps/examples/adc, pwm, and qencoder: Add support for testing + devices with multiple ADC, PWM, and QE devices. + * apps/nshlib/nsh_mntcmds.c: Separated mount-related commands out of + nsh_fscmds.c. Extended to the mount command so that if no arguments + are provided, then the current mountpoints are enumerated. + * apps/nshlib/nsh_mntcmds.c: Add an NSH df command to list the + properties of mounted file systems. diff --git a/apps/Kconfig b/apps/Kconfig new file mode 100644 index 000000000..1f38c58ba --- /dev/null +++ b/apps/Kconfig @@ -0,0 +1,36 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +menu "Named Applications" +source "$APPSDIR/namedapp/Kconfig" +endmenu + +menu "Examples" +source "$APPSDIR/examples/Kconfig" +endmenu + +menu "Interpreters" +source "$APPSDIR/interpreters/Kconfig" +endmenu + +menu "Network Utilities" +source "$APPSDIR/netutils/Kconfig" +endmenu + +menu "ModBus" +source "$APPSDIR/modbus/Kconfig" +endmenu + +menu "NSH Library" +source "$APPSDIR/nshlib/Kconfig" +endmenu + +menu "System NSH Add-Ons" +source "$APPSDIR/system/Kconfig" +endmenu + +menu "VSN board Add-Ons" +source "$APPSDIR/vsn/Kconfig" +endmenu diff --git a/apps/Make.defs b/apps/Make.defs new file mode 100644 index 000000000..f37a654c4 --- /dev/null +++ b/apps/Make.defs @@ -0,0 +1,41 @@ +############################################################################ +# apps/Make.defs +# Common make definitions provided to all applications +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +define REGISTER + @echo "Register: $1" + @echo "{ \"$1\", $2, $3, $4 }," >> "$(APPDIR)/namedapp/namedapp_list.h" + @echo "EXTERN int $4(int argc, char *argv[]);" >> "$(APPDIR)/namedapp/namedapp_proto.h" +endef diff --git a/apps/Makefile b/apps/Makefile new file mode 100644 index 000000000..f087b7c33 --- /dev/null +++ b/apps/Makefile @@ -0,0 +1,176 @@ +############################################################################ +# apps/Makefile +# +# Copyright (C) 2011-2012 Uros Platise. All rights reserved. +# Authors: Uros Platise <uros.platise@isotel.eu> +# Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs +-include $(TOPDIR)/.config + +APPDIR = ${shell pwd} + +# Application Directories + +# CONFIGURED_APPS is the list of all configured built-in directories/built +# action. It is created by the configured appconfig file (a copy of which +# appears in this directory as .config) +# SUBDIRS is the list of all directories containing Makefiles. It is used +# only for cleaning. namedapp must always be the first in the list. This +# list can be extended by the .config file as well + +CONFIGURED_APPS = +#SUBDIRS = examples graphics interpreters modbus namedapp nshlib netutils system vsn +ALL_SUBDIRS = $(dir $(shell /usr/bin/find . -name Makefile)) +SUBDIRS = namedapp/ $(filter-out ./ ./namedapp/ ./examples/,$(ALL_SUBDIRS)) + + +# There are two different mechanisms for obtaining the list of configured +# directories: +# +# (1) In the legacy method, these paths are all provided in the appconfig +# file that is copied to the top-level apps/ directory as .config +# (2) With the development of the NuttX configuration tool, however, the +# selected applications are now enabled by the configuration tool. +# The apps/.config file is no longer used. Instead, the set of +# configured build directories can be found by including a Make.defs +# file contained in each of the apps/subdirectories. +# +# When the NuttX configuration tools executes, it will always define the +# configure CONFIG_NUTTX_NEWCONFIG to select between these two cases. Then +# legacy appconfig files will still work but newly configuration files will +# also work. Eventually the CONFIG_NUTTX_NEWCONFIG option will be phased +# out. + +ifeq ($(CONFIG_NUTTX_NEWCONFIG),y) + +-include examples/Make.defs +-include graphics/Make.defs +-include interpreters/Make.defs +-include modbus/Make.defs +-include namedapp/Make.defs +-include netutils/Make.defs +-include nshlib/Make.defs +-include system/Make.defs +-include vsn/Make.defs + +# INSTALLED_APPS is the list of currently available application directories. It +# is the same as CONFIGURED_APPS, but filtered to exclude any non-existent +# application directory. namedapp is always in the list of applications to be +# built. + +INSTALLED_APPS = + +# The legacy case: + +else +-include .config + +# INSTALLED_APPS is the list of currently available application directories. It +# is the same as CONFIGURED_APPS, but filtered to exclude any non-existent +# application directory. namedapp is always in the list of applications to be +# built. + +INSTALLED_APPS = namedapp +endif + +# Create the list of available applications (INSTALLED_APPS) + +define ADD_BUILTIN +INSTALLED_APPS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi} +endef + +$(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN)))) + +# 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} + +# The final build target + +BIN = libapps$(LIBEXT) + +# Build targets + +all: $(BIN) +.PHONY: $(INSTALLED_APPS) context depend clean distclean + +$(INSTALLED_APPS): + @$(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 \ + rm -f $$dir/.context ; \ + $(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context ; \ + done + @touch $@ + +context: .context + +.depend: context Makefile $(SRCS) + @for dir in $(INSTALLED_APPS) ; do \ + rm -f $$dir/.depend ; \ + $(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend ; \ + done + @touch $@ + +depend: .depend + +clean: + @for dir in $(SUBDIRS) ; do \ + $(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + @rm -f $(BIN) *~ .*.swp *.o + $(call CLEAN) + +distclean: # clean + @for dir in $(SUBDIRS) ; do \ + $(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + @rm -f .config .context .depend + @( if [ -e external ]; then \ + echo "********************************************************"; \ + echo "* The external directory/link must be removed manually *"; \ + echo "********************************************************"; \ + fi; \ + ) + + diff --git a/apps/README.txt b/apps/README.txt new file mode 100644 index 000000000..6a78da4b5 --- /dev/null +++ b/apps/README.txt @@ -0,0 +1,211 @@ +Application Folder +================== + +Contents +-------- + + General + Directory Location + Named Applications + Named Startup main() function + NuttShell (NSH) Built-In Commands + Synchronous Built-In Commands + Application Configuration File + Example Named Application + Building NuttX with Board-Specific Pieces Outside the Source Tree + +General +------- +This folder provides various applications found in sub-directories. These +applications are not inherently a part of NuttX but are provided you help +you develop your own applications. The apps/ directory is a "break away" +part of the configuration that you may chose to use or not. + +Directory Location +------------------ +The default application directory used by the NuttX build should be named +apps/ (or apps-x.y/ where x.y is the NuttX version number). This apps/ +directory should appear in the directory tree at the same level as the +NuttX directory. Like: + + . + |- nuttx + | + `- apps + +If all of the above conditions are TRUE, then NuttX will be able to +find the application directory. If your application directory has a +different name or is location at a different position, then you will +have to inform the NuttX build system of that location. There are several +ways to do that: + +1) You can define CONFIG_APPS_DIR to be the full path to your application + directory in the NuttX configuration file. +2) You can provide the path to the application directory on the command line + like: make APPDIR=<path> or make CONFIG_APPS_DIR=<path> +3) When you configure NuttX using tools/configure.sh, you can provide that + path to the application directory on the configuration command line + like: ./configure.sh -a <app-dir> <board-name>/<config-name> + +Named Applications +------------------ +NuttX also supports applications that can be started using a name string. +In this case, application entry points with their requirements are gathered +together in two files: + + - namedapp/namedapp_proto.h Entry points, prototype function + - namedapp/namedapp_list.h Application specific information and requirements + +The build occurs in several phases as different build targets are executed: +(1) context, (2) depend, and (3) default (all). Application information is +collected during the make context build phase. + +To execute an application function: + + exec_namedapp() is defined in the nuttx/include/apps/apps.h + +NuttShell (NSH) Built-In Commands +--------------------------------- +One use of named applications is to provide a way of invoking your custom +application through the NuttShell (NSH) command line. NSH will support +a seamless method invoking the applications, when the following option is +enabled in the NuttX configuration file: + + CONFIG_NSH_BUILTIN_APPS=y + +Applications registered in the apps/namedapp/namedapp_list.h file will then +be accessible from the NSH command line. If you type 'help' at the NSH +prompt, you will see a list of the registered commands. + +Synchronous Built-In Commands +----------------------------- +By default, built-in commands started from the NSH command line will run +asynchronously with NSH. If you want to force NSH to execute commands +then wait for the command to execute, you can enable that feature by +adding the following to the NuttX configuration file: + +CONFIG_SCHED_WAITPID=y + +The configuration option enables support for the waitpid() RTOS interface. +When that interface is enabled, NSH will use it to wait, sleeping until +the built-in command executes to completion. + +Of course, even with CONFIG_SCHED_WAITPID=y defined, specific commands +can still be forced to run asynchronously by adding the ampersand (&) +after the NSH command. + +Application Configuration File +------------------------------ +A special configuration file is used to configure which applications +are to be included in the build. The source for this file is +configs/<board>/<configuration>/appconfig. The existence of the appconfig +file in the board configuration directory is sufficient to enable building +of applications. + +The appconfig file is copied into the apps/ directory as .config when +NuttX is configured. .config is included in the toplevel apps/Makefile. +As a minimum, this configuration file must define files to add to the +CONFIGURED_APPS list like: + + CONFIGURED_APPS += examples/hello vsn/poweroff + +Named Start-Up main() function +------------------------------ +A named application can even be used as the main, start-up entry point +into your embedded software. When the user defines this option in +the NuttX configuration file: + + CONFIG_BUILTIN_APP_START=<application name> + +that application shall be invoked immediately after system starts +*instead* of the normal, default "user_start" entry point. +Note that <application name> must be provided as: "hello", +will call: + + int hello_main(int argc, char *argv[]) + +Example Named Application +------------------------- +An example application skeleton can be found under the examples/hello +sub-directory. This example shows how a named application can be added +to the project. One must define: + + 1. create sub-directory as: appname + 2. provide entry point: appname_main() + 3. set the requirements in the file: Makefile, specially the lines: + + APPNAME = appname + PRIORITY = SCHED_PRIORITY_DEFAULT + STACKSIZE = 768 + ASRCS = asm source file list as a.asm b.asm ... + CSRCS = C source file list as foo1.c foo2.c .. + + 4. add application in the apps/.config + +Building NuttX with Board-Specific Pieces Outside the Source Tree +----------------------------------------------------------------- + +Q: Has anyone come up with a tidy way to build NuttX with board- + specific pieces outside the source tree? +A: Here are four: + + 1) There is a make target called 'make export'. It will build + NuttX, then bundle all of the header files, libaries, startup + objects, and other build components into a .zip file. You + can can move that .zip file into any build environment you + want. You even build NuttX under a DOS CMD window. + + This make target is documented in the top level nuttx/README.txt. + + 2) You can replace the entire apps/ directory. If there is + nothing in the apps/ directory that you need, you can define + CONFIG_APPS_DIR in your .config file so that it points to a + different, custom application directory. + + You can copy any pieces that you like from the old apps/directory + to your custom apps directory as necessary. + + This is documented in NuttX/configs/README.txt and + nuttx/Documentation/NuttxPortingGuide.html (Online at + http://nuttx.sourceforge.net/NuttxPortingGuide.html#apndxconfigs + under Build options). And in the apps/README.txt file. + + 3) If you like the random collection of stuff in the apps/ directory + but just want to expand the existing components with your own, + external sub-directory then there is an easy way to that too: + You just create the sympolic link at apps/external that + redirects to your application sub-directory. The apps/Makefile + will always automatically check for the existence of an + apps/external directory and if it exists, it will automatically + incorporate it into the build. + + This feature of the apps/Makefile is documented only here. + + You can, for example, create a script called install.sh that + installs a custom application, configuration, and board specific + directory: + + a) Copy 'MyBoard' directory to configs/MyBoard. + b) Add a symbolic link to MyApplication at apps/external + c) Configure NuttX (usually by: + + tools/configure.sh MyBoard/MyConfiguration + + or simply by copying defconfig->nutt/.config, + setenv.sh->nuttx/setenv.sh, Make.defs->nuttx/Make.defs, + appconfig->apps/.config + + Using the 'external' link makes it especially easy to add a + 'built-in' application an existing configuration. + + 4) Add any link to apps/ + + a) Add symbolic links apps/ to as many other directories as you + want. + b) Then just add the (relative) paths to the links in your + appconfig file (that becomes the apps/.config file). + + That is basically the same as my option #3 but doesn't use the + magic 'external' link. The toplevel apps/Makefile will always + to build whatever in finds in the apps/.config file (plus the + external link if present). diff --git a/apps/_dontignore b/apps/_dontignore new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/_dontignore diff --git a/apps/ardrone_control/.context b/apps/ardrone_control/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/ardrone_control/.context diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile new file mode 100644 index 000000000..23e2d49fb --- /dev/null +++ b/apps/ardrone_control/Makefile @@ -0,0 +1,45 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build uORB +# + +APPNAME = ardrone_control +PRIORITY = SCHED_PRIORITY_MAX - 15 +STACKSIZE = 2048 + +# explicit list of sources - not everything is built currently +CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c + +include $(APPDIR)/mk/app.mk diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c new file mode 100644 index 000000000..e49a0ae29 --- /dev/null +++ b/apps/ardrone_control/ardrone_control.c @@ -0,0 +1,272 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Implementation of AR.Drone 1.0 / 2.0 control interface + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <termios.h> +#include <time.h> +#include <sys/prctl.h> +#include <arch/board/up_hrt.h> +#include "ardrone_control.h" +#include "attitude_control.h" +#include "rate_control.h" +#include "ardrone_motor_control.h" +#include "position_control.h" +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/ardrone_control.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/ardrone_motors_setpoint.h> +#include <uORB/topics/sensor_combined.h> + +#include "ardrone_control_helper.h" + +__EXPORT int ardrone_control_main(int argc, char *argv[]); + +/**************************************************************************** + * Internal Definitions + ****************************************************************************/ + + +enum { + CONTROL_MODE_RATES = 0, + CONTROL_MODE_ATTITUDE = 1, +} control_mode; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/*File descriptors */ +int ardrone_write; +int gpios; + +bool position_control_thread_started; + +/**************************************************************************** + * pthread loops + ****************************************************************************/ +static void *position_control_loop(void *arg) +{ + struct vehicle_status_s *state = (struct vehicle_status_s *)arg; + // Set thread name + prctl(PR_SET_NAME, "ardrone pos ctrl", getpid()); + + while (1) { + if (state->state_machine == SYSTEM_STATE_AUTO) { +// control_position(); //FIXME TODO XXX + /* temporary 50 Hz execution */ + usleep(20000); + + } else { + position_control_thread_started = false; + break; + } + } + + return NULL; +} + +/**************************************************************************** + * main + ****************************************************************************/ + +int ardrone_control_main(int argc, char *argv[]) +{ + /* welcome user */ + printf("[ardrone_control] Control started, taking over motors\n"); + + /* default values for arguments */ + char *ardrone_uart_name = "/dev/ttyS1"; + control_mode = CONTROL_MODE_RATES; + + char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n"; + + /* read commandline arguments */ + int i; + + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set + if (argc > i + 1) { + ardrone_uart_name = argv[i + 1]; + + } else { + printf(commandline_usage); + return 0; + } + + } else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) { + if (argc > i + 1) { + if (strcmp(argv[i + 1], "rates") == 0) { + control_mode = CONTROL_MODE_RATES; + + } else if (strcmp(argv[i + 1], "attitude") == 0) { + control_mode = CONTROL_MODE_ATTITUDE; + + } else { + printf(commandline_usage); + return 0; + } + + } else { + printf(commandline_usage); + return 0; + } + } + } + + /* open uarts */ + printf("[ardrone_control] AR.Drone UART is %s\n", ardrone_uart_name); + ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY); + + /* initialize motors */ + ar_init_motors(ardrone_write, &gpios); + int counter = 0; + + /* pthread for position control */ + pthread_t position_control_thread; + position_control_thread_started = false; + + /* structures */ + struct vehicle_status_s state; + struct vehicle_attitude_s att; + struct ardrone_control_s ar_control; + struct rc_channels_s rc; + struct sensor_combined_s raw; + struct ardrone_motors_setpoint_s setpoint; + + /* subscribe to attitude, motor setpoints and system state */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); + + /* publish AR.Drone motor control state */ + int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control); + + while (1) { + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of rc */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + + if (state.state_machine == SYSTEM_STATE_AUTO) { + if (false == position_control_thread_started) { + pthread_attr_t position_control_thread_attr; + pthread_attr_init(&position_control_thread_attr); + pthread_attr_setstacksize(&position_control_thread_attr, 2048); + pthread_create(&position_control_thread, &position_control_thread_attr, position_control_loop, &state); + position_control_thread_started = true; + } + + control_attitude(&rc, &att, &state, ardrone_pub, &ar_control); + + //No check for remote sticks to disarm in auto mode, land/disarm with ground station + + } else if (state.state_machine == SYSTEM_STATE_MANUAL) { + if (control_mode == CONTROL_MODE_RATES) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint); + control_rates(&raw, &setpoint); + + } else if (control_mode == CONTROL_MODE_ATTITUDE) { + control_attitude(&rc, &att, &state, ardrone_pub, &ar_control); + } + + } else { + + } + + //fancy led animation... + static int blubb = 0; + + if (counter % 20 == 0) { + if (blubb == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); + + if (blubb == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); + + if (blubb == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); + + if (blubb == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); + + if (blubb == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); + + if (blubb == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); + + if (blubb == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); + + if (blubb == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); + + if (blubb == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); + + if (blubb == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); + + if (blubb == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); + + if (blubb == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); + + blubb++; + + if (blubb == 12) blubb = 0; + } + + /* run at approximately 200 Hz */ + usleep(5000); + counter++; + } + + /* close uarts */ + close(ardrone_write); + ar_multiplexing_deinit(gpios); + + printf("[ardrone_control] ending now...\r\n"); + fflush(stdout); + return 0; +} + diff --git a/apps/ardrone_control/ardrone_control.h b/apps/ardrone_control/ardrone_control.h new file mode 100644 index 000000000..7f9567f86 --- /dev/null +++ b/apps/ardrone_control/ardrone_control.h @@ -0,0 +1,12 @@ +/* + * ardrone_control.h + * + * Created on: Mar 23, 2012 + * Author: thomasgubler + */ + +#ifndef ARDRONE_CONTROL_H_ +#define ARDRONE_CONTROL_H_ + + +#endif /* ARDRONE_CONTROL_H_ */ diff --git a/apps/ardrone_control/ardrone_control_helper.c b/apps/ardrone_control/ardrone_control_helper.c new file mode 100644 index 000000000..c073119e0 --- /dev/null +++ b/apps/ardrone_control/ardrone_control_helper.c @@ -0,0 +1,60 @@ +#include "ardrone_control_helper.h" +#include <unistd.h> +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> + +// int read_sensors_raw(sensors_raw_t *sensors_raw) +// { +// static int ret; +// ret = global_data_wait(&global_data_sensors_raw->access_conf_rate_full); + +// if (ret == 0) { +// memcpy(sensors_raw->gyro_raw, global_data_sensors_raw->gyro_raw, sizeof(sensors_raw->gyro_raw)); +// // printf("Timestamp %d\n", &global_data_sensors_raw->timestamp); + +// } else { +// printf("Controller timeout, no new sensor values available\n"); +// } + +// global_data_unlock(&global_data_sensors_raw->access_conf_rate_full); +// return ret; +// } + +// int read_attitude(global_data_attitude_t *attitude) +// { + +// static int ret; +// ret = global_data_wait(&global_data_attitude->access_conf); + +// if (ret == 0) { +// memcpy(&attitude->roll, &global_data_attitude->roll, sizeof(global_data_attitude->roll)); +// memcpy(&attitude->pitch, &global_data_attitude->pitch, sizeof(global_data_attitude->pitch)); +// memcpy(&attitude->yaw, &global_data_attitude->yaw, sizeof(global_data_attitude->yaw)); +// memcpy(&attitude->rollspeed, &global_data_attitude->rollspeed, sizeof(global_data_attitude->rollspeed)); +// memcpy(&attitude->pitchspeed, &global_data_attitude->pitchspeed, sizeof(global_data_attitude->pitchspeed)); +// memcpy(&attitude->yawspeed, &global_data_attitude->yawspeed, sizeof(global_data_attitude->yawspeed)); + +// } else { +// printf("Controller timeout, no new attitude values available\n"); +// } + +// global_data_unlock(&global_data_attitude->access_conf); + + + +// return ret; +// } + +// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint) +// { + +// if (0 == global_data_trylock(&global_data_quad_motors_setpoint->access_conf)) { //TODO: check if trylock is the right choice, maybe only lock? +// rate_setpoint->motor_front_nw = global_data_quad_motors_setpoint->motor_front_nw; +// rate_setpoint->motor_right_ne = global_data_quad_motors_setpoint->motor_right_ne; +// rate_setpoint->motor_back_se = global_data_quad_motors_setpoint->motor_back_se; +// rate_setpoint->motor_left_sw = global_data_quad_motors_setpoint->motor_left_sw; + +// global_data_unlock(&global_data_quad_motors_setpoint->access_conf); +// } +// } diff --git a/apps/ardrone_control/ardrone_control_helper.h b/apps/ardrone_control/ardrone_control_helper.h new file mode 100644 index 000000000..22eebe986 --- /dev/null +++ b/apps/ardrone_control/ardrone_control_helper.h @@ -0,0 +1,21 @@ +/* + * ardrone_control_helper.h + * + * Created on: May 15, 2012 + * Author: thomasgubler + */ + +#ifndef ARDRONE_CONTROL_HELPER_H_ +#define ARDRONE_CONTROL_HELPER_H_ + +#include <stdint.h> + +// typedef struct { +// int16_t gyro_raw[3]; // l3gd20 +// } sensors_raw_t; + +// /* Copy quad_motors_setpoint values from global memory to private variables */ //TODO: change this once the new mavlink message for rates is available +// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint); + + +#endif /* ARDRONE_CONTROL_HELPER_H_ */ diff --git a/apps/ardrone_control/ardrone_motor_control.c b/apps/ardrone_control/ardrone_motor_control.c new file mode 100644 index 000000000..25847e1da --- /dev/null +++ b/apps/ardrone_control/ardrone_motor_control.c @@ -0,0 +1,277 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Implementation of AR.Drone 1.0 / 2.0 motor control interface + */ + + + +#include "ardrone_motor_control.h" + +static const unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2; +static const unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 }; + +typedef union { + uint16_t motor_value; + uint8_t bytes[2]; +} motor_union_t; + +/** + * @brief Generate the 8-byte motor set packet + * + * @return the number of bytes (8) + */ +void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) +{ + motor_buf[0] = 0x20; + motor_buf[1] = 0x00; + motor_buf[2] = 0x00; + motor_buf[3] = 0x00; + motor_buf[4] = 0x00; + /* + * {0x20, 0x00, 0x00, 0x00, 0x00}; + * 0x20 is start sign / motor command + */ + motor_union_t curr_motor; + uint16_t nineBitMask = 0x1FF; + + /* Set motor 1 */ + curr_motor.motor_value = (motor1 & nineBitMask) << 4; + motor_buf[0] |= curr_motor.bytes[1]; + motor_buf[1] |= curr_motor.bytes[0]; + + /* Set motor 2 */ + curr_motor.motor_value = (motor2 & nineBitMask) << 3; + motor_buf[1] |= curr_motor.bytes[1]; + motor_buf[2] |= curr_motor.bytes[0]; + + /* Set motor 3 */ + curr_motor.motor_value = (motor3 & nineBitMask) << 2; + motor_buf[2] |= curr_motor.bytes[1]; + motor_buf[3] |= curr_motor.bytes[0]; + + /* Set motor 4 */ + curr_motor.motor_value = (motor4 & nineBitMask) << 1; + motor_buf[3] |= curr_motor.bytes[1]; + motor_buf[4] |= curr_motor.bytes[0]; +} + +void ar_enable_broadcast(int fd) +{ + ar_select_motor(fd, 0); +} + +int ar_multiplexing_init() +{ + int fd; + + fd = open("/dev/gpio", O_RDONLY | O_NONBLOCK); + + if (fd < 0) { + printf("GPIO: open fail\n"); + return fd; + } + + if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) { + printf("GPIO: output set fail\n"); + close(fd); + return -1; + } + + /* deactivate all outputs */ + int ret = 0; + ret += ioctl(fd, GPIO_SET, motor_gpios); + + if (ret < 0) { + printf("GPIO: clearing pins fail\n"); + close(fd); + return -1; + } + + return fd; +} + +int ar_multiplexing_deinit(int fd) +{ + if (fd < 0) { + printf("GPIO: no valid descriptor\n"); + return fd; + } + + int ret = 0; + + /* deselect motor 1-4 */ + ret += ioctl(fd, GPIO_SET, motor_gpios); + + if (ret != 0) { + printf("GPIO: clear failed %d times\n", ret); + } + + if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) { + printf("GPIO: input set fail\n"); + return -1; + } + + close(fd); + + return ret; +} + +int ar_select_motor(int fd, uint8_t motor) +{ + int ret = 0; + unsigned long gpioset; + /* + * Four GPIOS: + * GPIO_EXT1 + * GPIO_EXT2 + * GPIO_UART2_CTS + * GPIO_UART2_RTS + */ + + /* select motor 0 to enable broadcast */ + if (motor == 0) { + /* select motor 1-4 */ + ret += ioctl(fd, GPIO_CLEAR, motor_gpios); + + } else { + /* select reqested motor */ + ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); + + /* deselect all others */ + gpioset = motor_gpios ^ motor_gpio[motor - 1]; + ret += ioctl(fd, GPIO_SET, gpioset); + } + + return ret; +} + +void ar_init_motors(int ardrone_uart, int *gpios_pin) +{ + /* Initialize multiplexing */ + *gpios_pin = ar_multiplexing_init(); + + /* Write ARDrone commands on UART2 */ + uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40}; + uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0}; + + /* initialize all motors + * - select one motor at a time + * - configure motor + */ + int i; + int errcounter = 0; + + for (i = 1; i < 5; ++i) { + /* Initialize motors 1-4 */ + initbuf[3] = i; + errcounter += ar_select_motor(*gpios_pin, i); + + write(ardrone_uart, initbuf + 0, 1); + + /* sleep 400 ms */ + usleep(200000); + usleep(200000); + + write(ardrone_uart, initbuf + 1, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 2, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 3, 1); + /* wait 50 ms */ + usleep(50000); + + write(ardrone_uart, initbuf + 4, 1); + /* wait 50 ms */ + usleep(50000); + + /* enable multicast */ + write(ardrone_uart, multicastbuf + 0, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 1, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 2, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 3, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 4, 1); + /* wait 1 ms */ + usleep(1000); + + write(ardrone_uart, multicastbuf + 5, 1); + /* wait 5 ms */ + usleep(50000); + } + + /* start the multicast part */ + errcounter += ar_select_motor(*gpios_pin, 0); + + if (errcounter != 0) { + printf("AR: init sequence incomplete, failed %d times", -errcounter); + fflush(stdout); + } +} + +/* + * Sets the leds on the motor controllers, 1 turns led on, 0 off. + */ +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) +{ + /* + * 2 bytes are sent. The first 3 bits describe the command: 011 means led control + * the following 4 bits are the red leds for motor 4, 3, 2, 1 + * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1 + * the last bit is unknown. + * + * The packet is therefore: + * 011 rrrr 0000 gggg 0 + */ + uint8_t leds[2]; + leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); + leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); + write(ardrone_uart, leds, 2); +} diff --git a/apps/ardrone_control/ardrone_motor_control.h b/apps/ardrone_control/ardrone_motor_control.h new file mode 100644 index 000000000..bf41e3f3b --- /dev/null +++ b/apps/ardrone_control/ardrone_motor_control.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * px4/ardrone_offboard_control.h + * + * Copyright (C) 2012 PX4 Autopilot Project. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ +#include <nuttx/config.h> +#include <pthread.h> +#include <poll.h> +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <drivers/drv_gpio.h> + + +/** + * @brief Generate the 8-byte motor set packet + * + * @return the number of bytes (8) + */ +void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4); + +int ar_select_motor(int fd, uint8_t motor); + +void ar_enable_broadcast(int fd); + +int ar_multiplexing_init(void); +int ar_multiplexing_deinit(int fd); + +void ar_init_motors(int ardrone_uart, int *gpios_uart); + +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green); + diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c new file mode 100644 index 000000000..2d2596d6d --- /dev/null +++ b/apps/ardrone_control/attitude_control.c @@ -0,0 +1,435 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Implementation of attitude controller + */ + +#include "attitude_control.h" +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdint.h> +#include <stdbool.h> +#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else? +#include "ardrone_motor_control.h" +#include <float.h> +#include <math.h> +#include "pid.h" +#include <arch/board/up_hrt.h> + +extern int ardrone_write; +extern int gpios; + +#define CONTROL_PID_ATTITUDE_INTERVAL 5e-3 + +void turn_xy_plane(const float_vect3 *vector, float yaw, + float_vect3 *result) +{ + //turn clockwise + static uint16_t counter; + + result->x = (cos(yaw) * vector->x + sin(yaw) * vector->y); + result->y = (-sin(yaw) * vector->x + cos(yaw) * vector->y); + result->z = vector->z; //leave direction normal to xy-plane untouched + + counter++; +} + +void navi2body_xy_plane(const float_vect3 *vector, const float yaw, + float_vect3 *result) +{ + turn_xy_plane(vector, yaw, result); +// result->x = vector->x; +// result->y = vector->y; +// result->z = vector->z; + // result->x = cos(yaw) * vector->x + sin(yaw) * vector->y; + // result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y; + // result->z = vector->z; //leave direction normal to xy-plane untouched +} + +void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control) +{ + static int motor_skip_counter = 0; + + static PID_t yaw_pos_controller; + static PID_t yaw_speed_controller; + static PID_t nick_controller; + static PID_t roll_controller; + + static const float min_gas = 1; + static const float max_gas = 512; + static uint16_t motor_pwm[4] = {0, 0, 0, 0}; + static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f}; +// static float remote_control_weight_z = 1; +// static float position_control_weight_z = 0; + + static float pid_yawpos_lim; + static float pid_yawspeed_lim; + static float pid_att_lim; + + static bool initialized; + + static float_vect3 attitude_setpoint_navigationframe_from_positioncontroller; + + static hrt_abstime now_time; + static hrt_abstime last_time; + + static commander_state_machine_t current_state; + + /* initialize the pid controllers when the function is called for the first time */ + if (initialized == false) { + + pid_init(&yaw_pos_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU], + PID_MODE_DERIVATIV_CALC, 154); + + pid_init(&yaw_speed_controller, + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU], + PID_MODE_DERIVATIV_CALC, 155); + + pid_init(&nick_controller, + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], + PID_MODE_DERIVATIV_SET, 156); + + pid_init(&roll_controller, + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU], + PID_MODE_DERIVATIV_SET, 157); + + pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; + pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; + pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; + + //TODO: true initialization? get gps while on ground? + attitude_setpoint_navigationframe_from_positioncontroller.x = 0.0f; + attitude_setpoint_navigationframe_from_positioncontroller.y = 0.0f; + attitude_setpoint_navigationframe_from_positioncontroller.z = 0.0f; + + last_time = 0; + initialized = true; + } + + /* load new parameters with lower rate */ + if (motor_skip_counter % 50 == 0) { + pid_set_parameters(&yaw_pos_controller, + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]); + + pid_set_parameters(&yaw_speed_controller, + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]); + + pid_set_parameters(&nick_controller, + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); + + pid_set_parameters(&roll_controller, + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D], + (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]); + + pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM]; + pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM]; + pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM]; + } + + current_state = status->state_machine; + float_vect3 attitude_setpoint_bodyframe = {}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller + + if (current_state == SYSTEM_STATE_AUTO) { + + attitude_setpoint_navigationframe_from_positioncontroller.x = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[0]; + attitude_setpoint_navigationframe_from_positioncontroller.y = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[1]; + attitude_setpoint_navigationframe_from_positioncontroller.z = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[2]; + + float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z; + + // don't turn around the wrong side (only works if yaw angle is between +- 180 degree) + if (yaw_e > M_PI) { + yaw_e -= 2.0f * M_PI; + } + + if (yaw_e < -M_PI) { + yaw_e += 2.0f * M_PI; + } + + attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL); + + + /* limit control output */ + if (attitude_setpoint_navigationframe_from_positioncontroller.z > pid_yawpos_lim) { + attitude_setpoint_navigationframe_from_positioncontroller.z = pid_yawpos_lim; + yaw_pos_controller.saturated = 1; + } + + if (attitude_setpoint_navigationframe_from_positioncontroller.z < -pid_yawpos_lim) { + attitude_setpoint_navigationframe_from_positioncontroller.z = -pid_yawpos_lim; + yaw_pos_controller.saturated = 1; + } + + //transform attitude setpoint from position controller from navi to body frame on xy_plane + float_vect3 attitude_setpoint_bodyframe_from_positioncontroller; + navi2body_xy_plane(&attitude_setpoint_navigationframe_from_positioncontroller, att->yaw , &attitude_setpoint_bodyframe_from_positioncontroller); //yaw angle= att->yaw + //now everything is in body frame + + + //TODO: here we decide which input (position controller or ppm) we use. For now we have only the ppm, this should be decided dpending on the state machione (manula or auto) ppm should always overwrite auto (?) + attitude_setpoint_bodyframe.x = attitude_setpoint_bodyframe_from_positioncontroller.x; + attitude_setpoint_bodyframe.y = attitude_setpoint_bodyframe_from_positioncontroller.y; + attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z; + + } else if (current_state == SYSTEM_STATE_MANUAL) { + attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * 3.14159265 / 8.0f; + attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * 3.14159265 / 8.0f; + attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * 3.14159265; + } + + /* add an attitude offset which needs to be estimated somewhere */ + attitude_setpoint_bodyframe.x += global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET]; + attitude_setpoint_bodyframe.y += global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET]; + + /*Calculate Controllers*/ + //control Nick + float nick = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL); + //control Roll + float roll = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL); + //control Yaw Speed + float yaw = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed! + + //compensation to keep force in z-direction + float zcompensation; + + if (fabs(att->roll) > 0.5f) { + zcompensation = 1.13949393f; + + } else { + zcompensation = 1.0f / cosf(att->roll); + } + + if (fabs(att->pitch) > 0.5f) { + zcompensation *= 1.13949393f; + + } else { + zcompensation *= 1.0f / cosf(att->pitch); + } + + // use global_data.position_control_output.z and mix parameter global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] + // to compute thrust for Z position control + // + // float motor_thrust = min_gas + + // ( ( 1 - global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] ) * ( max_gas - min_gas ) * global_data.gas_remote * zcompensation ) + // + ( global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] * ( max_gas - min_gas ) * controlled_thrust * zcompensation ); + //calculate the basic thrust + + + + float motor_thrust = 0; + + // FLYING MODES + if (current_state == SYSTEM_STATE_MANUAL) { + motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; + + } else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) { + motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; //TODO + + } else if (current_state == SYSTEM_STATE_EMCY_LANDING) { + motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; //TODO + + } else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) { + motor_thrust = 0; //immediately cut off thrust! + + } else { + motor_thrust = 0; // Motor thrust must be zero in any other mode! + } + + // Convertion to motor-step units + motor_thrust *= zcompensation; + motor_thrust *= max_gas / 20000.0f; //TODO: check this + motor_thrust += (max_gas - min_gas) / 2.f; + + //limit control output + //yawspeed + if (yaw > pid_yawspeed_lim) { + yaw = pid_yawspeed_lim; + yaw_speed_controller.saturated = 1; + } + + if (yaw < -pid_yawspeed_lim) { + yaw = -pid_yawspeed_lim; + yaw_speed_controller.saturated = 1; + } + + if (nick > pid_att_lim) { + nick = pid_att_lim; + nick_controller.saturated = 1; + } + + if (nick < -pid_att_lim) { + nick = -pid_att_lim; + nick_controller.saturated = 1; + } + + + if (roll > pid_att_lim) { + roll = pid_att_lim; + roll_controller.saturated = 1; + } + + if (roll < -pid_att_lim) { + roll = -pid_att_lim; + roll_controller.saturated = 1; + } + + /* Emit controller values */ + ar_control->setpoint_thrust_cast = motor_thrust; + ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x; + ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y; + ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z; + ar_control->attitude_control_output[0] = roll; + ar_control->attitude_control_output[1] = nick; + ar_control->attitude_control_output[2] = yaw; + ar_control->zcompensation = zcompensation; + orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control); + + static float output_band = 0.f; + static float band_factor = 0.75f; + static float startpoint_full_control = 150.0f; //TODO + static float yaw_factor = 1.0f; + + if (motor_thrust <= min_gas) { + motor_thrust = min_gas; + output_band = 0.f; + + } else if (motor_thrust < startpoint_full_control && motor_thrust > min_gas) { + output_band = band_factor * (motor_thrust - min_gas); + + } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_gas - band_factor * startpoint_full_control) { + output_band = band_factor * startpoint_full_control; + + } else if (motor_thrust >= max_gas - band_factor * startpoint_full_control) { + output_band = band_factor * (max_gas - motor_thrust); + } + + //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer + + // FRONT (MOTOR 1) + motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw); + + // RIGHT (MOTOR 2) + motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw); + + // BACK (MOTOR 3) + motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw); + + // LEFT (MOTOR 4) + motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw); + + // if we are not in the output band + if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band + && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band + && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band + && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { + + yaw_factor = 0.5f; + // FRONT (MOTOR 1) + motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw * yaw_factor); + + // RIGHT (MOTOR 2) + motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw * yaw_factor); + + // BACK (MOTOR 3) + motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw * yaw_factor); + + // LEFT (MOTOR 4) + motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw * yaw_factor); + } + + uint8_t i; + + for (i = 0; i < 4; i++) { + //check for limits + if (motor_calc[i] < motor_thrust - output_band) { + motor_calc[i] = motor_thrust - output_band; + } + + if (motor_calc[i] > motor_thrust + output_band) { + motor_calc[i] = motor_thrust + output_band; + } + } + + // Write out actual thrust + motor_pwm[0] = (uint16_t) motor_calc[0]; + motor_pwm[1] = (uint16_t) motor_calc[1]; + motor_pwm[2] = (uint16_t) motor_calc[2]; + motor_pwm[3] = (uint16_t) motor_calc[3]; + + //SEND MOTOR COMMANDS + uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5}; + ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); + write(ardrone_write, motorSpeedBuf, 5); + + motor_skip_counter++; + +// now_time = hrt_absolute_time() / 1000000; +// if(now_time - last_time > 0) +// { +// printf("Counter: %ld\n",control_counter); +// last_time = now_time; +// control_counter = 0; +// } +} diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h new file mode 100644 index 000000000..6f66926c0 --- /dev/null +++ b/apps/ardrone_control/attitude_control.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file attitude control for quadrotors */ + +#ifndef ATTITUDE_CONTROL_H_ +#define ATTITUDE_CONTROL_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/ardrone_control.h> +#include <uORB/topics/vehicle_status.h> + +void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, + const struct vehicle_status_s *status, int ardrone_pub, + struct ardrone_control_s *ar_control); + +#endif /* ATTITUDE_CONTROL_H_ */ diff --git a/apps/ardrone_control/pid.c b/apps/ardrone_control/pid.c new file mode 100644 index 000000000..5ce05670e --- /dev/null +++ b/apps/ardrone_control/pid.c @@ -0,0 +1,109 @@ +#include "pid.h" + +#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else? + +void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, + uint8_t mode, uint8_t plot_i) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->intmax = intmax; + pid->mode = mode; + pid->plot_i = plot_i; + pid->count = 0; + pid->saturated = 0; + + pid->sp = 0; + pid->error_previous = 0; + pid->integral = 0; +} +void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->intmax = intmax; + // pid->mode = mode; + + // pid->sp = 0; + // pid->error_previous = 0; + // pid->integral = 0; +} + +//void pid_set(PID_t *pid, float sp) +//{ +// pid->sp = sp; +// pid->error_previous = 0; +// pid->integral = 0; +//} + +/** + * + * @param pid + * @param val + * @param dt + * @return + */ +float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +{ + /* error = setpoint - actual_position + integral = integral + (error*dt) + derivative = (error - previous_error)/dt + output = (Kp*error) + (Ki*integral) + (Kd*derivative) + previous_error = error + wait(dt) + goto start + */ + + float i, d; + pid->sp = sp; + float error = pid->sp - val; + + if (pid->saturated && (pid->integral * error > 0)) { + //Output is saturated and the integral would get bigger (positive or negative) + i = pid->integral; + + //Reset saturation. If we are still saturated this will be set again at output limit check. + pid->saturated = 0; + + } else { + i = pid->integral + (error * dt); + } + + // Anti-Windup. Needed if we don't use the saturation above. + if (pid->intmax != 0.0) { + if (i > pid->intmax) { + pid->integral = pid->intmax; + + } else if (i < -pid->intmax) { + + pid->integral = -pid->intmax; + + } else { + pid->integral = i; + } + + //Send Controller integrals + // Disabled because of new possibilities with debug_vect. + // Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens + // if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1)) + // { + // mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral); + // } + } + + if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / dt; + + } else if (pid->mode == PID_MODE_DERIVATIV_SET) { + d = -val_dot; + + } else { + d = 0; + } + + pid->error_previous = error; + + return (error * pid->kp) + (i * pid->ki) + (d * pid->kd); +} diff --git a/apps/ardrone_control/pid.h b/apps/ardrone_control/pid.h new file mode 100644 index 000000000..a721c839c --- /dev/null +++ b/apps/ardrone_control/pid.h @@ -0,0 +1,40 @@ +/* + * pid.h + * + * Created on: May 29, 2012 + * Author: thomasgubler + */ + +#ifndef PID_H_ +#define PID_H_ + +#include <stdint.h> + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC 0 +/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ +#define PID_MODE_DERIVATIV_SET 1 + +typedef struct { + float kp; + float ki; + float kd; + float intmax; + float sp; + float integral; + float error_previous; + uint8_t mode; + uint8_t plot_i; + uint8_t count; + uint8_t saturated; +} PID_t; + +void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i); +void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax); +//void pid_set(PID_t *pid, float sp); +float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); + + + +#endif /* PID_H_ */ diff --git a/apps/ardrone_control/position_control.c b/apps/ardrone_control/position_control.c new file mode 100644 index 000000000..6ba165459 --- /dev/null +++ b/apps/ardrone_control/position_control.c @@ -0,0 +1,308 @@ +/**************************************************************************** + * ardrone_control/position_control.c + * + * Copyright (C) 2008, 2012 Thomas Gubler, Julian Oes, Lorenz Meier. All rights reserved. + * Author: Based on the pixhawk quadrotor controller + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#include "position_control.h" +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdint.h> +#include <math.h> +#include <stdbool.h> +#include <float.h> +#include "pid.h" + +#ifndef FM_PI +#define FM_PI 3.1415926535897932384626433832795f +#endif + + + +#define CONTROL_PID_POSITION_INTERVAL 0.020 + +int read_lock_position(global_data_position_t *position) +{ + static int ret; + ret = global_data_wait(&global_data_position->access_conf); + + if (ret == 0) { + memcpy(&position->lat, &global_data_position->lat, sizeof(global_data_position->lat)); + memcpy(&position->lon, &global_data_position->lon, sizeof(global_data_position->lon)); + memcpy(&position->alt, &global_data_position->alt, sizeof(global_data_position->alt)); + memcpy(&position->relative_alt, &global_data_position->relative_alt, sizeof(global_data_position->relative_alt)); + memcpy(&position->vx, &global_data_position->vx, sizeof(global_data_position->vx)); + memcpy(&position->vy, &global_data_position->vy, sizeof(global_data_position->vy)); + memcpy(&position->vz, &global_data_position->vz, sizeof(global_data_position->vz)); + memcpy(&position->hdg, &global_data_position->hdg, sizeof(global_data_position->hdg)); + + + } else { + printf("Controller timeout, no new position values available\n"); + } + + global_data_unlock(&global_data_position->access_conf); + return ret; +} + +float get_distance_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next) +{ + double lat_now_rad = lat_now / 180.0 * M_PI; + double lon_now_rad = lon_now / 180.0 * M_PI; + double lat_next_rad = lat_next / 180.0 * M_PI; + double lon_next_rad = lon_next / 180.0 * M_PI; + + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + const double radius_earth = 6371000.0; + + return radius_earth * c; +} + +float get_bearing_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next) +{ + double lat_now_rad = lat_now / 180.0 * M_PI; + double lon_now_rad = lon_now / 180.0 * M_PI; + double lat_next_rad = lat_next / 180.0 * M_PI; + double lon_next_rad = lon_next / 180.0 * M_PI; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + if (theta < 0) { + theta = theta + 2 * FM_PI; + } + + return theta; +} + +void control_position(void) +{ + static PID_t distance_controller; + + static int read_ret; + static global_data_position_t position_estimated; + + static uint16_t counter; + + static bool initialized; + static uint16_t pm_counter; + + static float lat_next; + static float lon_next; + + static float pitch_current; + + static float thrust_total; + + + if (initialized == false) { + + global_data_lock(&global_data_parameter_storage->access_conf); + + pid_init(&distance_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], + PID_MODE_DERIVATIV_CALC, 150);//150 + +// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; +// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; + + pm_counter = global_data_parameter_storage->counter; + + global_data_unlock(&global_data_parameter_storage->access_conf); + + thrust_total = 0.0f; + + /* Position initialization */ + /* Wait for new position estimate */ + do { + read_ret = read_lock_position(&position_estimated); + } while (read_ret != 0); + + lat_next = position_estimated.lat; + lon_next = position_estimated.lon; + + /* attitude initialization */ + global_data_lock(&global_data_attitude->access_conf); + pitch_current = global_data_attitude->pitch; + global_data_unlock(&global_data_attitude->access_conf); + + initialized = true; + } + + /* load new parameters with 10Hz */ + if (counter % 50 == 0) { + if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { + /* check whether new parameters are available */ + if (global_data_parameter_storage->counter > pm_counter) { + pid_set_parameters(&distance_controller, + global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); + +// +// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; +// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; + + pm_counter = global_data_parameter_storage->counter; + printf("Position controller changed pid parameters\n"); + } + } + + global_data_unlock(&global_data_parameter_storage->access_conf); + } + + + /* Wait for new position estimate */ + do { + read_ret = read_lock_position(&position_estimated); + } while (read_ret != 0); + + /* Get next waypoint */ //TODO: add local copy + + if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { + lat_next = global_data_position_setpoint->x; + lon_next = global_data_position_setpoint->y; + global_data_unlock(&global_data_position_setpoint->access_conf); + } + + /* Get distance to waypoint */ + float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); +// if(counter % 5 == 0) +// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); + + /* Get bearing to waypoint (direction on earth surface to next waypoint) */ + float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); + + if (counter % 5 == 0) + printf("bearing: %.4f\n", bearing); + + /* Calculate speed in direction of bearing (needed for controller) */ + float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); +// if(counter % 5 == 0) +// printf("speed_norm: %.4f\n", speed_norm); + float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller + + /* Control Thrust in bearing direction */ + float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, + CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else + +// if(counter % 5 == 0) +// printf("horizontal thrust: %.4f\n", horizontal_thrust); + + /* Get total thrust (from remote for now) */ + if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { + thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? + global_data_unlock(&global_data_rc_channels->access_conf); + } + + const float max_gas = 500.0f; + thrust_total *= max_gas / 20000.0f; //TODO: check this + thrust_total += max_gas / 2.0f; + + + if (horizontal_thrust > thrust_total) { + horizontal_thrust = thrust_total; + + } else if (horizontal_thrust < -thrust_total) { + horizontal_thrust = -thrust_total; + } + + + + //TODO: maybe we want to add a speed controller later... + + /* Calclulate thrust in east and north direction */ + float thrust_north = cosf(bearing) * horizontal_thrust; + float thrust_east = sinf(bearing) * horizontal_thrust; + + if (counter % 10 == 0) { + printf("thrust north: %.4f\n", thrust_north); + printf("thrust east: %.4f\n", thrust_east); + fflush(stdout); + } + + /* Get current attitude */ + if (0 == global_data_trylock(&global_data_attitude->access_conf)) { + pitch_current = global_data_attitude->pitch; + global_data_unlock(&global_data_attitude->access_conf); + } + + /* Get desired pitch & roll */ + float pitch_desired = 0.0f; + float roll_desired = 0.0f; + + if (thrust_total != 0) { + float pitch_fraction = -thrust_north / thrust_total; + float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); + + if (roll_fraction < -1) { + roll_fraction = -1; + + } else if (roll_fraction > 1) { + roll_fraction = 1; + } + +// if(counter % 5 == 0) +// { +// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); +// fflush(stdout); +// } + + pitch_desired = asinf(pitch_fraction); + roll_desired = asinf(roll_fraction); + } + + /*Broadcast desired angles */ + global_data_lock(&global_data_ardrone_control->access_conf); + global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[0] = roll_desired; + global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[1] = pitch_desired; + global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[2] = bearing; //TODO: add yaw setpoint + global_data_unlock(&global_data_ardrone_control->access_conf); + global_data_broadcast(&global_data_ardrone_control->access_conf); + + + counter++; +} diff --git a/apps/ardrone_control/position_control.h b/apps/ardrone_control/position_control.h new file mode 100644 index 000000000..dbc0650e6 --- /dev/null +++ b/apps/ardrone_control/position_control.h @@ -0,0 +1,13 @@ +/* + * position_control.h + * + * Created on: May 29, 2012 + * Author: thomasgubler + */ + +#ifndef POSITION_CONTROL_H_ +#define POSITION_CONTROL_H_ + +void control_position(void); + +#endif /* POSITION_CONTROL_H_ */ diff --git a/apps/ardrone_control/rate_control.c b/apps/ardrone_control/rate_control.c new file mode 100644 index 000000000..4abba6255 --- /dev/null +++ b/apps/ardrone_control/rate_control.c @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <nagelit@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Implementation of attitude rate control + */ + +#include "rate_control.h" +#include "ardrone_control_helper.h" +#include "ardrone_motor_control.h" +#include <arch/board/up_hrt.h> + +extern int ardrone_write; +extern int gpios; + +typedef struct { + uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration + uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration + uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration + uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration + uint8_t target_system; ///< System ID of the system that should set these motor commands +} quad_motors_setpoint_t; + + +void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints) +{ + static quad_motors_setpoint_t actuators_desired; + //static quad_motors_setpoint_t quad_motors_setpoint_desired; + + static int16_t outputBand = 0; + +// static uint16_t control_counter; + static hrt_abstime now_time; + static hrt_abstime last_time; + + static float setpointXrate; + static float setpointYrate; + static float setpointZrate; + + static float setpointRateCast[3]; + static float Kp; +// static float Ki; + static float setpointThrustCast; + static float startpointFullControll; + static float maxThrustSetpoints; + + static float gyro_filtered[3]; + static float gyro_filtered_offset[3]; + static float gyro_alpha; + static float gyro_alpha_offset; +// static float errXrate; + static float attRatesScaled[3]; + + static uint16_t offsetCnt; +// static float antiwindup; + static int motor_skip_counter; + + static int read_ret; + + static bool initialized; + + if (initialized == false) { + initialized = true; + + /* Read sensors for initial values */ + + gyro_filtered_offset[0] = 0.00026631611f * (float)raw->gyro_raw[0]; + gyro_filtered_offset[1] = 0.00026631611f * (float)raw->gyro_raw[1]; + gyro_filtered_offset[2] = 0.00026631611f * (float)raw->gyro_raw[2]; + + gyro_filtered[0] = 0.00026631611f * (float)raw->gyro_raw[0]; + gyro_filtered[1] = 0.00026631611f * (float)raw->gyro_raw[1]; + gyro_filtered[2] = 0.00026631611f * (float)raw->gyro_raw[2]; + + outputBand = 0; + startpointFullControll = 150.0f; + maxThrustSetpoints = 511.0f; + //Kp=60; + //Kp=40.0f; + //Kp=45; + Kp = 30.0f; +// Ki=0.0f; +// antiwindup=50.0f; + } + + /* Get setpoint */ + + + //Rate Controller + setpointRateCast[0] = -((float)setpoints->motor_right_ne - 9999.0f) * 0.01f / 180.0f * 3.141f; + setpointRateCast[1] = -((float)setpoints->motor_front_nw - 9999.0f) * 0.01f / 180.0f * 3.141f; + setpointRateCast[2] = 0; //-((float)setpoints->motor_back_se-9999.0f)*0.01f; + //Ki=actuatorDesired.motorRight_NE*0.001f; + setpointThrustCast = setpoints->motor_left_sw; + + attRatesScaled[0] = 0.000317603994f * (float)raw->gyro_raw[0]; + attRatesScaled[1] = 0.000317603994f * (float)raw->gyro_raw[1]; + attRatesScaled[2] = 0.000317603994f * (float)raw->gyro_raw[2]; + + //filtering of the gyroscope values + + //compute filter coefficient alpha + + //gyro_alpha=0.005/(2.0f*3.1415f*200.0f+0.005f); + //gyro_alpha=0.009; + gyro_alpha = 0.09f; + gyro_alpha_offset = 0.001f; + //gyro_alpha=0.001; + //offset estimation and filtering + offsetCnt++; + uint8_t i; + + for (i = 0; i < 3; i++) { + if (offsetCnt < 5000) { + gyro_filtered_offset[i] = attRatesScaled[i] * gyro_alpha_offset + gyro_filtered_offset[i] * (1 - gyro_alpha_offset); + } + + gyro_filtered[i] = 1.0f * ((attRatesScaled[i] - gyro_filtered_offset[i]) * gyro_alpha + gyro_filtered[i] * (1 - gyro_alpha)) - 0 * setpointRateCast[i]; + } + + // //START DEBUG + // /* write filtered values to global_data_attitude */ + // global_data_attitude->rollspeed = gyro_filtered[0]; + // global_data_attitude->pitchspeed = gyro_filtered[1]; + // global_data_attitude->yawspeed = gyro_filtered[2]; + // //END DEBUG + + //rate controller + + //X-axis + setpointXrate = -Kp * (setpointRateCast[0] - gyro_filtered[0]); + //Y-axis + setpointYrate = -Kp * (setpointRateCast[1] - gyro_filtered[1]); + //Z-axis + setpointZrate = -Kp * (setpointRateCast[2] - gyro_filtered[2]); + + + + + //Mixing + if (setpointThrustCast <= 0) { + setpointThrustCast = 0; + outputBand = 0; + } + + if ((setpointThrustCast < startpointFullControll) && (setpointThrustCast > 0)) { + outputBand = 0.75f * setpointThrustCast; + } + + if ((setpointThrustCast >= startpointFullControll) && (setpointThrustCast < maxThrustSetpoints - 0.75f * startpointFullControll)) { + outputBand = 0.75f * startpointFullControll; + } + + if (setpointThrustCast >= maxThrustSetpoints - 0.75f * startpointFullControll) { + setpointThrustCast = 0.75f * startpointFullControll; + outputBand = 0.75f * startpointFullControll; + } + + actuators_desired.motor_front_nw = setpointThrustCast + (setpointXrate + setpointYrate + setpointZrate); + actuators_desired.motor_right_ne = setpointThrustCast + (-setpointXrate + setpointYrate - setpointZrate); + actuators_desired.motor_back_se = setpointThrustCast + (-setpointXrate - setpointYrate + setpointZrate); + actuators_desired.motor_left_sw = setpointThrustCast + (setpointXrate - setpointYrate - setpointZrate); + + + if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) > (setpointThrustCast + outputBand)) { + actuators_desired.motor_front_nw = setpointThrustCast + outputBand; + } + + if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) < (setpointThrustCast - outputBand)) { + actuators_desired.motor_front_nw = setpointThrustCast - outputBand; + } + + if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) > (setpointThrustCast + outputBand)) { + actuators_desired.motor_right_ne = setpointThrustCast + outputBand; + } + + if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) < (setpointThrustCast - outputBand)) { + actuators_desired.motor_right_ne = setpointThrustCast - outputBand; + } + + if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) > (setpointThrustCast + outputBand)) { + actuators_desired.motor_back_se = setpointThrustCast + outputBand; + } + + if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) < (setpointThrustCast - outputBand)) { + actuators_desired.motor_back_se = setpointThrustCast - outputBand; + } + + if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) > (setpointThrustCast + outputBand)) { + actuators_desired.motor_left_sw = setpointThrustCast + outputBand; + } + + if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) < (setpointThrustCast - outputBand)) { + actuators_desired.motor_left_sw = setpointThrustCast - outputBand; + } + + //printf("%lu,%lu,%lu,%lu\n",actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw); + + if (motor_skip_counter % 5 == 0) { + uint8_t motorSpeedBuf[5]; + ar_get_motor_packet(motorSpeedBuf, actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw); +// uint8_t* motorSpeedBuf = ar_get_motor_packet(1, 1, 1, 1); +// if(motor_skip_counter %50 == 0) +// { +// if(0==actuators_desired.motor_front_nw || 0 == actuators_desired.motor_right_ne || 0 == actuators_desired.motor_back_se || 0 == actuators_desired.motor_left_sw) +// printf("Motors set: %u, %u, %u, %u\n", actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw); +// printf("input: %u\n", setpoints->motor_front_nw); +// printf("Roll casted desired: %f, Pitch casted desired: %f, Yaw casted desired: %f\n", setpointRateCast[0], setpointRateCast[1], setpointRateCast[2]); +// } + write(ardrone_write, motorSpeedBuf, 5); +// motor_skip_counter = 0; + } + + motor_skip_counter++; + + //START DEBUG +// global_data_lock(&global_data_ardrone_control->access_conf); +// global_data_ardrone_control->timestamp = hrt_absolute_time(); +// global_data_ardrone_control->gyro_scaled[0] = attRatesScaled[0]; +// global_data_ardrone_control->gyro_scaled[1] = attRatesScaled[1]; +// global_data_ardrone_control->gyro_scaled[2] = attRatesScaled[2]; +// global_data_ardrone_control->gyro_filtered[0] = gyro_filtered[0]; +// global_data_ardrone_control->gyro_filtered[1] = gyro_filtered[1]; +// global_data_ardrone_control->gyro_filtered[2] = gyro_filtered[2]; +// global_data_ardrone_control->gyro_filtered_offset[0] = gyro_filtered_offset[0]; +// global_data_ardrone_control->gyro_filtered_offset[1] = gyro_filtered_offset[1]; +// global_data_ardrone_control->gyro_filtered_offset[2] = gyro_filtered_offset[2]; +// global_data_ardrone_control->setpoint_rate_cast[0] = setpointRateCast[0]; +// global_data_ardrone_control->setpoint_rate_cast[1] = setpointRateCast[1]; +// global_data_ardrone_control->setpoint_rate_cast[2] = setpointRateCast[2]; +// global_data_ardrone_control->setpoint_thrust_cast = setpointThrustCast; +// global_data_ardrone_control->setpoint_rate[0] = setpointXrate; +// global_data_ardrone_control->setpoint_rate[1] = setpointYrate; +// global_data_ardrone_control->setpoint_rate[2] = setpointZrate; +// global_data_ardrone_control->motor_front_nw = actuators_desired.motor_front_nw; +// global_data_ardrone_control->motor_right_ne = actuators_desired.motor_right_ne; +// global_data_ardrone_control->motor_back_se = actuators_desired.motor_back_se; +// global_data_ardrone_control->motor_left_sw = actuators_desired.motor_left_sw; +// global_data_unlock(&global_data_ardrone_control->access_conf); +// global_data_broadcast(&global_data_ardrone_control->access_conf); + //END DEBUG + + + +// gettimeofday(&tv, NULL); +// now = ((uint32_t)tv.tv_sec) * 1000 + tv.tv_usec/1000; +// time_elapsed = now - last_run; +// if (time_elapsed*1000 > CONTROL_LOOP_USLEEP) +// { +// sleep_time = (int32_t)CONTROL_LOOP_USLEEP - ((int32_t)time_elapsed*1000 - (int32_t)CONTROL_LOOP_USLEEP); +// +// if(motor_skip_counter %500 == 0) +// { +// printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run); +// } +// } +// +// if (sleep_time <= 0) +// { +// printf("WARNING: CPU Overload!\n"); +// printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run); +// usleep(CONTROL_LOOP_USLEEP); +// } +// else +// { +// usleep(sleep_time); +// } +// last_run = now; +// +// now_time = hrt_absolute_time(); +// if(control_counter % 500 == 0) +// { +// printf("Now: %lu\n",(unsigned long)now_time); +// printf("Last: %lu\n",(unsigned long)last_time); +// printf("Difference: %lu\n", (unsigned long)(now_time - last_time)); +// printf("now seconds: %lu\n", (unsigned long)(now_time / 1000000)); +// } +// last_time = now_time; +// +// now_time = hrt_absolute_time() / 1000000; +// if(now_time - last_time > 0) +// { +// printf("Counter: %ld\n",control_counter); +// last_time = now_time; +// control_counter = 0; +// } +// control_counter++; +} diff --git a/apps/ardrone_control/rate_control.h b/apps/ardrone_control/rate_control.h new file mode 100644 index 000000000..a69745d9b --- /dev/null +++ b/apps/ardrone_control/rate_control.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <nagelit@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Definition of attitude rate control + */ +#ifndef RATE_CONTROL_H_ +#define RATE_CONTROL_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/ardrone_motors_setpoint.h> +#include <uORB/topics/sensor_combined.h> + +void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints); + +#endif /* RATE_CONTROL_H_ */ diff --git a/apps/attitude_estimator_ekf/.context b/apps/attitude_estimator_ekf/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/attitude_estimator_ekf/.context diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile new file mode 100644 index 000000000..cad20d375 --- /dev/null +++ b/apps/attitude_estimator_ekf/Makefile @@ -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. +# +############################################################################ + +APPNAME = attitude_estimator_ekf +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 20000 + +CSRCS = attitude_estimator_ekf_main.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c + +# XXX this is *horribly* broken +INCLUDES += $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.m b/apps/attitude_estimator_ekf/attitudeKalmanfilter.m new file mode 100755 index 000000000..5fb4aa94f --- /dev/null +++ b/apps/attitude_estimator_ekf/attitudeKalmanfilter.m @@ -0,0 +1,108 @@ +function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+%#codegen
+
+
+%Extended Attitude Kalmanfilter
+%
+ %state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]'
+ %measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]'
+ %knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW]
+ %
+ %[x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+ %
+ %Example....
+ %
+ % $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $
+
+
+ %%define the matrices
+ acc_ProcessNoise=knownConst(1);
+ mag_ProcessNoise=knownConst(2);
+ ratesOffset_ProcessNoise=knownConst(3);
+ rates_ProcessNoise=knownConst(4);
+
+
+ acc_MeasurementNoise=knownConst(5);
+ mag_MeasurementNoise=knownConst(6);
+ rates_MeasurementNoise=knownConst(7);
+
+ %process noise covariance matrix
+ Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3);
+ zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3);
+ zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3);
+ zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise];
+
+ %measurement noise covariance matrix
+ R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3);
+ zeros(3), eye(3)*mag_MeasurementNoise, zeros(3);
+ zeros(3), zeros(3), eye(3)*rates_MeasurementNoise];
+
+
+ %observation matrix
+ H_k=[ eye(3), zeros(3), zeros(3), zeros(3);
+ zeros(3), eye(3), zeros(3), zeros(3);
+ zeros(3), zeros(3), eye(3), eye(3)];
+
+ %compute A(t,w)
+
+ %x_aposteriori_k[10,11,12] should be [p,q,r]
+ %R_temp=[1,-r, q
+ % r, 1, -p
+ % -q, p, 1]
+
+ R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11);
+ dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10);
+ -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1];
+
+ %strange, should not be transposed
+ A_pred=[R_temp', zeros(3), zeros(3), zeros(3);
+ zeros(3), R_temp', zeros(3), zeros(3);
+ zeros(3), zeros(3), eye(3), zeros(3);
+ zeros(3), zeros(3), zeros(3), eye(3)];
+
+ %%prediction step
+ x_apriori=A_pred*x_aposteriori_k;
+
+ %linearization
+ acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2);
+ -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1);
+ dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0];
+
+ mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5);
+ -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4);
+ dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0];
+
+ A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat';
+ zeros(3), R_temp', zeros(3), mag_temp_mat';
+ zeros(3), zeros(3), eye(3), zeros(3);
+ zeros(3), zeros(3), zeros(3), eye(3)];
+
+
+ P_apriori=A_lin*P_aposteriori_k*A_lin'+Q;
+
+
+ %%update step
+
+ y_k=z_k-H_k*x_apriori;
+ S_k=H_k*P_apriori*H_k'+R;
+ K_k=(P_apriori*H_k'/(S_k));
+
+
+ x_aposteriori=x_apriori+K_k*y_k;
+ P_aposteriori=(eye(12)-K_k*H_k)*P_apriori;
+
+
+ %%Rotation matrix generation
+
+ earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3));
+ earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6)));
+ earth_y=cross(earth_x,earth_z);
+
+ Rot_matrix=[earth_x,earth_y,earth_z];
+
+
+
+
+
+
+
diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj b/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj new file mode 100755 index 000000000..431ddb71e --- /dev/null +++ b/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -0,0 +1,270 @@ +<?xml version="1.0" encoding="UTF-8"?>
+<deployment-project>
+ <configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="attitudeKalmanfilter" location="F:\codegenerationMatlabAttFilter" file="F:\codegenerationMatlabAttFilter\attitudeKalmanfilter.prj" build-checksum="2344418414">
+ <param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>
+ <param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>
+ <param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>
+ <param.mex.general.EnableBLAS>true</param.mex.general.EnableBLAS>
+ <param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>
+ <param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>
+ <param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>
+ <param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>
+ <param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>
+ <param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>
+ <param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>
+ <param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>
+ <param.mex.paths.working>option.paths.working.project</param.mex.paths.working>
+ <param.mex.paths.working.specified />
+ <param.mex.paths.build>option.paths.build.project</param.mex.paths.build>
+ <param.mex.paths.build.specified />
+ <param.mex.paths.search />
+ <param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>
+ <param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>
+ <param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>
+ <param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>
+ <param.mex.symbols.ReservedNameArray />
+ <param.mex.customcode.CustomSourceCode />
+ <param.mex.customcode.CustomHeaderCode />
+ <param.mex.customcode.CustomInitializer />
+ <param.mex.customcode.CustomTerminator />
+ <param.mex.customcode.CustomInclude />
+ <param.mex.customcode.CustomSource />
+ <param.mex.customcode.CustomLibrary />
+ <param.mex.PostCodeGenCommand />
+ <param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>
+ <param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>
+ <param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>
+ <param.mex.InlineThreshold>10</param.mex.InlineThreshold>
+ <param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>
+ <param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>
+ <param.mex.StackUsageMax>200000</param.mex.StackUsageMax>
+ <param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>
+ <param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>
+ <param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>
+ <param.grt.CCompilerOptimization>option.CCompilerOptimization.On</param.grt.CCompilerOptimization>
+ <param.grt.CCompilerCustomOptimizations />
+ <param.grt.general.GenerateMakefile>true</param.grt.general.GenerateMakefile>
+ <param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>
+ <param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>
+ <param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>
+ <param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>
+ <param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>
+ <param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>
+ <param.grt.paths.working>option.paths.working.project</param.grt.paths.working>
+ <param.grt.paths.working.specified />
+ <param.grt.paths.build>option.paths.build.project</param.grt.paths.build>
+ <param.grt.paths.build.specified />
+ <param.grt.paths.search />
+ <param.grt.report.GenerateReport>true</param.grt.report.GenerateReport>
+ <param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
+ <param.grt.GenerateComments>true</param.grt.GenerateComments>
+ <param.grt.MATLABSourceComments>true</param.grt.MATLABSourceComments>
+ <param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>
+ <param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>
+ <param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>
+ <param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>
+ <param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>
+ <param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>
+ <param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>
+ <param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>
+ <param.grt.MaxIdLength>32</param.grt.MaxIdLength>
+ <param.grt.ReservedNameArray />
+ <param.grt.customcode.CustomSourceCode />
+ <param.grt.customcode.CustomHeaderCode />
+ <param.grt.customcode.CustomInitializer />
+ <param.grt.customcode.CustomTerminator />
+ <param.grt.customcode.CustomInclude />
+ <param.grt.customcode.CustomSource />
+ <param.grt.customcode.CustomLibrary />
+ <param.grt.PostCodeGenCommand />
+ <param.grt.Verbose>false</param.grt.Verbose>
+ <param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>
+ <param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>
+ <param.ert.TargetFunctionLibrary>C99 (ISO)</param.ert.TargetFunctionLibrary>
+ <param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>
+ <param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>
+ <param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>
+ <param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>
+ <param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>
+ <param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>
+ <param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>
+ <param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>
+ <param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>
+ <param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>
+ <param.grt.InlineThreshold>10</param.grt.InlineThreshold>
+ <param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>
+ <param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>
+ <param.grt.StackUsageMax>200000</param.grt.StackUsageMax>
+ <param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>
+ <param.UseECoderFeatures>true</param.UseECoderFeatures>
+ <param.mex.outputfile>attitudeKalmanfilter_mex</param.mex.outputfile>
+ <param.grt.outputfile>attitudeKalmanfilter</param.grt.outputfile>
+ <param.artifact>option.target.artifact.lib</param.artifact>
+ <param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
+ <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
+ <param.version>R2011a</param.version>
+ <param.HasECoderFeatures>true</param.HasECoderFeatures>
+ <param.mex.mainhtml />
+ <param.grt.mainhtml>F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter\html\index.html</param.grt.mainhtml>
+ <unset>
+ <param.mex.general.TargetLang />
+ <param.mex.general.IntegrityChecks />
+ <param.mex.general.ResponsivenessChecks />
+ <param.mex.general.EnableBLAS />
+ <param.mex.general.ExtrinsicCalls />
+ <param.mex.general.EchoExpressions />
+ <param.mex.general.EnableDebugging />
+ <param.mex.general.SaturateOnIntegerOverflow />
+ <param.mex.general.FilePartitionMethod />
+ <param.mex.general.GlobalDataSyncMethod />
+ <param.mex.general.EnableVariableSizing />
+ <param.mex.general.DynamicMemoryAllocation />
+ <param.mex.paths.working />
+ <param.mex.paths.working.specified />
+ <param.mex.paths.build />
+ <param.mex.paths.build.specified />
+ <param.mex.paths.search />
+ <param.mex.report.GenerateReport />
+ <param.mex.report.LaunchReport />
+ <param.mex.comments.GenerateComments />
+ <param.mex.comments.MATLABSourceComments />
+ <param.mex.symbols.ReservedNameArray />
+ <param.mex.customcode.CustomSourceCode />
+ <param.mex.customcode.CustomHeaderCode />
+ <param.mex.customcode.CustomInitializer />
+ <param.mex.customcode.CustomTerminator />
+ <param.mex.customcode.CustomInclude />
+ <param.mex.customcode.CustomSource />
+ <param.mex.customcode.CustomLibrary />
+ <param.mex.PostCodeGenCommand />
+ <param.mex.EnableMemcpy />
+ <param.mex.MemcpyThreshold />
+ <param.mex.InitFltsAndDblsToZero />
+ <param.mex.InlineThreshold />
+ <param.mex.InlineThresholdMax />
+ <param.mex.InlineStackLimit />
+ <param.mex.StackUsageMax />
+ <param.mex.ConstantFoldingTimeout />
+ <param.grt.general.TargetLang />
+ <param.grt.CCompilerCustomOptimizations />
+ <param.grt.general.GenerateMakefile />
+ <param.grt.general.MakeCommand />
+ <param.grt.general.TemplateMakefile />
+ <param.grt.general.SaturateOnIntegerOverflow />
+ <param.grt.general.FilePartitionMethod />
+ <param.grt.general.EnableVariableSizing />
+ <param.grt.general.DynamicMemoryAllocation />
+ <param.grt.paths.working />
+ <param.grt.paths.working.specified />
+ <param.grt.paths.build />
+ <param.grt.paths.build.specified />
+ <param.grt.paths.search />
+ <param.grt.report.GenerateReport />
+ <param.grt.report.LaunchReport />
+ <param.grt.GenerateComments />
+ <param.ert.MATLABFcnDesc />
+ <param.ert.CustomSymbolStrGlobalVar />
+ <param.ert.CustomSymbolStrType />
+ <param.ert.CustomSymbolStrField />
+ <param.ert.CustomSymbolStrFcn />
+ <param.ert.CustomSymbolStrTmpVar />
+ <param.ert.CustomSymbolStrMacro />
+ <param.ert.CustomSymbolStrEMXArray />
+ <param.grt.MaxIdLength />
+ <param.grt.ReservedNameArray />
+ <param.grt.customcode.CustomHeaderCode />
+ <param.grt.customcode.CustomInitializer />
+ <param.grt.customcode.CustomTerminator />
+ <param.grt.customcode.CustomInclude />
+ <param.grt.customcode.CustomSource />
+ <param.grt.customcode.CustomLibrary />
+ <param.grt.PostCodeGenCommand />
+ <param.grt.Verbose />
+ <param.grt.SupportNonFinite />
+ <param.ert.PurelyIntegerCode />
+ <param.ert.SupportNonFinite />
+ <param.ert.IncludeTerminateFcn />
+ <param.ert.MultiInstanceCode />
+ <param.ert.ParenthesesLevel />
+ <param.ert.ConvertIfToSwitch />
+ <param.ert.PreserveExternInFcnDecls />
+ <param.grt.EnableMemcpy />
+ <param.grt.MemcpyThreshold />
+ <param.grt.InitFltsAndDblsToZero />
+ <param.grt.InlineThreshold />
+ <param.grt.InlineThresholdMax />
+ <param.grt.InlineStackLimit />
+ <param.grt.StackUsageMax />
+ <param.grt.ConstantFoldingTimeout />
+ <param.UseECoderFeatures />
+ <param.mex.outputfile />
+ <param.grt.outputfile />
+ <param.mex.GenCodeOnly />
+ <param.version />
+ <param.HasECoderFeatures />
+ <param.mex.mainhtml />
+ </unset>
+ <fileset.entrypoints>
+ <file value="${PROJECT_ROOT}\attitudeKalmanfilter.m" custom-data-expanded="true">
+ <Inputs fileName="attitudeKalmanfilter.m" functionName="attitudeKalmanfilter">
+ <Input Name="dt">
+ <Class>single</Class>
+ <Size>1 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="z_k">
+ <Class>single</Class>
+ <Size>9 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="x_aposteriori_k">
+ <Class>single</Class>
+ <Size>12 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="P_aposteriori_k">
+ <Class>single</Class>
+ <Size>12 x 12</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="knownConst">
+ <Class>single</Class>
+ <Size>7 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ </Inputs>
+ </file>
+ </fileset.entrypoints>
+ <fileset.package />
+ <build-deliverables />
+ <matlab>
+ <root>C:\Program Files\MATLAB\R2011a</root>
+ </matlab>
+ <platform>
+ <unix>false</unix>
+ <mac>false</mac>
+ <windows>true</windows>
+ <win2k>false</win2k>
+ <winxp>false</winxp>
+ <vista>false</vista>
+ <linux>false</linux>
+ <solaris>false</solaris>
+ <osver>6.1</osver>
+ <os32>false</os32>
+ <os64>true</os64>
+ <arch>win64</arch>
+ <matlab>true</matlab>
+ </platform>
+ </configuration>
+</deployment-project>
+
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c new file mode 100644 index 000000000..fdf6c9d91 --- /dev/null +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -0,0 +1,263 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Extended Kalman Filter for Attitude Estimation + */ + + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <poll.h> +#include <fcntl.h> +#include <v1.0/common/mavlink.h> +#include <float.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <arch/board/up_hrt.h> + +#include "codegen/attitudeKalmanfilter_initialize.h" +#include "codegen/attitudeKalmanfilter.h" + +__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); + + +// #define N_STATES 6 + +// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 +// #define REPROJECTION_COUNTER_LIMIT 125 + +static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds + +static float dt = 1; +/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ +/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ +static float z_k[9] = {0}; /**< Measurement vector */ +static float x_aposteriori[12] = {0}; /**< */ +static float P_aposteriori[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, 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 + }; /**< init: diagonal matrix with big values */ +static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ +static float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + +// static float x_aposteriori_k[12] = {0}; +// static float P_aposteriori_k[144] = {0}; + +/* + * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + +/* + * EKF Attitude Estimator main function. + * + * Estimates the attitude recursively once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_ekf_main(int argc, char *argv[]) +{ + // print text + printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); + fflush(stdout); + + int overloadcounter = 19; + + /* Initialize filter */ + attitudeKalmanfilter_initialize(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s raw = {0}; + struct vehicle_attitude_s att = {}; + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* advertise attitude */ + int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + + int loopcounter = 0; + int printcounter = 0; + + /* Main loop*/ + while (true) { + + struct pollfd fds[1] = { + { .fd = sub_raw, .events = POLLIN }, + }; + int ret = poll(fds, 1, 1000); + + /* check for a timeout */ + if (ret == 0) { + /* */ + + /* update successful, copy data on every 2nd run and execute filter */ + } else if (loopcounter & 0x01) { + + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + + // XXX Read out accel range via SPI on init, assuming 4G range at 14 bit res here + float range_g = 4.0f; + float mag_offset[3] = {0}; + /* scale from 14 bit to m/s2 */ + z_k[3] = ((raw.accelerometer_raw[0] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); + z_k[4] = ((raw.accelerometer_raw[1] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); + z_k[5] = ((raw.accelerometer_raw[2] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); + + // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here + z_k[0] = (raw.magnetometer_raw[0] - mag_offset[0]) * 0.01f; + z_k[1] = (raw.magnetometer_raw[1] - mag_offset[1]) * 0.01f; + z_k[2] = (raw.magnetometer_raw[2] - mag_offset[2]) * 0.01f; + + /* Fill in gyro measurements */ + z_k[6] = raw.gyro_raw[0] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; + z_k[7] = raw.gyro_raw[1] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; + z_k[8] = raw.gyro_raw[2] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; + + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + if (overloadcounter == 20) { + printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + overloadcounter = 0; + } + + overloadcounter++; + } + +// now = hrt_absolute_time(); + /* filter values */ + /* + * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + uint64_t timing_start = hrt_absolute_time(); + attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori); + uint64_t timing_diff = hrt_absolute_time() - timing_start; + + /* print rotation matrix every 200th time */ + if (printcounter % 200 == 0) { + printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), + (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), + (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); + } + + printcounter++; + +// time_elapsed = hrt_absolute_time() - now; +// if (blubb == 20) +// { +// printf("Estimator: %lu\n", time_elapsed); +// blubb = 0; +// } +// blubb++; + + if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data); + +// printf("%llu -> %llu = %llu\n", last_data, raw.timestamp, raw.timestamp - last_data); + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + // att.roll = euler.x; + // att.pitch = euler.y; + // att.yaw = euler.z + F_M_PI; + + // if (att.yaw > 2 * F_M_PI) { + // att.yaw -= 2 * F_M_PI; + // } + + // att.rollspeed = rates.x; + // att.pitchspeed = rates.y; + // att.yawspeed = rates.z; + + // memcpy()R + + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + } + + loopcounter++; + } + + /* Should never reach here */ + return 0; +} + + + + + diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c new file mode 100755 index 000000000..4e275e3ee --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -0,0 +1,635 @@ +/*
+ * attitudeKalmanfilter.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "norm.h"
+#include "eye.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+ */
+void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T
+ x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T
+ knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
+ P_aposteriori[144])
+{
+ real32_T R_temp[9];
+ real_T dv0[9];
+ real_T dv1[9];
+ int32_T i;
+ int32_T i0;
+ real32_T A_pred[144];
+ real32_T x_apriori[12];
+ real32_T b_A_pred[144];
+ int32_T i1;
+ real32_T c_A_pred[144];
+ static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
+
+ real32_T P_apriori[144];
+ real32_T b_P_apriori[108];
+ static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1 };
+
+ real32_T K_k[108];
+ static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+ real32_T fv0[81];
+ real32_T fv1[81];
+ real32_T fv2[81];
+ real32_T B;
+ real_T dv2[144];
+ real32_T b_B;
+ real32_T earth_z[3];
+ real32_T y[3];
+ real32_T earth_x[3];
+
+ /* Extended Attitude Kalmanfilter */
+ /* */
+ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
+ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+ /* */
+ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
+ /* */
+ /* Example.... */
+ /* */
+ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
+ /* %define the matrices */
+ /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */
+ /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */
+ /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */
+ /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */
+ /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */
+ /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */
+ /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */
+ /* process noise covariance matrix */
+ /* 'attitudeKalmanfilter:30' Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:31' zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:32' zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); */
+ /* 'attitudeKalmanfilter:33' zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; */
+ /* measurement noise covariance matrix */
+ /* 'attitudeKalmanfilter:36' R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:37' zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); */
+ /* 'attitudeKalmanfilter:38' zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:42' H_k=[ eye(3), zeros(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:43' zeros(3), eye(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:44' zeros(3), zeros(3), eye(3), eye(3)]; */
+ /* compute A(t,w) */
+ /* x_aposteriori_k[10,11,12] should be [p,q,r] */
+ /* R_temp=[1,-r, q */
+ /* r, 1, -p */
+ /* -q, p, 1] */
+ /* 'attitudeKalmanfilter:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */
+ /* 'attitudeKalmanfilter:54' dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */
+ /* 'attitudeKalmanfilter:55' -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; */
+ R_temp[0] = 1.0F;
+ R_temp[3] = -dt * x_aposteriori_k[11];
+ R_temp[6] = dt * x_aposteriori_k[10];
+ R_temp[1] = dt * x_aposteriori_k[11];
+ R_temp[4] = 1.0F;
+ R_temp[7] = -dt * x_aposteriori_k[9];
+ R_temp[2] = -dt * x_aposteriori_k[10];
+ R_temp[5] = dt * x_aposteriori_k[9];
+ R_temp[8] = 1.0F;
+
+ /* strange, should not be transposed */
+ /* 'attitudeKalmanfilter:58' A_pred=[R_temp', zeros(3), zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:59' zeros(3), R_temp', zeros(3), zeros(3); */
+ /* 'attitudeKalmanfilter:60' zeros(3), zeros(3), eye(3), zeros(3); */
+ /* 'attitudeKalmanfilter:61' zeros(3), zeros(3), zeros(3), eye(3)]; */
+ eye(dv0);
+ eye(dv1);
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * i] = R_temp[i + 3 * i0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 9)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i];
+ }
+ }
+
+ /* %prediction step */
+ /* 'attitudeKalmanfilter:64' x_apriori=A_pred*x_aposteriori_k; */
+ for (i = 0; i < 12; i++) {
+ x_apriori[i] = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ x_apriori[i] += A_pred[i + 12 * i0] * x_aposteriori_k[i0];
+ }
+ }
+
+ /* linearization */
+ /* 'attitudeKalmanfilter:67' acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); */
+ /* 'attitudeKalmanfilter:68' -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); */
+ /* 'attitudeKalmanfilter:69' dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; */
+ /* 'attitudeKalmanfilter:71' mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); */
+ /* 'attitudeKalmanfilter:72' -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); */
+ /* 'attitudeKalmanfilter:73' dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; */
+ /* 'attitudeKalmanfilter:75' A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; */
+ /* 'attitudeKalmanfilter:76' zeros(3), R_temp', zeros(3), mag_temp_mat'; */
+ /* 'attitudeKalmanfilter:77' zeros(3), zeros(3), eye(3), zeros(3); */
+ /* 'attitudeKalmanfilter:78' zeros(3), zeros(3), zeros(3), eye(3)]; */
+ eye(dv0);
+ eye(dv1);
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * i] = R_temp[i + 3 * i0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ }
+ }
+
+ A_pred[108] = 0.0F;
+ A_pred[109] = dt * x_aposteriori_k[2];
+ A_pred[110] = -dt * x_aposteriori_k[1];
+ A_pred[120] = -dt * x_aposteriori_k[2];
+ A_pred[121] = 0.0F;
+ A_pred[122] = dt * x_aposteriori_k[0];
+ A_pred[132] = dt * x_aposteriori_k[1];
+ A_pred[133] = -dt * x_aposteriori_k[0];
+ A_pred[134] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ }
+ }
+
+ A_pred[111] = 0.0F;
+ A_pred[112] = dt * x_aposteriori_k[5];
+ A_pred[113] = -dt * x_aposteriori_k[4];
+ A_pred[123] = -dt * x_aposteriori_k[5];
+ A_pred[124] = 0.0F;
+ A_pred[125] = dt * x_aposteriori_k[3];
+ A_pred[135] = dt * x_aposteriori_k[4];
+ A_pred[136] = -dt * x_aposteriori_k[3];
+ A_pred[137] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * i) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i];
+ }
+ }
+
+ /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ b_A_pred[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
+ i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 12; i0++) {
+ c_A_pred[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1];
+ }
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[i0 + 12 * (i + 3)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[i0 + 12 * (i + 6)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[i0 + 12 * (i + 9)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] *
+ knownConst[1];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * i) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] *
+ knownConst[2];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * i) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ b_A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)iv0[i0 + 3 * i] *
+ knownConst[3];
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_apriori[i0 + 12 * i] = c_A_pred[i0 + 12 * i] + b_A_pred[i0 + 12 * i];
+ }
+ }
+
+ /* %update step */
+ /* 'attitudeKalmanfilter:86' y_k=z_k-H_k*x_apriori; */
+ /* 'attitudeKalmanfilter:87' S_k=H_k*P_apriori*H_k'+R; */
+ /* 'attitudeKalmanfilter:88' K_k=(P_apriori*H_k'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ b_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 +
+ 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ K_k[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 9; i0++) {
+ fv0[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[i0 + 9 * (i + 3)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[i0 + 9 * (i + 6)] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * i) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * knownConst[5];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * i) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ fv1[(i0 + 9 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * knownConst[6];
+ }
+ }
+
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i];
+ }
+ }
+
+ mrdivide(b_P_apriori, fv2, K_k);
+
+ /* 'attitudeKalmanfilter:91' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 9; i++) {
+ B = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ B += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];
+ }
+
+ R_temp[i] = z_k[i] - B;
+ }
+
+ for (i = 0; i < 12; i++) {
+ B = 0.0F;
+ for (i0 = 0; i0 < 9; i0++) {
+ B += K_k[i + 12 * i0] * R_temp[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + B;
+ }
+
+ /* 'attitudeKalmanfilter:92' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
+ b_eye(dv2);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ B = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ B += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];
+ }
+
+ A_pred[i + 12 * i0] = (real32_T)dv2[i + 12 * i0] - B;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += A_pred[i + 12 * i1] * P_apriori[i1 + 12 *
+ i0];
+ }
+ }
+ }
+
+ /* %Rotation matrix generation */
+ /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */
+ B = norm(*(real32_T (*)[3])&x_aposteriori[0]);
+
+ /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */
+ b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]);
+ for (i = 0; i < 3; i++) {
+ earth_z[i] = x_aposteriori[i] / B;
+ y[i] = x_aposteriori[i + 3] / b_B;
+ }
+
+ earth_x[0] = earth_z[1] * y[2] - earth_z[2] * y[1];
+ earth_x[1] = earth_z[2] * y[0] - earth_z[0] * y[2];
+ earth_x[2] = earth_z[0] * y[1] - earth_z[1] * y[0];
+
+ /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */
+ /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */
+ y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1];
+ y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2];
+ y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0];
+ for (i = 0; i < 3; i++) {
+ Rot_matrix[i] = earth_x[i];
+ Rot_matrix[3 + i] = y[i];
+ Rot_matrix[6 + i] = earth_z[i];
+ }
+}
+
+/* End of code generation (attitudeKalmanfilter.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h new file mode 100755 index 000000000..7aa3d048b --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -0,0 +1,32 @@ +/*
+ * attitudeKalmanfilter.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_H__
+#define __ATTITUDEKALMANFILTER_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
+#endif
+/* End of code generation (attitudeKalmanfilter.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c new file mode 100755 index 000000000..2800191c3 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -0,0 +1,31 @@ +/*
+ * attitudeKalmanfilter_initialize.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_initialize(void)
+{
+ rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (attitudeKalmanfilter_initialize.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h new file mode 100755 index 000000000..d2d97bb3b --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -0,0 +1,32 @@ +/*
+ * attitudeKalmanfilter_initialize.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
+#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter_initialize(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_initialize.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp new file mode 100755 index 000000000..e69de29bb --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat new file mode 100755 index 000000000..76fb78ca7 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat @@ -0,0 +1,11 @@ +@echo off
+call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64
+
+cd .
+nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
+@if errorlevel 1 goto error_exit
+exit /B 0
+
+:error_exit
+echo The make command returned an error of %errorlevel%
+An_error_occurred_during_the_call_to_make
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk new file mode 100755 index 000000000..b2123704b --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk @@ -0,0 +1,328 @@ +# Copyright 1994-2010 The MathWorks, Inc.
+#
+# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $
+#
+# Abstract:
+# Code generation template makefile for building a Windows-based,
+# stand-alone real-time version of MATLAB-code using generated C/C++
+# code and the
+# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64
+#
+# Note that this template is automatically customized by the
+# code generation build procedure to create "<model>.mk"
+#
+# The following defines can be used to modify the behavior of the
+# build:
+#
+# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in
+# vctools.mak for default.
+# OPTS - User specific options.
+# CPP_OPTS - C++ compiler options.
+# USER_SRCS - Additional user sources, such as files needed by
+# S-functions.
+# USER_INCLUDES - Additional include paths
+# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2")
+#
+# To enable debugging:
+# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc)
+# modify tmf LDFLAGS to include /DEBUG
+#
+
+#------------------------ Macros read by make_rtw -----------------------------
+#
+# The following macros are read by the code generation build procedure:
+#
+# MAKECMD - This is the command used to invoke the make utility
+# HOST - What platform this template makefile is targeted for
+# (i.e. PC or UNIX)
+# BUILD - Invoke make from the code generation build procedure
+# (yes/no)?
+# SYS_TARGET_FILE - Name of system target file.
+
+MAKECMD = nmake
+HOST = PC
+BUILD = yes
+SYS_TARGET_FILE = ert.tlc
+BUILD_SUCCESS = ^#^#^# Created
+COMPILER_TOOL_CHAIN = vcx64
+
+#---------------------- Tokens expanded by make_rtw ---------------------------
+#
+# The following tokens, when wrapped with "|>" and "<|" are expanded by the
+# code generation build procedure.
+#
+# MODEL_NAME - Name of the Simulink block diagram
+# MODEL_MODULES - Any additional generated source modules
+# MAKEFILE_NAME - Name of makefile created from template makefile <model>.mk
+# MATLAB_ROOT - Path to where MATLAB is installed.
+# MATLAB_BIN - Path to MATLAB executable.
+# S_FUNCTIONS - List of S-functions.
+# S_FUNCTIONS_LIB - List of S-functions libraries to link.
+# SOLVER - Solver source file name
+# NUMST - Number of sample times
+# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task
+# (tid=0) and 1st discrete task equal.
+# NCSTATES - Number of continuous states
+# BUILDARGS - Options passed in at the command line.
+# MULTITASKING - yes (1) or no (0): Is solver mode multitasking
+# EXT_MODE - yes (1) or no (0): Build for external mode
+# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode
+# testing.
+# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode
+# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc.
+# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer.
+
+MODEL = attitudeKalmanfilter
+MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c
+MAKEFILE = attitudeKalmanfilter_rtw.mk
+MATLAB_ROOT = C:\Program Files\MATLAB\R2011a
+ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a
+MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin
+ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin
+S_FUNCTIONS =
+S_FUNCTIONS_LIB =
+SOLVER =
+NUMST = 1
+TID01EQ = 0
+NCSTATES = 0
+BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
+MULTITASKING = 0
+EXT_MODE = 0
+TMW_EXTMODE_TESTING = 0
+EXTMODE_TRANSPORT = 0
+EXTMODE_STATIC = 0
+EXTMODE_STATIC_SIZE = 1000000
+
+MODELREFS =
+SHARED_SRC =
+SHARED_SRC_DIR =
+SHARED_BIN_DIR =
+SHARED_LIB =
+TARGET_LANG_EXT = c
+OPTIMIZATION_FLAGS = /O2 /Oy-
+ADDITIONAL_LDFLAGS =
+
+#--------------------------- Model and reference models -----------------------
+MODELLIB = attitudeKalmanfilter.lib
+MODELREF_LINK_LIBS =
+MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp
+MODELREF_INC_PATH =
+RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1
+MODELREF_TARGET_TYPE = RTW
+
+!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)"
+MATLAB_ROOT = $(ALT_MATLAB_ROOT)
+!endif
+!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)"
+MATLAB_BIN = $(ALT_MATLAB_BIN)
+!endif
+
+#--------------------------- Tool Specifications ------------------------------
+
+CPU = AMD64
+!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak
+APPVER = 5.02
+
+PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl
+#------------------------------ Include/Lib Path ------------------------------
+
+MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common
+
+# Additional file include paths
+
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter
+
+INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH)
+
+!if "$(SHARED_SRC_DIR)" != ""
+INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR)
+!endif
+
+#------------------------ External mode ---------------------------------------
+# Uncomment -DVERBOSE to have information printed to stdout
+# To add a new transport layer, see the comments in
+# <matlabroot>/toolbox/simulink/simulink/extmode_transports.m
+!if $(EXT_MODE) == 1
+EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE
+!if $(EXTMODE_TRANSPORT) == 0 #tcpip
+EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c
+EXT_LIB = wsock32.lib
+!endif
+!if $(EXTMODE_TRANSPORT) == 1 #serial_win32
+EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c
+EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c
+EXT_LIB =
+!endif
+!if $(TMW_EXTMODE_TESTING) == 1
+EXT_SRC = $(EXT_SRC) ext_test.c
+EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING
+!endif
+!if $(EXTMODE_STATIC) == 1
+EXT_SRC = $(EXT_SRC) mem_mgr.c
+EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE)
+!endif
+!else
+EXT_SRC =
+EXT_CC_OPTS =
+EXT_LIB =
+!endif
+
+#------------------------ rtModel ----------------------------------------------
+
+RTM_CC_OPTS = -DUSE_RTMODEL
+
+#----------------- Compiler and Linker Options --------------------------------
+
+# Optimization Options
+# Set OPT_OPTS=-Zi for debugging (may depend on compiler version)
+OPT_OPTS = $(DEFAULT_OPT_OPTS)
+
+# General User Options
+OPTS =
+
+!if "$(OPTIMIZATION_FLAGS)" != ""
+CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS)
+!else
+CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS)
+!endif
+
+CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \
+ -DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \
+ -DMT=$(MULTITASKING) -DHAVESTDIO
+
+# Uncomment this line to move warning level to W4
+# cflags = $(cflags:W3=W4)
+CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
+ $(cflags) $(cvarsmt) /wd4996
+CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
+ $(cflags) $(cvarsmt) /wd4996 /EHsc-
+LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS)
+
+# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib
+
+#----------------------------- Source Files -----------------------------------
+
+
+#Standalone executable
+!if "$(MODELREF_TARGET_TYPE)" == "NONE"
+PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe
+REQ_SRCS = $(MODULES) $(EXT_SRC)
+
+#Model Reference Target
+!else
+PRODUCT = $(MODELLIB)
+REQ_SRCS = $(MODULES) $(EXT_SRC)
+!endif
+
+USER_SRCS =
+
+SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS)
+OBJS_CPP_UPPER = $(SRCS:.CPP=.obj)
+OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj)
+OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj)
+OBJS = $(OBJS_C_UPPER:.c=.obj)
+SHARED_OBJS = $(SHARED_SRC:.c=.obj)
+# ------------------------- Additional Libraries ------------------------------
+
+LIBS =
+
+
+LIBS = $(LIBS)
+
+# ---------------------------- Linker Script ----------------------------------
+
+CMD_FILE = $(MODEL).lnk
+GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl
+
+#--------------------------------- Rules --------------------------------------
+all: set_environment_variables $(PRODUCT)
+
+!if "$(MODELREF_TARGET_TYPE)" == "NONE"
+#--- Stand-alone model ---
+$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS)
+ @cmd /C "echo ### Linking ..."
+ $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
+ $(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@
+ @del $(CMD_FILE)
+ @cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe"
+!else
+#--- Model reference code generation Target ---
+$(PRODUCT) : $(OBJS) $(SHARED_LIB)
+ @cmd /C "echo ### Linking ..."
+ $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
+ $(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB)
+ @cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)"
+!endif
+
+{$(MATLAB_ROOT)\rtw\c\src}.c.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CFLAGS) $<
+
+{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CFLAGS) $<
+
+{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CFLAGS) $<
+
+{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CFLAGS) $<
+
+{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CFLAGS) $<
+
+# Additional sources
+
+
+
+
+
+# Put these rule last, otherwise nmake will check toolboxes first
+
+{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CFLAGS) $<
+
+{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CPPFLAGS) $<
+
+.c.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CFLAGS) $<
+
+.cpp.obj :
+ @cmd /C "echo ### Compiling $<"
+ $(CC) $(CPPFLAGS) $<
+
+!if "$(SHARED_LIB)" != ""
+$(SHARED_LIB) : $(SHARED_SRC)
+ @cmd /C "echo ### Creating $@"
+ @$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<<
+$?
+<<
+ @$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS)
+ @cmd /C "echo ### $@ Created"
+!endif
+
+
+set_environment_variables:
+ @set INCLUDE=$(INCLUDE)
+ @set LIB=$(LIB)
+
+# Libraries:
+
+
+
+
+
+#----------------------------- Dependencies -----------------------------------
+
+$(OBJS) : $(MAKEFILE) rtw_proj.tmw
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c new file mode 100755 index 000000000..eab8c7d6e --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -0,0 +1,31 @@ +/*
+ * attitudeKalmanfilter_terminate.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_terminate(void)
+{
+ /* (no terminate code required) */
+}
+
+/* End of code generation (attitudeKalmanfilter_terminate.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h new file mode 100755 index 000000000..fafd866e4 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -0,0 +1,32 @@ +/*
+ * attitudeKalmanfilter_terminate.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
+#define __ATTITUDEKALMANFILTER_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter_terminate(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_terminate.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h new file mode 100755 index 000000000..05f4664cd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -0,0 +1,16 @@ +/*
+ * attitudeKalmanfilter_types.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
+#define __ATTITUDEKALMANFILTER_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (attitudeKalmanfilter_types.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/buildInfo.mat b/apps/attitude_estimator_ekf/codegen/buildInfo.mat Binary files differnew file mode 100755 index 000000000..b811d0039 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/buildInfo.mat diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c new file mode 100755 index 000000000..e4655839c --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -0,0 +1,51 @@ +/*
+ * eye.c
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "eye.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void b_eye(real_T I[144])
+{
+ int32_T i;
+ memset((void *)&I[0], 0, 144U * sizeof(real_T));
+ for (i = 0; i < 12; i++) {
+ I[i + 12 * i] = 1.0;
+ }
+}
+
+/*
+ *
+ */
+void eye(real_T I[9])
+{
+ int32_T i;
+ memset((void *)&I[0], 0, 9U * sizeof(real_T));
+ for (i = 0; i < 3; i++) {
+ I[i + 3 * i] = 1.0;
+ }
+}
+
+/* End of code generation (eye.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h new file mode 100755 index 000000000..e8881747f --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -0,0 +1,33 @@ +/*
+ * eye.h
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __EYE_H__
+#define __EYE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void b_eye(real_T I[144]);
+extern void eye(real_T I[9]);
+#endif
+/* End of code generation (eye.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c new file mode 100755 index 000000000..cb81b5328 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -0,0 +1,158 @@ +/*
+ * mrdivide.c
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
+{
+ int32_T jy;
+ int32_T iy;
+ real32_T b_A[81];
+ int8_T ipiv[9];
+ int32_T j;
+ int32_T mmj;
+ int32_T jj;
+ int32_T jp1j;
+ int32_T c;
+ int32_T ix;
+ real32_T temp;
+ int32_T k;
+ real32_T s;
+ int32_T loop_ub;
+ real32_T Y[108];
+ for (jy = 0; jy < 9; jy++) {
+ for (iy = 0; iy < 9; iy++) {
+ b_A[iy + 9 * jy] = B[jy + 9 * iy];
+ }
+
+ ipiv[jy] = (int8_T)(1 + jy);
+ }
+
+ for (j = 0; j < 8; j++) {
+ mmj = -j;
+ jj = j * 10;
+ jp1j = jj + 1;
+ c = mmj + 9;
+ jy = 0;
+ ix = jj;
+ temp = fabsf(b_A[jj]);
+ for (k = 2; k <= c; k++) {
+ ix++;
+ s = fabsf(b_A[ix]);
+ if (s > temp) {
+ jy = k - 1;
+ temp = s;
+ }
+ }
+
+ if ((real_T)b_A[jj + jy] != 0.0) {
+ if (jy != 0) {
+ ipiv[j] = (int8_T)((j + jy) + 1);
+ ix = j;
+ iy = j + jy;
+ for (k = 0; k < 9; k++) {
+ temp = b_A[ix];
+ b_A[ix] = b_A[iy];
+ b_A[iy] = temp;
+ ix += 9;
+ iy += 9;
+ }
+ }
+
+ loop_ub = (jp1j + mmj) + 8;
+ for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
+ b_A[iy] /= b_A[jj];
+ }
+ }
+
+ c = 8 - j;
+ jy = jj + 9;
+ for (iy = 1; iy <= c; iy++) {
+ if ((real_T)b_A[jy] != 0.0) {
+ temp = b_A[jy] * -1.0F;
+ ix = jp1j;
+ loop_ub = (mmj + jj) + 18;
+ for (k = 10 + jj; k + 1 <= loop_ub; k++) {
+ b_A[k] += b_A[ix] * temp;
+ ix++;
+ }
+ }
+
+ jy += 9;
+ jj += 9;
+ }
+ }
+
+ for (jy = 0; jy < 12; jy++) {
+ for (iy = 0; iy < 9; iy++) {
+ Y[iy + 9 * jy] = A[jy + 12 * iy];
+ }
+ }
+
+ for (iy = 0; iy < 9; iy++) {
+ if (ipiv[iy] != iy + 1) {
+ for (j = 0; j < 12; j++) {
+ temp = Y[iy + 9 * j];
+ Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
+ Y[(ipiv[iy] + 9 * j) - 1] = temp;
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 9 * j;
+ for (k = 0; k < 9; k++) {
+ jy = 9 * k;
+ if ((real_T)Y[k + c] != 0.0) {
+ for (iy = k + 2; iy < 10; iy++) {
+ Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
+ }
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 9 * j;
+ for (k = 8; k > -1; k += -1) {
+ jy = 9 * k;
+ if ((real_T)Y[k + c] != 0.0) {
+ Y[k + c] /= b_A[k + jy];
+ for (iy = 0; iy + 1 <= k; iy++) {
+ Y[iy + c] -= Y[k + c] * b_A[iy + jy];
+ }
+ }
+ }
+ }
+
+ for (jy = 0; jy < 9; jy++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * jy] = Y[jy + 9 * iy];
+ }
+ }
+}
+
+/* End of code generation (mrdivide.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h new file mode 100755 index 000000000..e81bfffc0 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -0,0 +1,32 @@ +/*
+ * mrdivide.h
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __MRDIVIDE_H__
+#define __MRDIVIDE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
+#endif
+/* End of code generation (mrdivide.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c new file mode 100755 index 000000000..1fbde052b --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -0,0 +1,62 @@ +/*
+ * norm.c
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "norm.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+real32_T norm(const real32_T x[3])
+{
+ real32_T y;
+ real32_T scale;
+ boolean_T firstNonZero;
+ int32_T k;
+ real32_T absxk;
+ real32_T t;
+ y = 0.0F;
+ scale = 0.0F;
+ firstNonZero = TRUE;
+ for (k = 0; k < 3; k++) {
+ if (x[k] != 0.0F) {
+ absxk = fabsf(x[k]);
+ if (firstNonZero) {
+ scale = absxk;
+ y = 1.0F;
+ firstNonZero = FALSE;
+ } else if (scale < absxk) {
+ t = scale / absxk;
+ y = 1.0F + y * t * t;
+ scale = absxk;
+ } else {
+ t = absxk / scale;
+ y += t * t;
+ }
+ }
+ }
+
+ return scale * sqrtf(y);
+}
+
+/* End of code generation (norm.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h new file mode 100755 index 000000000..b0c7fe430 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -0,0 +1,32 @@ +/*
+ * norm.h
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __NORM_H__
+#define __NORM_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern real32_T norm(const real32_T x[3]);
+#endif
+/* End of code generation (norm.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c new file mode 100755 index 000000000..3cef17684 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/*
+ * rtGetInf.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finite, Inf and MinusInf
+ */
+#include "rtGetInf.h"
+#define NumBitsPerChar 8U
+
+/* Function: rtGetInf ==================================================
+ * Abstract:
+ * Initialize rtInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetInf(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T inf = 0.0;
+ if (bitsPerReal == 32U) {
+ inf = rtGetInfF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ inf = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ inf = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return inf;
+}
+
+/* Function: rtGetInfF ==================================================
+ * Abstract:
+ * Initialize rtInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetInfF(void)
+{
+ IEEESingle infF;
+ infF.wordL.wordLuint = 0x7F800000U;
+ return infF.wordL.wordLreal;
+}
+
+/* Function: rtGetMinusInf ==================================================
+ * Abstract:
+ * Initialize rtMinusInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetMinusInf(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T minf = 0.0;
+ if (bitsPerReal == 32U) {
+ minf = rtGetMinusInfF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ minf = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF00000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ minf = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return minf;
+}
+
+/* Function: rtGetMinusInfF ==================================================
+ * Abstract:
+ * Initialize rtMinusInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetMinusInfF(void)
+{
+ IEEESingle minfF;
+ minfF.wordL.wordLuint = 0xFF800000U;
+ return minfF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetInf.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h new file mode 100755 index 000000000..ab2d5a70d --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/*
+ * rtGetInf.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __RTGETINF_H__
+#define __RTGETINF_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetInf(void);
+extern real32_T rtGetInfF(void);
+extern real_T rtGetMinusInf(void);
+extern real32_T rtGetMinusInfF(void);
+
+#endif
+/* End of code generation (rtGetInf.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c new file mode 100755 index 000000000..17a093461 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finite, NaN
+ */
+#include "rtGetNaN.h"
+#define NumBitsPerChar 8U
+
+/* Function: rtGetNaN ==================================================
+ * Abstract:
+ * Initialize rtNaN needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetNaN(void)
+{
+ size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+ real_T nan = 0.0;
+ if (bitsPerReal == 32U) {
+ nan = rtGetNaNF();
+ } else {
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ union {
+ LittleEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0xFFF80000U;
+ tmpVal.bitVal.words.wordL = 0x00000000U;
+ nan = tmpVal.fltVal;
+ break;
+ }
+
+ case BigEndian:
+ {
+ union {
+ BigEndianIEEEDouble bitVal;
+ real_T fltVal;
+ } tmpVal;
+
+ tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
+ tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
+ nan = tmpVal.fltVal;
+ break;
+ }
+ }
+ }
+
+ return nan;
+}
+
+/* Function: rtGetNaNF ==================================================
+ * Abstract:
+ * Initialize rtNaNF needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetNaNF(void)
+{
+ IEEESingle nanF = { { 0 } };
+ uint16_T one = 1U;
+ enum {
+ LittleEndian,
+ BigEndian
+ } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+ switch (machByteOrder) {
+ case LittleEndian:
+ {
+ nanF.wordL.wordLuint = 0xFFC00000U;
+ break;
+ }
+
+ case BigEndian:
+ {
+ nanF.wordL.wordLuint = 0x7FFFFFFFU;
+ break;
+ }
+ }
+
+ return nanF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetNaN.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h new file mode 100755 index 000000000..2c254bbbf --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/*
+ * rtGetNaN.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __RTGETNAN_H__
+#define __RTGETNAN_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetNaN(void);
+extern real32_T rtGetNaNF(void);
+
+#endif
+/* End of code generation (rtGetNaN.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c new file mode 100755 index 000000000..005c4f28d --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/*
+ * rt_nonfinite.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/*
+ * Abstract:
+ * MATLAB for code generation function to initialize non-finites,
+ * (Inf, NaN and -Inf).
+ */
+#include "rt_nonfinite.h"
+#include "rtGetNaN.h"
+#include "rtGetInf.h"
+
+real_T rtInf;
+real_T rtMinusInf;
+real_T rtNaN;
+real32_T rtInfF;
+real32_T rtMinusInfF;
+real32_T rtNaNF;
+
+/* Function: rt_InitInfAndNaN ==================================================
+ * Abstract:
+ * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
+ * generated code. NaN is initialized as non-signaling. Assumes IEEE.
+ */
+void rt_InitInfAndNaN(size_t realSize)
+{
+ (void) (realSize);
+ rtNaN = rtGetNaN();
+ rtNaNF = rtGetNaNF();
+ rtInf = rtGetInf();
+ rtInfF = rtGetInfF();
+ rtMinusInf = rtGetMinusInf();
+ rtMinusInfF = rtGetMinusInfF();
+}
+
+/* Function: rtIsInf ==================================================
+ * Abstract:
+ * Test if value is infinite
+ */
+boolean_T rtIsInf(real_T value)
+{
+ return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
+}
+
+/* Function: rtIsInfF =================================================
+ * Abstract:
+ * Test if single-precision value is infinite
+ */
+boolean_T rtIsInfF(real32_T value)
+{
+ return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
+}
+
+/* Function: rtIsNaN ==================================================
+ * Abstract:
+ * Test if value is not a number
+ */
+boolean_T rtIsNaN(real_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+ return _isnan(value)? TRUE:FALSE;
+#else
+ return (value!=value)? 1U:0U;
+#endif
+}
+
+/* Function: rtIsNaNF =================================================
+ * Abstract:
+ * Test if single-precision value is not a number
+ */
+boolean_T rtIsNaNF(real32_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+ return _isnan((real_T)value)? true:false;
+#else
+ return (value!=value)? 1U:0U;
+#endif
+}
+
+
+/* End of code generation (rt_nonfinite.c) */
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h new file mode 100755 index 000000000..6481f011f --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/*
+ * rt_nonfinite.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __RT_NONFINITE_H__
+#define __RT_NONFINITE_H__
+
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+#include <float.h>
+#endif
+#include <stddef.h>
+#include "rtwtypes.h"
+
+extern real_T rtInf;
+extern real_T rtMinusInf;
+extern real_T rtNaN;
+extern real32_T rtInfF;
+extern real32_T rtMinusInfF;
+extern real32_T rtNaNF;
+extern void rt_InitInfAndNaN(size_t realSize);
+extern boolean_T rtIsInf(real_T value);
+extern boolean_T rtIsInfF(real32_T value);
+extern boolean_T rtIsNaN(real_T value);
+extern boolean_T rtIsNaNF(real32_T value);
+
+typedef struct {
+ struct {
+ uint32_T wordH;
+ uint32_T wordL;
+ } words;
+} BigEndianIEEEDouble;
+
+typedef struct {
+ struct {
+ uint32_T wordL;
+ uint32_T wordH;
+ } words;
+} LittleEndianIEEEDouble;
+
+typedef struct {
+ union {
+ real32_T wordLreal;
+ uint32_T wordLuint;
+ } wordL;
+} IEEESingle;
+
+#endif
+/* End of code generation (rt_nonfinite.h) */
diff --git a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw new file mode 100755 index 000000000..1fb585abd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw @@ -0,0 +1 @@ +Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a.
diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h new file mode 100755 index 000000000..fd629ebcd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================*
+ * Target hardware information
+ * Device type: Generic->MATLAB Host Computer
+ * Number of bits: char: 8 short: 16 int: 32
+ * long: 32 native word size: 32
+ * Byte ordering: LittleEndian
+ * Signed integer division rounds to: Zero
+ * Shift right on a signed integer as arithmetic shift: on
+ *=======================================================================*/
+
+/*=======================================================================*
+ * Fixed width word size data types: *
+ * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
+ * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
+ * real32_T, real64_T - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================*
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
+ * ulong_T, char_T and byte_T. *
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================*
+ * Complex number type definitions *
+ *===========================================================================*/
+#define CREAL_T
+ typedef struct {
+ real32_T re;
+ real32_T im;
+ } creal32_T;
+
+ typedef struct {
+ real64_T re;
+ real64_T im;
+ } creal64_T;
+
+ typedef struct {
+ real_T re;
+ real_T im;
+ } creal_T;
+
+ typedef struct {
+ int8_T re;
+ int8_T im;
+ } cint8_T;
+
+ typedef struct {
+ uint8_T re;
+ uint8_T im;
+ } cuint8_T;
+
+ typedef struct {
+ int16_T re;
+ int16_T im;
+ } cint16_T;
+
+ typedef struct {
+ uint16_T re;
+ uint16_T im;
+ } cuint16_T;
+
+ typedef struct {
+ int32_T re;
+ int32_T im;
+ } cint32_T;
+
+ typedef struct {
+ uint32_T re;
+ uint32_T im;
+ } cuint32_T;
+
+
+/*=======================================================================*
+ * Min and Max: *
+ * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
+ * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
+ *=======================================================================*/
+
+#define MAX_int8_T ((int8_T)(127))
+#define MIN_int8_T ((int8_T)(-128))
+#define MAX_uint8_T ((uint8_T)(255))
+#define MIN_uint8_T ((uint8_T)(0))
+#define MAX_int16_T ((int16_T)(32767))
+#define MIN_int16_T ((int16_T)(-32768))
+#define MAX_uint16_T ((uint16_T)(65535))
+#define MIN_uint16_T ((uint16_T)(0))
+#define MAX_int32_T ((int32_T)(2147483647))
+#define MIN_int32_T ((int32_T)(-2147483647-1))
+#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T ((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+# ifndef false
+# define false (0U)
+# endif
+# ifndef true
+# define true (1U)
+# endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX 64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */
diff --git a/apps/commander/.context b/apps/commander/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/commander/.context diff --git a/apps/commander/Makefile b/apps/commander/Makefile new file mode 100644 index 000000000..a1149eb8d --- /dev/null +++ b/apps/commander/Makefile @@ -0,0 +1,45 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Commander application +# + +APPNAME = commander +PRIORITY = SCHED_PRIORITY_MAX - 10 +STACKSIZE = 3072 + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk + diff --git a/apps/commander/commander.c b/apps/commander/commander.c new file mode 100644 index 000000000..f342298ee --- /dev/null +++ b/apps/commander/commander.c @@ -0,0 +1,862 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander.c + * Main system state machine implementation. + */ + +#include "commander.h" + +#include <nuttx/config.h> +#include <pthread.h> +#include <stdio.h> +#include <stdlib.h> +#include <stdbool.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <sys/prctl.h> +#include <v1.0/common/mavlink.h> +#include <string.h> +#include <arch/board/drv_led.h> +#include <arch/board/up_hrt.h> +#include <arch/board/drv_tone_alarm.h> +#include <arch/board/up_hrt.h> +#include "state_machine_helper.h" +#include "systemlib/systemlib.h" +#include <math.h> +#include <poll.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_command.h> +#include <mavlink/mavlink_log.h> + +#include <systemlib/systemlib.h> + +#include <arch/board/up_cpuload.h> +extern struct system_load_s system_load; + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define STICK_ON_OFF_LIMIT 7500 +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define GPS_FIX_TYPE_2D 2 +#define GPS_FIX_TYPE_3D 3 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT 50 + +/* File descriptors */ +static int leds; +static int buzzer; +static int mavlink_fd; +static bool commander_initialized = false; +static struct vehicle_status_s current_status = { + .state_machine = SYSTEM_STATE_PREFLIGHT, + .mode = 0 +}; /**< Main state machine */ +static int stat_pub; + +static uint16_t nofix_counter = 0; +static uint16_t gotfix_counter = 0; + +static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); + +/* pthread loops */ +static void *command_handling_loop(void *arg); +// static void *subsystem_info_loop(void *arg); + +__EXPORT int commander_main(int argc, char *argv[]); + +#ifdef CONFIG_TONE_ALARM +static int buzzer_init(void); +static void buzzer_deinit(void); + +static int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + fprintf(stderr, "[commander] Buzzer: open fail\n"); + return ERROR; + } + + return 0; +} + +static void buzzer_deinit() +{ + close(buzzer); +} +#endif + +static int led_init(void); +static void led_deinit(void); +static int led_toggle(int led); +static int led_on(int led); +static int led_off(int led); + +static int led_init() +{ + leds = open("/dev/led", O_RDONLY | O_NONBLOCK); + + if (leds < 0) { + fprintf(stderr, "[commander] LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + fprintf(stderr, "[commander] LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +static void led_deinit() +{ + close(leds); +} + +static int led_toggle(int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +static int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +static int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + +enum AUDIO_PATTERN { + AUDIO_PATTERN_ERROR = 1, + AUDIO_PATTERN_NOTIFY_POSITIVE = 2, + AUDIO_PATTERN_NOTIFY_NEUTRAL = 3, + AUDIO_PATTERN_NOTIFY_NEGATIVE = 4, + AUDIO_PATTERN_TETRIS = 5 +}; + +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) { + + /* Trigger alarm if going into any error state */ + if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || + ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); + } + + /* Trigger neutral on arming / disarming */ + if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); + } + + /* Trigger Tetris on being bored */ + + return 0; +} + +void do_gyro_calibration(void) +{ + + const int calibration_count = 3000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0, 0, 0}; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + if (poll(fds, 1, 1000)) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_raw[0]; + gyro_offset[1] += raw.gyro_raw[1]; + gyro_offset[2] += raw.gyro_raw[2]; + calibration_counter++; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET] = gyro_offset[0]; + global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET] = gyro_offset[1]; + global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2]; + + char offset_output[50]; + sprintf(offset_output, "[commander] gyro calibration finished, offsets: x:%d, y:%d, z:%d", (int)gyro_offset[0], (int)gyro_offset[1], (int)gyro_offset[2]); + mavlink_log_info(mavlink_fd, offset_output); + + close(sub_sensor_combined); + + // XXX Add a parameter changed broadcast notification +} + + + +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +{ + /* result of the command */ + uint8_t result = MAV_RESULT_UNSUPPORTED; + + + /* supported command handling start */ + + /* request to set different system mode */ + switch (cmd->command) { + + + case MAV_CMD_DO_SET_MODE: + { + update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1); + } + break; +// +// case MAV_CMD_COMPONENT_ARM_DISARM: +// { +// /* request to arm */ +// if (cmd->param1 == 1.0f) { +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) +// result = MAV_RESULT_ACCEPTED; +// /* request to disarm */ +// } else if (cmd->param1 == 0.0f) { +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_STANDBY)) +// result = MAV_RESULT_ACCEPTED; +// } +// } +// break; +// +// /* request for an autopilot reboot */ +// case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: +// { +// if (cmd->param1 == 1.0f) { +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) { +// result = MAV_RESULT_ACCEPTED;//TODO: this has no effect +// } +// } +// +// } +// break; +// +// /* request to land */ +// case MAV_CMD_NAV_LAND: +// { +// //TODO: add check if landing possible +// //TODO: add landing maneuver +// +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { +// result = MAV_RESULT_ACCEPTED; +// } } +// break; +// +// /* request to takeoff */ +// case MAV_CMD_NAV_TAKEOFF: +// { +// //TODO: add check if takeoff possible +// //TODO: add takeoff maneuver +// +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { +// result = MAV_RESULT_ACCEPTED; +// } +// } +// break; +// + /* preflight calibration */ + case MAV_CMD_PREFLIGHT_CALIBRATION: { + if (cmd->param1 == 1.0) { + mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration"); + do_gyro_calibration(); + result = MAV_RESULT_ACCEPTED; + + } else { + fprintf(stderr, "[commander] refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported calibration request"); + result = MAV_RESULT_UNSUPPORTED; + } + } + break; + + /* preflight parameter load / store */ + case MAV_CMD_PREFLIGHT_STORAGE: { + /* Read all parameters from EEPROM to RAM */ + + if (cmd->param1 == 0.0) { + + if (OK == get_params_from_eeprom(global_data_parameter_storage)) { + printf("[commander] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM"); + result = MAV_RESULT_ACCEPTED; + + } else { + fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n"); + mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM"); + result = MAV_RESULT_FAILED; + } + + /* Write all parameters from RAM to EEPROM */ + + } else if (cmd->param1 == 1.0) { + + if (OK == store_params_in_eeprom(global_data_parameter_storage)) { + printf("[commander] RAM params written to EEPROM\n"); + mavlink_log_info(mavlink_fd, "[commander] RAM params written to EEPROM"); + result = MAV_RESULT_ACCEPTED; + + } else { + fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n"); + mavlink_log_critical(mavlink_fd, "[commander] ERROR writing RAM params to EEPROM"); + result = MAV_RESULT_FAILED; + } + + } else { + fprintf(stderr, "[commander] refusing unsupported storage request\n"); + mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported storage request"); + result = MAV_RESULT_UNSUPPORTED; + } + } + break; + + default: { + mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command"); + result = MAV_RESULT_UNSUPPORTED; + } + break; + } + + /* supported command handling stop */ + + + /* send any requested ACKs */ + if (cmd->confirmation > 0) { + /* send acknowledge command */ + mavlink_message_t msg; + mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result); + //global_data_send_mavlink_message_out(&msg); + + } + +} + + +static void *command_handling_loop(void *arg) //handles commands which come from the mavlink app +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander cmd handler", getpid()); + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + + while (1) { + struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } }; + + if (poll(fds, 1, 5000) == 0) { + /* timeout, but this is no problem */ + } else { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(stat_pub, ¤t_status, &cmd); + } + } + + return NULL; +} + +// static void *subsystem_info_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +// { +// /* Set thread name */ +// prctl(PR_SET_NAME, "commander subsys", getpid()); + +// uint8_t current_info_local = SUBSYSTEM_INFO_BUFFER_SIZE; +// uint16_t total_counter = 0; + +// while (1) { + +// if (0 == global_data_wait(&global_data_subsystem_info->access_conf)) { +// // printf("got subsystem_info\n"); + +// while (current_info_local != global_data_subsystem_info->current_info) { +// // printf("current_info_local = %d, current_info = %d \n", current_info_local, global_data_subsystem_info->current_info); + +// current_info_local++; + +// if (current_info_local >= SUBSYSTEM_INFO_BUFFER_SIZE) +// current_info_local = 0; + +// /* Handle the new subsystem info and write updated version of global_data_sys_status */ +// subsystem_info_t *info = &(global_data_subsystem_info->info[current_info_local]); + +// // printf("Commander got subsystem info: %d %d %d\n", info->present, info->enabled, info->health); + + +// if (info->present != 0) { +// update_state_machine_subsystem_present(stat_pub, ¤t_status, &info->subsystem_type); + +// } else { +// update_state_machine_subsystem_notpresent(stat_pub, ¤t_status, &info->subsystem_type); +// } + +// if (info->enabled != 0) { +// update_state_machine_subsystem_enabled(stat_pub, ¤t_status, &info->subsystem_type); + +// } else { +// update_state_machine_subsystem_disabled(stat_pub, ¤t_status, &info->subsystem_type); +// } + +// if (info->health != 0) { +// update_state_machine_subsystem_healthy(stat_pub, ¤t_status, &info->subsystem_type); + +// } else { +// update_state_machine_subsystem_unhealthy(stat_pub, ¤t_status, &info->subsystem_type); +// } + +// total_counter++; +// } + +// if (global_data_subsystem_info->counter - total_counter > SUBSYSTEM_INFO_BUFFER_SIZE) { +// printf("[commander] Warning: Too many subsystem status updates, subsystem_info buffer full\n"); //TODO: add to error queue +// global_data_subsystem_info->counter = total_counter; //this makes sure we print the warning only once +// } + +// global_data_unlock(&global_data_subsystem_info->access_conf); +// } +// } + +// return NULL; +// } + + + +enum BAT_CHEM { + BAT_CHEM_LITHIUM_POLYMERE = 0, +}; + +/* + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage) +{ + float ret = 0; + // XXX do this properly + // XXX rebase on parameters + const float chemistry_voltage_empty[] = {3.2f}; + const float chemistry_voltage_full[] = {4.05f}; + + ret = (voltage - cells * chemistry_voltage_empty[chemistry]) / (cells * (chemistry_voltage_full[chemistry] - chemistry_voltage_empty[chemistry])); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; +} + +/**************************************************************************** + * Name: commander + ****************************************************************************/ + +int commander_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + + /* welcome user */ + printf("[commander] I am in command now!\n"); + + /* Pthreads */ + pthread_t command_handling_thread; + // pthread_t subsystem_info_thread; + + /* initialize */ + if (led_init() != 0) { + fprintf(stderr, "[commander] ERROR: Failed to initialize leds\n"); + } + + if (buzzer_init() != 0) { + fprintf(stderr, "[commander] ERROR: Failed to initialize buzzer\n"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + } + + /* advertise to ORB */ + stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + + if (stat_pub < 0) { + printf("[commander] ERROR: orb_advertise failed.\n"); + } + + /* make sure we are in preflight state */ + //do_state_update(stat_pub, ¤t_status, (commander_state_machine_t)SYSTEM_STATE_PREFLIGHT); + + mavlink_log_info(mavlink_fd, "[commander] system is running"); + + /* load EEPROM parameters */ + if (OK == get_params_from_eeprom(global_data_parameter_storage)) { + printf("[commander] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM"); + + } else { + fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n"); + mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM"); + } + + /* create pthreads */ + pthread_attr_t command_handling_attr; + pthread_attr_init(&command_handling_attr); + pthread_attr_setstacksize(&command_handling_attr, 3072); + pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL); + + // pthread_attr_t subsystem_info_attr; + // pthread_attr_init(&subsystem_info_attr); + // pthread_attr_setstacksize(&subsystem_info_attr, 2048); + // pthread_create(&subsystem_info_thread, &subsystem_info_attr, subsystem_info_loop, NULL); + + /* Start monitoring loop */ + uint16_t counter = 0; + uint8_t flight_env; + // uint8_t fix_type; + /* Initialize to 3.0V to make sure the low-pass loads below valid threshold */ + float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS; + bool battery_voltage_valid = true; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; + uint8_t low_voltage_counter = 0; + uint16_t critical_voltage_counter = 0; + int16_t mode_switch_rc_value; + float bat_remain = 1.0f; + +// bool arm_done = false; +// bool disarm_done = false; + + uint16_t stick_off_counter = 0; + uint16_t stick_on_counter = 0; + + float hdop = 65535.0f; + + int gps_quality_good_counter = 0; + + /* Subscribe to RC data */ + int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + struct rc_channels_s rc = {0}; + + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps = {0}; + + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors = {0}; + + uint8_t vehicle_state_previous = current_status.state_machine; + + uint64_t last_idle_time = 0; + + /* now initialized */ + commander_initialized = true; + + while (1) { //TODO: this while loop needs cleanup, split into sub-functions + + /* Get current values */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + + battery_voltage = sensors.battery_voltage_v; + battery_voltage_valid = sensors.battery_voltage_valid; + bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage); + + flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + + /* Slow but important 5 Hz checks */ + if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { + /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ + if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || current_status.state_machine == SYSTEM_STATE_AUTO || current_status.state_machine == SYSTEM_STATE_MANUAL)) { + /* armed */ + led_toggle(LED_BLUE); + + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_BLUE); + } + + /* toggle error led at 5 Hz in HIL mode */ + if ((current_status.mode & MAV_MODE_FLAG_HIL_ENABLED)) { + /* armed */ + led_toggle(LED_AMBER); + + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle error (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); + + } else { + /* Constant error indication in standby mode without GPS */ + if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR && !current_status.gps_valid) { + led_on(LED_AMBER); + + } else { + led_off(LED_AMBER); + } + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + //compute system load + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + } + } + + // // XXX Export patterns and threshold to parameters + /* Trigger audio event for low battery */ + if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { + /* For less than 10%, start be really annoying at 5 Hz */ + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, 3); + + } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { + ioctl(buzzer, TONE_SET_ALARM, 0); + + } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { + /* For less than 20%, start be slightly annoying at 1 Hz */ + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, 2); + + } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { + ioctl(buzzer, TONE_SET_ALARM, 0); + } + + /* Check if last transition deserved an audio event */ +#warning This code depends on state that is no longer? maintained +#if 0 + trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine); +#endif + + /* only check gps fix if we are outdoor */ +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// +// hdop = (float)(gps.eph) / 100.0f; +// +// /* check if gps fix is ok */ +// if (gps.fix_type == GPS_FIX_TYPE_3D) { //TODO: is 2d-fix ok? //see http://en.wikipedia.org/wiki/Dilution_of_precision_%28GPS%29 +// +// if (gotfix_counter >= GPS_GOTFIX_COUNTER_REQUIRED) { //TODO: add also a required time? +// update_state_machine_got_position_fix(stat_pub, ¤t_status); +// gotfix_counter = 0; +// } else { +// gotfix_counter++; +// } +// nofix_counter = 0; +// +// if (hdop < 5.0f) { //TODO: this should be a parameter +// if (gps_quality_good_counter > GPS_QUALITY_GOOD_COUNTER_LIMIT) { +// current_status.gps_valid = true;//--> position estimator can use the gps measurements +// } +// +// gps_quality_good_counter++; +// +// +//// if(counter%10 == 0)//for testing only +//// printf("gps_quality_good_counter = %u\n", gps_quality_good_counter);//for testing only +// +// } else { +// gps_quality_good_counter = 0; +// current_status.gps_valid = false;//--> position estimator can not use the gps measurements +// } +// +// } else { +// gps_quality_good_counter = 0; +// current_status.gps_valid = false;//--> position estimator can not use the gps measurements +// +// if (nofix_counter > GPS_NOFIX_COUNTER_LIMIT) { //TODO: add also a timer limit? +// update_state_machine_no_position_fix(stat_pub, ¤t_status); +// nofix_counter = 0; +// } else { +// nofix_counter++; +// } +// gotfix_counter = 0; +// } +// +// } +// +// +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests + update_state_machine_got_position_fix(stat_pub, ¤t_status); + /* end: check gps */ + + /* Check battery voltage */ + /* write to sys_status */ + current_status.voltage_battery = battery_voltage; + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!"); + } + + low_voltage_counter++; + } + + /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + else if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_CRITICAL_VOLTS && false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!"); + state_machine_emergency(stat_pub, ¤t_status); + } + + critical_voltage_counter++; + + } else { + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + /* Start RC state check */ + + int16_t chan3_scale = rc.chan[rc.function[YAW]].scale; + int16_t chan2_scale = rc.chan[rc.function[THROTTLE]].scale; + + /* check if left stick is in lower left position --> switch to standby state */ + if (chan3_scale > STICK_ON_OFF_LIMIT && chan2_scale < -STICK_ON_OFF_LIMIT) { //TODO: remove hardcoded values + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_disarm(stat_pub, ¤t_status); + stick_on_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } + + /* check if left stick is in lower right position --> arm */ + if (chan3_scale < -STICK_ON_OFF_LIMIT && chan2_scale < -STICK_ON_OFF_LIMIT) { //TODO: remove hardcoded values + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_arm(stat_pub, ¤t_status); + stick_on_counter = 0; + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + + /* Check the value of the rc channel of the mode switch */ + mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; + + if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { + update_state_machine_mode_manual(stat_pub, ¤t_status); + + } else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) { + update_state_machine_mode_auto(stat_pub, ¤t_status); + + } else { + update_state_machine_mode_stabilized(stat_pub, ¤t_status); + } + + /* End mode switch */ + + /* END RC state check */ + + + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + + + + /* Store old modes to detect and act on state transitions */ + vehicle_state_previous = current_status.state_machine; + + fflush(stdout); + counter++; + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + pthread_join(command_handling_thread, NULL); + // pthread_join(subsystem_info_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + + return 0; +} + diff --git a/apps/commander/commander.h b/apps/commander/commander.h new file mode 100644 index 000000000..d537a0602 --- /dev/null +++ b/apps/commander/commander.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file Main system state machine definition */ + +#ifndef COMMANDER_H_ +#define COMMANDER_H_ + +#define VOLTAGE_BATTERY_HIGH_VOLTS 12.0f +#define VOLTAGE_BATTERY_LOW_VOLTS 10.5f +#define VOLTAGE_BATTERY_CRITICAL_VOLTS 10.0f +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + +#endif /* COMMANDER_H_ */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c new file mode 100644 index 000000000..2171ccedf --- /dev/null +++ b/apps/commander/state_machine_helper.c @@ -0,0 +1,564 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file State machine helper functions implementations */ + +#include <stdio.h> +#include "state_machine_helper.h" + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <systemlib/systemlib.h> +#include <arch/board/up_hrt.h> + +static const char* system_state_txt[] = { + "SYSTEM_STATE_PREFLIGHT", + "SYSTEM_STATE_STANDBY", + "SYSTEM_STATE_GROUND_READY", + "SYSTEM_STATE_MANUAL", + "SYSTEM_STATE_STABILIZED", + "SYSTEM_STATE_AUTO", + "SYSTEM_STATE_MISSION_ABORT", + "SYSTEM_STATE_EMCY_LANDING", + "SYSTEM_STATE_EMCY_CUTOFF", + "SYSTEM_STATE_GROUND_ERROR", + "SYSTEM_STATE_REBOOT", + +}; + + +void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state) +{ + int invalid_state = false; + + switch (new_state) { + case SYSTEM_STATE_MISSION_ABORT: { + /* Indoor or outdoor */ + uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + + if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + + } else { + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); + } + + return; + } + break; + + case SYSTEM_STATE_EMCY_LANDING: + /* Tell the controller to land */ + //TODO: add emcy landing code here + + fprintf(stderr, "[commander] EMERGENCY LANDING!\n"); + //global_data_send_mavlink_statustext_message_out("Commander: state: emergency landing", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + /* Tell the controller to cutoff the motors (thrust = 0), make sure that this is not overwritten by another app and stays at 0 */ + //TODO: add emcy cutoff code here + + fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n"); + //global_data_send_mavlink_statustext_message_out("Commander: state: emergency cutoff", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_GROUND_ERROR: + fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n"); + //global_data_send_mavlink_statustext_message_out("Commander: state: ground error", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_PREFLIGHT: + //global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_REBOOT: + usleep(500000); + reboot(); + break; + + case SYSTEM_STATE_STANDBY: + //global_data_send_mavlink_statustext_message_out("Commander: state: standby", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_GROUND_READY: + //global_data_send_mavlink_statustext_message_out("Commander: state: armed", MAV_SEVERITY_INFO); + + //if in manual mode switch to manual state +// if (current_status->remote_manual) { +// printf("[commander] manual mode\n"); +// do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL); +// return; +// } + + break; + + case SYSTEM_STATE_AUTO: + //global_data_send_mavlink_statustext_message_out("Commander: state: auto", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_STABILIZED: + //global_data_send_mavlink_statustext_message_out("Commander: state: stabilized", MAV_SEVERITY_INFO); + break; + + case SYSTEM_STATE_MANUAL: + //global_data_send_mavlink_statustext_message_out("Commander: state: manual", MAV_SEVERITY_INFO); + break; + + default: + invalid_state = true; + break; + } + + if (invalid_state == false) { + //publish the new state + current_status->state_machine = new_state; + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + printf("[commander] new state: %s\n", system_state_txt[new_state]); + } +} + + +/* + * Private functions, update the state machine + */ +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status) +{ + fprintf(stderr, "[commander] 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, (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_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); + + } else { + fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine); + } +} + +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors +{ + if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself + state_machine_emergency_always_critical(status_pub, current_status); + + } 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); + } + +} + + + +// /* +// * Wrapper functions (to be used in the commander), all functions assume lock on current_status +// */ + +// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR +// * +// * START SUBSYSTEM/EMERGENCY FUNCTIONS +// * */ + +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was removed something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was disabled something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + + +// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //TODO state machine change (recovering) +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //TODO state machine change +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //TODO state machine change +// break; + +// case SUBSYSTEM_TYPE_GPS: +// //TODO state machine change +// break; + +// default: +// break; +// } + + +// } + + +// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); +// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_GPS: +// // //TODO: remove this block +// // break; +// // /////////////////// +// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); + +// // printf("previosly_healthy = %u\n", previosly_healthy); +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency(status_pub, current_status); + +// break; + +// default: +// break; +// } + +// } + + +/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ + + +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { + state_machine_emergency(status_pub, current_status); + } +} + +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status) +{ + if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + printf("[commander] arming\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + } /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) { + + printf("[commander] landing\n"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + }*/ +} + +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status) +{ + 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"); + do_state_update(status_pub, current_status, (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"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status) +{ + 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"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + } +} + +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status) +{ + 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"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + } +} + +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status) +{ + 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"); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO); + } +} + + +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode) +{ + commander_state_machine_t current_system_state = current_status->state_machine; + + printf("in update state request\n"); + uint8_t ret = 1; + + /* Switch on HIL if in standby */ + if ((current_system_state == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) { + /* Enable HIL on request */ + current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* NEVER actually switch off HIL without reboot */ + if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) { + fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n"); + ret = ERROR; + } + + //TODO: clarify mapping between mavlink enum MAV_MODE and the state machine, then add more decisions to the switch (see also the system_state_loop function in mavlink.c) + switch (mode) { + case MAV_MODE_MANUAL_ARMED: //= SYSTEM_STATE_ARMED + if (current_system_state == SYSTEM_STATE_STANDBY) { + /* Set armed flag */ + current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED; + /* Set manual input enabled flag */ + current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + ret = OK; + } + + break; + + case MAV_MODE_MANUAL_DISARMED: + if (current_system_state == SYSTEM_STATE_GROUND_READY) { + /* Clear armed flag, leave manual input enabled */ + current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + ret = OK; + } + + break; + + default: + break; + } + + #warning STATE MACHINE IS INCOMPLETE HERE + +// if(ret != 0) //Debug +// { +// strcpy(mavlink_statustext_msg_content.values[0].string_value, "Commander: command rejected"); +// global_data_send_mavlink_message_out(&mavlink_statustext_msg_content); +// } + + return ret; +} + +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations +{ + commander_state_machine_t current_system_state = current_status->state_machine; + + uint8_t ret = 1; + + switch (custom_mode) { + case SYSTEM_STATE_GROUND_READY: + break; + + case SYSTEM_STATE_STANDBY: + break; + + case SYSTEM_STATE_REBOOT: + printf("try to reboot\n"); + + if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) { + printf("system will reboot\n"); + //global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO); + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_REBOOT); + ret = 0; + } + + break; + + case SYSTEM_STATE_AUTO: + printf("try to switch to auto/takeoff\n"); + + if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO); + printf("state: auto\n"); + ret = 0; + } + + break; + + case SYSTEM_STATE_MANUAL: + printf("try to switch to manual\n"); + + if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { + do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + printf("state: manual\n"); + ret = 0; + } + + break; + + default: + break; + } + + return ret; +} + diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h new file mode 100644 index 000000000..0abc6349c --- /dev/null +++ b/apps/commander/state_machine_helper.h @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file State machine helper functions definitions */ + +#ifndef STATE_MACHINE_HELPER_H_ +#define STATE_MACHINE_HELPER_H_ + +#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) +#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <v1.0/common/mavlink.h> + + +/* + * @brief switch to new state with no checking * + * + * do_state_update: this is the functions that all other functions have to call in order to update the state. + * the function does not question the state change, this must be done before + * The function performs actions that are connected with the new state (buzzer, reboot, ...) + * + * + */ +void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state); + +/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + +// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + + +/* + * @brief handle state machine if got position fix + */ +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status); + +/* + * @brief handle state machine if position fix lost + */ +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status); + +/* + * @brief handle state machine if user wants to arm + */ +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status); + +/* + * @brief handle state machine if user wants to disarm + */ +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status); + +/* + * @brief handle state machine if mode switch is manual + */ +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status); + +/* + * @brief handle state machine if mode switch is stabilized + */ +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status); + +/* + * @brief handle state machine if mode switch is auto + */ +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status); + + + + +/* + * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). + * If the request is obeyed the functions return 0 + * + */ + +/* + * @brief handles *incoming request* to switch to a specific state, if state change is successful returns 0 + */ +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode); + +/* + * @brief handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 + */ +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode); + + +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status); +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status); + + + + +#endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/doxygen_mainpage.dox b/apps/doxygen_mainpage.dox new file mode 100644 index 000000000..98b0eb690 --- /dev/null +++ b/apps/doxygen_mainpage.dox @@ -0,0 +1,49 @@ +/** @mainpage PX4 Codebase Overview + +PX4 is an open-source, open-hardware project aiming at providing a high-end autopilot to the academic, hobby and industrial communities (BSD licensed). For full documentation, refer to the official project website: + +https://pixhawk.ethz.ch/px4/ + +Please follow the toolchain installation instructions at https://pixhawk.ethz.ch/px4/dev/px4_quickstart. + +*/ + +/** +@defgroup topics uORB Topics + +Small and efficient object request broker. + +The micro object request broker (uORB) is a very efficient implementation of an object +request broker following the publisher/subscriber design pattern. + +It is in detail described here: +https://pixhawk.ethz.ch/px4/dev/shared_object_communication +*/ + +/** +@defgroup apps Onboard Applications + +This is the list of the main functions of all onboard applications. The use of these +applications is documented here: +https://pixhawk.ethz.ch/px4/users/apps/start + +Developer documentation of individual applications and the uORB data sharing mechanisms +are described here: +https://pixhawk.ethz.ch/px4/dev/apps/start + +*/ + +/** +@defgroup attitude_estimation Attitude Estimation + +*/ + +/** +@defgroup position_estimation Position Estimation + +*/ + +/** + * @addtogroup topics + * @{ + */
\ No newline at end of file diff --git a/apps/drivers/bma180/Makefile b/apps/drivers/bma180/Makefile new file mode 100644 index 000000000..cc01b629e --- /dev/null +++ b/apps/drivers/bma180/Makefile @@ -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. +# +############################################################################ + +# +# Makefile to build the BMA180 driver. +# + +APPNAME = bma180 +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp new file mode 100644 index 000000000..a5d66d86b --- /dev/null +++ b/apps/drivers/bma180/bma180.cpp @@ -0,0 +1,774 @@ +/**************************************************************************** + * + * 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 Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI. + */ + +#include <nuttx/config.h> + +#include <device/spi.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stddef.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/up_hrt.h> + +#include <drivers/drv_accel.h> + +extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); } + +class BMA180 : public device::SPI +{ +public: + BMA180(int bus, spi_dev_e device); + ~BMA180(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int open_first(struct file *filp); + virtual int close_last(struct file *filp); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + struct hrt_call _call; + unsigned _call_interval; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct accel_report *_reports; + + struct accel_scale _scale; + float _range_scale; + + unsigned _reads; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Read a register from the BMA180 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the BMA180 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the BMA180 + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the BMA180 measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int set_range(unsigned max_g); + + /** + * Set the BMA180 lowpass filter. + * + * @param frequency Set the lowpass filter cutoff frequency to no less than + * this frequency. + * @return OK if the value can be supported. + */ + int set_bandwidth(unsigned frequency); +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) + +#define ADDR_CHIP_ID 0x00 +#define CHIP_ID 0x03 + +#define ADDR_ACC_X_LSB 0x02 +#define ADDR_ACC_Y_LSB 0x04 +#define ADDR_ACC_Z_LSB 0x06 +#define ADDR_TEMPERATURE 0x08 + +#define ADDR_RESET 0x10 +#define SOFT_RESET 0xB6 + +#define ADDR_BW_TCS 0x20 +#define BW_TCS_BW_MASK (0xf<<4) +#define BW_TCS_BW_10HZ (0<<4) +#define BW_TCS_BW_20HZ (1<<4) +#define BW_TCS_BW_40HZ (2<<4) +#define BW_TCS_BW_75HZ (3<<4) +#define BW_TCS_BW_150HZ (4<<4) +#define BW_TCS_BW_300HZ (5<<4) +#define BW_TCS_BW_600HZ (6<<4) +#define BW_TCS_BW_1200HZ (7<<4) + +#define ADDR_HIGH_DUR 0x27 +#define HIGH_DUR_DIS_I2C (1<<0) + +#define ADDR_TCO_Z 0x30 +#define TCO_Z_MODE_MASK 0x3 + +#define ADDR_GAIN_Y 0x33 +#define GAIN_Y_SHADOW_DIS (1<<0) + +#define ADDR_OFFSET_LSB1 0x35 +#define OFFSET_LSB1_RANGE_MASK (7<<1) +#define OFFSET_LSB1_RANGE_1G (0<<1) +#define OFFSET_LSB1_RANGE_2G (2<<1) +#define OFFSET_LSB1_RANGE_3G (3<<1) +#define OFFSET_LSB1_RANGE_4G (4<<1) +#define OFFSET_LSB1_RANGE_8G (5<<1) +#define OFFSET_LSB1_RANGE_16G (6<<1) + +#define ADDR_OFFSET_T 0x37 +#define OFFSET_T_READOUT_12BIT (1<<0) + +/* + * Driver 'main' command. + */ +extern "C" { int bma180_main(int argc, char *argv[]); } + + +BMA180::BMA180(int bus, spi_dev_e device) : + SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _reads(0) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors + _scale.x_offset = 0; + _scale.x_scale = 1.0f; + _scale.y_offset = 0; + _scale.y_scale = 1.0f; + _scale.z_offset = 0; + _scale.z_scale = 1.0f; +} + +BMA180::~BMA180() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +BMA180::init() +{ + int ret; + + /* do SPI init (and probe) first */ + ret = SPI::init(); + + /* if probe/setup successful, finish chip init */ + if (ret == OK) { + + /* perform soft reset (p48) */ + write_reg(ADDR_RESET, SOFT_RESET); + + /* wait 10us (p49) */ + usleep(10); + + /* disable I2C interface */ + modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0); + + /* switch to low-noise mode */ + modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0); + + /* disable 12-bit mode */ + modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0); + + /* disable shadow-disable mode */ + modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0); + } + + return ret; +} + +int +BMA180::open_first(struct file *filp) +{ + /* reset to manual-poll mode */ + _call_interval = 0; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct accel_report[_num_reports]; + _oldest_report = _next_report = 0; + + /* set default range and lowpass */ + set_range(4); /* 4G */ + set_bandwidth(600); /* 600Hz */ + + return OK; +} + +int +BMA180::close_last(struct file *filp) +{ + /* stop measurement */ + stop(); + + /* free report buffers */ + if (_reports != nullptr) { + delete[] _reports; + _num_reports = 0; + } + + return OK; +} + +int +BMA180::probe() +{ + if (read_reg(ADDR_CHIP_ID) == CHIP_ID) + return OK; + + return -EIO; +} + +ssize_t +BMA180::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct accel_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + _reads++; + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_report = _next_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + return ret; +} + +int +BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case ACCELIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case ACC_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case ACC_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call.period = _call_interval; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case ACCELIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case ACCELIOCSLOWPASS: + return set_bandwidth(arg); + + case ACCELIORANGE: + return set_range(arg); + + case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */ + case ACCELIOCSREPORTFORMAT: /* no alternate report formats */ + return -EINVAL; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +uint8_t +BMA180::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +BMA180::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +BMA180::set_range(unsigned max_g) +{ + uint8_t rangebits; + float rangescale; + + if (max_g > 16) { + return -ERANGE; + + } else if (max_g > 8) { /* 16G */ + rangebits = OFFSET_LSB1_RANGE_16G; + rangescale = 1.98; + + } else if (max_g > 4) { /* 8G */ + rangebits = OFFSET_LSB1_RANGE_8G; + rangescale = 0.99; + + } else if (max_g > 3) { /* 4G */ + rangebits = OFFSET_LSB1_RANGE_4G; + rangescale = 0.5; + + } else if (max_g > 2) { /* 3G */ + rangebits = OFFSET_LSB1_RANGE_3G; + rangescale = 0.38; + + } else if (max_g > 1) { /* 2G */ + rangebits = OFFSET_LSB1_RANGE_2G; + rangescale = 0.25; + + } else { /* 1G */ + rangebits = OFFSET_LSB1_RANGE_1G; + rangescale = 0.13; + } + + /* adjust sensor configuration */ + modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits); + _range_scale = rangescale; + + return OK; +} + +int +BMA180::set_bandwidth(unsigned frequency) +{ + uint8_t bwbits; + + if (frequency > 1200) { + return -ERANGE; + + } else if (frequency > 600) { + bwbits = BW_TCS_BW_1200HZ; + + } else if (frequency > 300) { + bwbits = BW_TCS_BW_600HZ; + + } else if (frequency > 150) { + bwbits = BW_TCS_BW_300HZ; + + } else if (frequency > 75) { + bwbits = BW_TCS_BW_150HZ; + + } else if (frequency > 40) { + bwbits = BW_TCS_BW_75HZ; + + } else if (frequency > 20) { + bwbits = BW_TCS_BW_40HZ; + + } else if (frequency > 10) { + bwbits = BW_TCS_BW_20HZ; + + } else { + bwbits = BW_TCS_BW_10HZ; + } + + /* adjust sensor configuration */ + modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits); + + return OK; +} + +void +BMA180::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _oldest_report = _next_report = 0; + + /* start polling at the specified rate */ + hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); +} + +void +BMA180::stop() +{ + hrt_cancel(&_call); +} + +void +BMA180::measure_trampoline(void *arg) +{ + BMA180 *dev = (BMA180 *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +BMA180::measure() +{ + /* + * This evil is to deal with the stupid layout of the BMA180 + * measurement registers vs. the SPI transaction model. + */ + union { + uint8_t bytes[10]; + uint16_t words[5]; + } buf; + + /* + * Fetch the full set of measurements from the BMA180 in one pass; + * 7 bytes starting from the X LSB. + */ + buf.bytes[1] = ADDR_ACC_X_LSB; + transfer(&buf.bytes[1], &buf.bytes[1], 8); + + /* + * Adjust and scale results to mg. + * + * Note that we ignore the "new data" bits. At any time we read, each + * of the axis measurements are the "most recent", even if we've seen + * them before. There is no good way to synchronise with the internal + * measurement flow without using the external interrupt. + */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].x = (buf.words[1] >> 2) * _range_scale; + _reports[_next_report].y = (buf.words[2] >> 2) * _range_scale; + _reports[_next_report].z = (buf.words[3] >> 2) * _range_scale; + + /* + * @todo Apply additional scaling / calibration factors here. + */ + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, fix it */ + if (_next_report == _oldest_report) + INCREMENT(_oldest_report, _num_reports); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); +} + +void +BMA180::print_info() +{ + printf("reads: %u\n", _reads); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +BMA180 *g_dev; + +/* + * XXX this should just be part of the generic sensors test... + */ + +int +test() +{ + int fd = -1; + struct accel_report report; + ssize_t sz; + const char *reason = "test OK"; + + do { + + /* get the driver */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + reason = "can't open driver"; + break; + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + reason = "immediate read failed"; + break; + } + + printf("single read\n"); + fflush(stdout); + printf("time: %lld\n", report.timestamp); + printf("x: %f\n", report.x); + printf("y: %f\n", report.y); + printf("z: %f\n", report.z); + + } while (0); + + printf("BMA180: %s\n", reason); + + return OK; +} + +int +info() +{ + if (g_dev == nullptr) { + fprintf(stderr, "BMA180: driver not running\n"); + return -ENOENT; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return OK; +} + + +} // namespace + +int +bma180_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + fprintf(stderr, "BMA180: already loaded\n"); + return -EBUSY; + } + + /* create the driver */ + g_dev = new BMA180(CONFIG_BMA180_SPI_BUS, (spi_dev_e)CONFIG_BMA180_SPI_DEVICE); + + if (g_dev == nullptr) { + fprintf(stderr, "BMA180: driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "BMA180: driver init failed\n"); + usleep(100000); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + printf("BMA180: driver started\n"); + return OK; + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + return test(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + return info(); + + fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n"); + return -EINVAL; +} diff --git a/apps/drivers/device/Makefile b/apps/drivers/device/Makefile new file mode 100644 index 000000000..f7b1fff88 --- /dev/null +++ b/apps/drivers/device/Makefile @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build the device driver framework. +# + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/device/cdev.cpp b/apps/drivers/device/cdev.cpp new file mode 100644 index 000000000..6b8bf0c2d --- /dev/null +++ b/apps/drivers/device/cdev.cpp @@ -0,0 +1,396 @@ +/**************************************************************************** + * + * 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 Character device base class. + */ + +#include "device.h" + +#include <sys/ioctl.h> +#include <arch/irq.h> + +#include <stdlib.h> +#include <stdio.h> + +#ifdef CONFIG_DISABLE_POLL +# error This driver is not compatible with CONFIG_DISABLE_POLL +#endif + +namespace device +{ + +/* how much to grow the poll waiter set each time it has to be increased */ +static const unsigned pollset_increment = 0; + +/* + * The standard NuttX operation dispatch table can't call C++ member functions + * directly, so we have to bounce them through this dispatch table. + */ +static int cdev_open(struct file *filp); +static int cdev_close(struct file *filp); +static ssize_t cdev_read(struct file *filp, char *buffer, size_t buflen); +static ssize_t cdev_write(struct file *filp, const char *buffer, size_t buflen); +static off_t cdev_seek(struct file *filp, off_t offset, int whence); +static int cdev_ioctl(struct file *filp, int cmd, unsigned long arg); +static int cdev_poll(struct file *filp, struct pollfd *fds, bool setup); + +/** + * Character device indirection table. + * + * Every cdev we register gets the same function table; we use the private data + * field in the inode to store the instance pointer. + * + * Note that we use the GNU extension syntax here because we don't get designated + * initialisers in gcc 4.6. + */ +static const struct file_operations cdev_fops = { +open : cdev_open, +close : cdev_close, +read : cdev_read, +write : cdev_write, +seek : cdev_seek, +ioctl : cdev_ioctl, +poll : cdev_poll, +}; + +CDev::CDev(const char *name, + const char *devname, + int irq) : + // base class + Device(name, irq), + // public + // protected + // private + _devname(devname), + _registered(false), + _open_count(0) +{ + for (unsigned i = 0; i < _max_pollwaiters; i++) + _pollset[i] = nullptr; +} + +CDev::~CDev() +{ + if (_registered) + unregister_driver(_devname); +} + +int +CDev::init() +{ + int ret = OK; + + // base class init first + ret = Device::init(); + + if (ret != OK) + goto out; + + // now register the driver + ret = register_driver(_devname, &cdev_fops, 0666, (void *)this); + + if (ret != OK) + goto out; + + _registered = true; + +out: + return ret; +} + +/* + * Default implementations of the character device interface + */ +int +CDev::open(struct file *filp) +{ + int ret = OK; + + lock(); + /* increment the open count */ + _open_count++; + + if (_open_count == 1) { + + /* first-open callback may decline the open */ + ret = open_first(filp); + + if (ret != OK) + _open_count--; + } + + unlock(); + + return ret; +} + +int +CDev::open_first(struct file *filp) +{ + return OK; +} + +int +CDev::close(struct file *filp) +{ + int ret = OK; + + lock(); + + if (_open_count > 0) { + /* decrement the open count */ + _open_count--; + + /* callback cannot decline the close */ + if (_open_count == 0) + ret = close_last(filp); + + } else { + ret = -EBADF; + } + + unlock(); + + return ret; +} + +int +CDev::close_last(struct file *filp) +{ + return OK; +} + +ssize_t +CDev::read(struct file *filp, char *buffer, size_t buflen) +{ + return -ENOSYS; +} + +ssize_t +CDev::write(struct file *filp, const char *buffer, size_t buflen) +{ + return -ENOSYS; +} + +off_t +CDev::seek(struct file *filp, off_t offset, int whence) +{ + return -ENOSYS; +} + +int +CDev::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + /* fetch a pointer to the driver's private data */ + case DIOC_GETPRIV: + *(void **)(uintptr_t)arg = (void *)this; + return OK; + } + + return -ENOTTY; +} + +int +CDev::poll(struct file *filp, struct pollfd *fds, bool setup) +{ + int ret = OK; + + /* + * Lock against pollnotify() (and possibly other callers) + */ + lock(); + + if (setup) { + /* + * Save the file pointer in the pollfd for the subclass' + * benefit. + */ + fds->priv = (void *)filp; + + /* + * Handle setup requests. + */ + ret = store_poll_waiter(fds); + + if (ret == OK) { + + /* + * Check to see whether we should send a poll notification + * immediately. + */ + fds->revents |= fds->events & poll_state(filp); + + /* yes? post the notification */ + if (fds->revents != 0) + sem_post(fds->sem); + } + + } else { + /* + * Handle a teardown request. + */ + ret = remove_poll_waiter(fds); + } + + unlock(); + + return ret; +} + +void +CDev::poll_notify(pollevent_t events) +{ + /* lock against poll() as well as other wakeups */ + irqstate_t state = irqsave(); + + for (unsigned i = 0; i < _max_pollwaiters; i++) + if (nullptr != _pollset[i]) + poll_notify_one(_pollset[i], events); + + irqrestore(state); +} + +void +CDev::poll_notify_one(struct pollfd *fds, pollevent_t events) +{ + /* update the reported event set */ + fds->revents |= fds->events & events; + + /* if the state is now interesting, wake the waiter if it's still asleep */ + /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ + if ((fds->revents != 0) && (fds->sem->semcount <= 0)) + sem_post(fds->sem); +} + +pollevent_t +CDev::poll_state(struct file *filp) +{ + /* by default, no poll events to report */ + return 0; +} + +int +CDev::store_poll_waiter(struct pollfd *fds) +{ + /* + * Look for a free slot. + */ + for (unsigned i = 0; i < _max_pollwaiters; i++) { + if (nullptr == _pollset[i]) { + + /* save the pollfd */ + _pollset[i] = fds; + + return OK; + } + } + + return ENOMEM; +} + +int +CDev::remove_poll_waiter(struct pollfd *fds) +{ + for (unsigned i = 0; i < _max_pollwaiters; i++) { + if (fds == _pollset[i]) { + + _pollset[i] = nullptr; + return OK; + + } + } + + puts("poll: bad fd state"); + return -EINVAL; +} + +static int +cdev_open(struct file *filp) +{ + CDev *cdev = (CDev *)(filp->f_inode->i_private); + + return cdev->open(filp); +} + +static int +cdev_close(struct file *filp) +{ + CDev *cdev = (CDev *)(filp->f_inode->i_private); + + return cdev->close(filp); +} + +static ssize_t +cdev_read(struct file *filp, char *buffer, size_t buflen) +{ + CDev *cdev = (CDev *)(filp->f_inode->i_private); + + return cdev->read(filp, buffer, buflen); +} + +static ssize_t +cdev_write(struct file *filp, const char *buffer, size_t buflen) +{ + CDev *cdev = (CDev *)(filp->f_inode->i_private); + + return cdev->write(filp, buffer, buflen); +} + +static off_t +cdev_seek(struct file *filp, off_t offset, int whence) +{ + CDev *cdev = (CDev *)(filp->f_inode->i_private); + + return cdev->seek(filp, offset, whence); +} + +static int +cdev_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + CDev *cdev = (CDev *)(filp->f_inode->i_private); + + return cdev->ioctl(filp, cmd, arg); +} + +static int +cdev_poll(struct file *filp, struct pollfd *fds, bool setup) +{ + CDev *cdev = (CDev *)(filp->f_inode->i_private); + + return cdev->poll(filp, fds, setup); +} + +} // namespace device
\ No newline at end of file diff --git a/apps/drivers/device/device.cpp b/apps/drivers/device/device.cpp new file mode 100644 index 000000000..c14a3234d --- /dev/null +++ b/apps/drivers/device/device.cpp @@ -0,0 +1,225 @@ +/**************************************************************************** + * + * 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 Fundamental driver base class for the device framework. + */ + +#include "device.h" + +#include <nuttx/arch.h> +#include <stdio.h> +#include <unistd.h> + +namespace device +{ + +/** + * Interrupt dispatch table entry. + */ +struct irq_entry { + int irq; + Device *owner; +}; + +static const unsigned irq_nentries = 8; /**< size of the interrupt dispatch table */ +static irq_entry irq_entries[irq_nentries]; /**< interrupt dispatch table (XXX should be a vector) */ + +/** + * Register an interrupt to a specific device. + * + * @param irq The interrupt number to register. + * @param owner The device receiving the interrupt. + * @return OK if the interrupt was registered. + */ +static int register_interrupt(int irq, Device *owner); + +/** + * Unregister an interrupt. + * + * @param irq The previously-registered interrupt to be de-registered. + */ +static void unregister_interrupt(int irq); + +/** + * Handle an interrupt. + * + * @param irq The interrupt being invoked. + * @param context The interrupt register context. + * @return Always returns OK. + */ +static int interrupt(int irq, void *context); + +Device::Device(const char *name, + int irq) : + // public + // protected + _name(name), + _debug_enabled(false), + // private + _irq(irq), + _irq_attached(false) +{ + sem_init(&_lock, 0, 1); +} + +Device::~Device() +{ + sem_destroy(&_lock); + + if (_irq_attached) + unregister_interrupt(_irq); +} + +int +Device::init() +{ + int ret = OK; + + // If assigned an interrupt, connect it + if (_irq) { + /* ensure it's disabled */ + up_disable_irq(_irq); + + /* register */ + ret = register_interrupt(_irq, this); + + if (ret != OK) + goto out; + + _irq_attached = true; + } + +out: + return ret; +} + +void +Device::interrupt_enable() +{ + if (_irq_attached) + up_enable_irq(_irq); +} + +void +Device::interrupt_disable() +{ + if (_irq_attached) + up_disable_irq(_irq); +} + +void +Device::interrupt(void *context) +{ + // default action is to disable the interrupt so we don't get called again + interrupt_disable(); +} + +void +Device::log(const char *fmt, ...) +{ + va_list ap; + + printf("[%s] ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +} + +void +Device::debug(const char *fmt, ...) +{ + va_list ap; + + if (_debug_enabled) { + printf("<%s> ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); + } +} + +static int +register_interrupt(int irq, Device *owner) +{ + int ret = -ENOMEM; + + // look for a slot where we can register the interrupt + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == 0) { + + // great, we could put it here; try attaching it + ret = irq_attach(irq, &interrupt); + + if (ret == OK) { + irq_entries[i].irq = irq; + irq_entries[i].owner = owner; + } + + break; + } + } + + return ret; +} + +static void +unregister_interrupt(int irq) +{ + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == irq) { + irq_entries[i].irq = 0; + irq_entries[i].owner = nullptr; + } + } +} + +static int +interrupt(int irq, void *context) +{ + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == irq) { + irq_entries[i].owner->interrupt(context); + break; + } + } + + return OK; +} + + +} // namespace device
\ No newline at end of file diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h new file mode 100644 index 000000000..98dbf8bfb --- /dev/null +++ b/apps/drivers/device/device.h @@ -0,0 +1,439 @@ +/**************************************************************************** + * + * 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 Definitions for the generic base classes in the device framework. + */ + +#ifndef _DEVICE_DEVICE_H +#define _DEVICE_DEVICE_H + +/* + * Includes here should only cover the needs of the framework definitions. + */ +#include <nuttx/config.h> + +#include <errno.h> +#include <stdbool.h> +#include <stddef.h> +#include <stdint.h> +#include <poll.h> + +#include <nuttx/fs/fs.h> + +/** + * Namespace encapsulating all device framework classes, functions and data. + */ +namespace device __EXPORT +{ + +/** + * Fundamental base class for all device drivers. + * + * This class handles the basic "being a driver" things, including + * interrupt registration and dispatch. + */ +class __EXPORT Device +{ +public: + /** + * Interrupt handler. + */ + virtual void interrupt(void *ctx); /**< interrupt handler */ + +protected: + const char *_name; /**< driver name */ + bool _debug_enabled; /**< if true, debug messages are printed */ + + /** + * Constructor + * + * @param name Driver name + * @param irq Interrupt assigned to the device. + */ + Device(const char *name, + int irq = 0); + ~Device(); + + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialised OK. + */ + virtual int init(); + + /** + * Enable the device interrupt + */ + void interrupt_enable(); + + /** + * Disable the device interrupt + */ + void interrupt_disable(); + + /** + * Take the driver lock. + * + * Each driver instance has its own lock/semaphore. + * + * Note that we must loop as the wait may be interrupted by a signal. + */ + void lock() { + do {} while (sem_wait(&_lock) != 0); + } + + /** + * Release the driver lock. + */ + void unlock() { + sem_post(&_lock); + } + + /** + * Log a message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void log(const char *fmt, ...); + + /** + * Print a debug message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void debug(const char *fmt, ...); + +private: + int _irq; + bool _irq_attached; + sem_t _lock; + + /** disable copy construction for this and all subclasses */ + Device(const Device &); + + /** disable assignment for this and all subclasses */ + Device &operator = (const Device &); + + /** + * Register ourselves as a handler for an interrupt + * + * @param irq The interrupt to claim + * @return OK if the interrupt was registered + */ + int dev_register_interrupt(int irq); + + /** + * Unregister ourselves as a handler for any interrupt + */ + void dev_unregister_interrupt(); + + /** + * Interrupt dispatcher + * + * @param irq The interrupt that has been triggered. + * @param context Pointer to the interrupted context. + */ + static void dev_interrupt(int irq, void *context); +}; + +/** + * Abstract class for any character device + */ +class __EXPORT CDev : public Device +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param irq Interrupt assigned to the device + */ + CDev(const char *name, const char *devname, int irq = 0); + + /** + * Destructor + */ + ~CDev(); + + virtual int init(); + + /** + * Handle an open of the device. + * + * This function is called for every open of the device. The default + * implementation maintains _open_count and always returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open is allowed, -errno otherwise. + */ + virtual int open(struct file *filp); + + /** + * Handle a close of the device. + * + * This function is called for every close of the device. The default + * implementation maintains _open_count and returns OK as long as it is not zero. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the close was successful, -errno otherwise. + */ + virtual int close(struct file *filp); + + /** + * Perform a read from the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer into which data should be placed. + * @param buflen The number of bytes to be read. + * @return The number of bytes read or -errno otherwise. + */ + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + /** + * Perform a write to the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer from which data should be read. + * @param buflen The number of bytes to be written. + * @return The number of bytes written or -errno otherwise. + */ + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + + /** + * Perform a logical seek operation on the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param offset The new file position relative to whence. + * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. + * @return The previous offset, or -errno otherwise. + */ + virtual off_t seek(struct file *filp, off_t offset, int whence); + + /** + * Perform an ioctl operation on the device. + * + * The default implementation handles DIOC_GETPRIV, and otherwise + * returns -ENOTTY. Subclasses should call the default implementation + * for any command they do not handle themselves. + * + * @param filp Pointer to the NuttX file structure. + * @param cmd The ioctl command value. + * @param arg The ioctl argument value. + * @return OK on success, or -errno otherwise. + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Perform a poll setup/teardown operation. + * + * This is handled internally and should not normally be overridden. + * + * @param filp Pointer to the NuttX file structure. + * @param fds Poll descriptor being waited on. + * @param arg True if this is establishing a request, false if + * it is being torn down. + * @return OK on success, or -errno otherwise. + */ + virtual int poll(struct file *filp, struct pollfd *fds, bool setup); + + /** + * Test whether the device is currently open. + * + * This can be used to avoid tearing down a device that is still active. + * + * @return True if the device is currently open. + */ + bool is_open() { return _open_count > 0; } + +protected: + /** + * Check the current state of the device for poll events from the + * perspective of the file. + * + * This function is called by the default poll() implementation when + * a poll is set up to determine whether the poll should return immediately. + * + * The default implementation returns no events. + * + * @param filp The file that's interested. + * @return The current set of poll events. + */ + virtual pollevent_t poll_state(struct file *filp); + + /** + * Report new poll events. + * + * This function should be called anytime the state of the device changes + * in a fashion that might be interesting to a poll waiter. + * + * @param events The new event(s) being announced. + */ + virtual void poll_notify(pollevent_t events); + + /** + * Internal implementation of poll_notify. + * + * @param fds A poll waiter to notify. + * @param events The event(s) to send to the waiter. + */ + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + + /** + * Notification of the first open. + * + * This function is called when the device open count transitions from zero + * to one. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open should proceed, -errno otherwise. + */ + virtual int open_first(struct file *filp); + + /** + * Notification of the last close. + * + * This function is called when the device open count transitions from + * one to zero. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open should return OK, -errno otherwise. + */ + virtual int close_last(struct file *filp); + +private: + static const unsigned _max_pollwaiters = 8; + + const char *_devname; /**< device node name */ + bool _registered; /**< true if device name was registered */ + unsigned _open_count; /**< number of successful opens */ + + struct pollfd *_pollset[_max_pollwaiters]; + + /** + * Store a pollwaiter in a slot where we can find it later. + * + * Expands the pollset as required. Must be called with the driver locked. + * + * @return OK, or -errno on error. + */ + int store_poll_waiter(struct pollfd *fds); + + /** + * Remove a poll waiter. + * + * @return OK, or -errno on error. + */ + int remove_poll_waiter(struct pollfd *fds); +}; + +/** + * Abstract class for character device accessed via PIO + */ +class __EXPORT PIO : public CDev +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param base Base address of the device PIO area + * @param irq Interrupt assigned to the device (or zero if none) + */ + PIO(const char *name, + const char *devname, + uint32_t base, + int irq = 0); + ~PIO(); + + int init(); + +protected: + + /** + * Read a register + * + * @param offset Register offset in bytes from the base address. + */ + uint32_t reg(uint32_t offset) { + return *(volatile uint32_t *)(_base + offset); + } + + /** + * Write a register + * + * @param offset Register offset in bytes from the base address. + * @param value Value to write. + */ + void reg(uint32_t offset, uint32_t value) { + *(volatile uint32_t *)(_base + offset) = value; + } + + /** + * Modify a register + * + * Note that there is a risk of a race during the read/modify/write cycle + * that must be taken care of by the caller. + * + * @param offset Register offset in bytes from the base address. + * @param clearbits Bits to clear in the register + * @param setbits Bits to set in the register + */ + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + uint32_t val = reg(offset); + val &= ~clearbits; + val |= setbits; + reg(offset, val); + } + +private: + uint32_t _base; +}; + +} // namespace device + +#endif /* _DEVICE_DEVICE_H */
\ No newline at end of file diff --git a/apps/drivers/device/device.h.orig b/apps/drivers/device/device.h.orig new file mode 100644 index 000000000..5030666e1 --- /dev/null +++ b/apps/drivers/device/device.h.orig @@ -0,0 +1,430 @@ +/**************************************************************************** + * + * 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 Definitions for the generic base classes in the device framework. + */ + +#ifndef _DEVICE_DEVICE_H +#define _DEVICE_DEVICE_H + +/* + * Includes here should only cover the needs of the framework definitions. + */ +#include <nuttx/config.h> + +#include <errno.h> +#include <stdbool.h> +#include <stddef.h> +#include <stdint.h> +#include <poll.h> + +#include <nuttx/fs/fs.h> + +/** + * Namespace encapsulating all device framework classes, functions and data. + */ +namespace device __EXPORT +{ + +/** + * Fundamental base class for all device drivers. + * + * This class handles the basic "being a driver" things, including + * interrupt registration and dispatch. + */ +class __EXPORT Device +{ +public: + /** + * Interrupt handler. + */ + virtual void interrupt(void *ctx); /**< interrupt handler */ + +protected: + const char *_name; /**< driver name */ + bool _debug_enabled; /**< if true, debug messages are printed */ + + /** + * Constructor + * + * @param name Driver name + * @param irq Interrupt assigned to the device. + */ + Device(const char *name, + int irq = 0); + ~Device(); + + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialised OK. + */ + virtual int init(); + + /** + * Enable the device interrupt + */ + void interrupt_enable(); + + /** + * Disable the device interrupt + */ + void interrupt_disable(); + + /** + * Take the driver lock. + * + * Each driver instance has its own lock/semaphore. + * + * Note that we must loop as the wait may be interrupted by a signal. + */ + void lock() { + do {} while (sem_wait(&_lock) != 0); + } + + /** + * Release the driver lock. + */ + void unlock() { + sem_post(&_lock); + } + + /** + * Log a message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void log(const char *fmt, ...); + + /** + * Print a debug message. + * + * The message is prefixed with the driver name, and followed + * by a newline. + */ + void debug(const char *fmt, ...); + +private: + int _irq; + bool _irq_attached; + sem_t _lock; + + /** disable copy construction for this and all subclasses */ + Device(const Device &); + + /** disable assignment for this and all subclasses */ + Device &operator = (const Device &); + + /** + * Register ourselves as a handler for an interrupt + * + * @param irq The interrupt to claim + * @return OK if the interrupt was registered + */ + int dev_register_interrupt(int irq); + + /** + * Unregister ourselves as a handler for any interrupt + */ + void dev_unregister_interrupt(); + + /** + * Interrupt dispatcher + * + * @param irq The interrupt that has been triggered. + * @param context Pointer to the interrupted context. + */ + static void dev_interrupt(int irq, void *context); +}; + +/** + * Abstract class for any character device + */ +class __EXPORT CDev : public Device +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param irq Interrupt assigned to the device + */ + CDev(const char *name, const char *devname, int irq = 0); + + /** + * Destructor + */ + ~CDev(); + + virtual int init(); + + /** + * Handle an open of the device. + * + * This function is called for every open of the device. The default + * implementation maintains _open_count and always returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open is allowed, -errno otherwise. + */ + virtual int open(struct file *filp); + + /** + * Handle a close of the device. + * + * This function is called for every close of the device. The default + * implementation maintains _open_count and returns OK as long as it is not zero. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the close was successful, -errno otherwise. + */ + virtual int close(struct file *filp); + + /** + * Perform a read from the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer into which data should be placed. + * @param buflen The number of bytes to be read. + * @return The number of bytes read or -errno otherwise. + */ + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + /** + * Perform a write to the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param buffer Pointer to the buffer from which data should be read. + * @param buflen The number of bytes to be written. + * @return The number of bytes written or -errno otherwise. + */ + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + + /** + * Perform a logical seek operation on the device. + * + * The default implementation returns -ENOSYS. + * + * @param filp Pointer to the NuttX file structure. + * @param offset The new file position relative to whence. + * @param whence SEEK_OFS, SEEK_CUR or SEEK_END. + * @return The previous offset, or -errno otherwise. + */ + virtual off_t seek(struct file *filp, off_t offset, int whence); + + /** + * Perform an ioctl operation on the device. + * + * The default implementation handles DIOC_GETPRIV, and otherwise + * returns -ENOTTY. Subclasses should call the default implementation + * for any command they do not handle themselves. + * + * @param filp Pointer to the NuttX file structure. + * @param cmd The ioctl command value. + * @param arg The ioctl argument value. + * @return OK on success, or -errno otherwise. + */ + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Perform a poll setup/teardown operation. + * + * This is handled internally and should not normally be overridden. + * + * @param filp Pointer to the NuttX file structure. + * @param fds Poll descriptor being waited on. + * @param arg True if this is establishing a request, false if + * it is being torn down. + * @return OK on success, or -errno otherwise. + */ + virtual int poll(struct file *filp, struct pollfd *fds, bool setup); + +protected: + /** + * Check the current state of the device for poll events from the + * perspective of the file. + * + * This function is called by the default poll() implementation when + * a poll is set up to determine whether the poll should return immediately. + * + * The default implementation returns no events. + * + * @param filp The file that's interested. + * @return The current set of poll events. + */ + virtual pollevent_t poll_state(struct file *filp); + + /** + * Report new poll events. + * + * This function should be called anytime the state of the device changes + * in a fashion that might be interesting to a poll waiter. + * + * @param events The new event(s) being announced. + */ + virtual void poll_notify(pollevent_t events); + + /** + * Internal implementation of poll_notify. + * + * @param fds A poll waiter to notify. + * @param events The event(s) to send to the waiter. + */ + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + + /** + * Notification of the first open. + * + * This function is called when the device open count transitions from zero + * to one. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open should proceed, -errno otherwise. + */ + virtual int open_first(struct file *filp); + + /** + * Notification of the last close. + * + * This function is called when the device open count transitions from + * one to zero. The driver lock is held for the duration of the call. + * + * The default implementation returns OK. + * + * @param filp Pointer to the NuttX file structure. + * @return OK if the open should return OK, -errno otherwise. + */ + virtual int close_last(struct file *filp); + +private: + static const unsigned _max_pollwaiters = 8; + + const char *_devname; /**< device node name */ + bool _registered; /**< true if device name was registered */ + unsigned _open_count; /**< number of successful opens */ + + struct pollfd *_pollset[_max_pollwaiters]; + + /** + * Store a pollwaiter in a slot where we can find it later. + * + * Expands the pollset as required. Must be called with the driver locked. + * + * @return OK, or -errno on error. + */ + int store_poll_waiter(struct pollfd *fds); + + /** + * Remove a poll waiter. + * + * @return OK, or -errno on error. + */ + int remove_poll_waiter(struct pollfd *fds); +}; + +/** + * Abstract class for character device accessed via PIO + */ +class __EXPORT PIO : public CDev +{ +public: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param base Base address of the device PIO area + * @param irq Interrupt assigned to the device (or zero if none) + */ + PIO(const char *name, + const char *devname, + uint32_t base, + int irq = 0); + ~PIO(); + + int init(); + +protected: + + /** + * Read a register + * + * @param offset Register offset in bytes from the base address. + */ + uint32_t reg(uint32_t offset) { + return *(volatile uint32_t *)(_base + offset); + } + + /** + * Write a register + * + * @param offset Register offset in bytes from the base address. + * @param value Value to write. + */ + void reg(uint32_t offset, uint32_t value) { + *(volatile uint32_t *)(_base + offset) = value; + } + + /** + * Modify a register + * + * Note that there is a risk of a race during the read/modify/write cycle + * that must be taken care of by the caller. + * + * @param offset Register offset in bytes from the base address. + * @param clearbits Bits to clear in the register + * @param setbits Bits to set in the register + */ + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + uint32_t val = reg(offset); + val &= ~clearbits; + val |= setbits; + reg(offset, val); + } + +private: + uint32_t _base; +}; + +} // namespace device + +#endif /* _DEVICE_DEVICE_H */
\ No newline at end of file diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp new file mode 100644 index 000000000..8489db4bb --- /dev/null +++ b/apps/drivers/device/i2c.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * 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 Base class for devices attached via the I2C bus. + * + * @todo Bus frequency changes; currently we do nothing with the value + * that is supplied. Should we just depend on the bus knowing? + */ + +#include "i2c.h" + +namespace device +{ + +I2C::I2C(const char *name, + const char *devname, + int bus, + uint16_t address, + uint32_t frequency, + int irq) : + // base class + CDev(name, devname, irq), + // public + // protected + // private + _bus(bus), + _address(address), + _frequency(frequency), + _dev(nullptr) +{ +} + +I2C::~I2C() +{ + if (_dev) + up_i2cuninitialize(_dev); +} + +int +I2C::init() +{ + int ret = OK; + + // attach to the i2c bus + _dev = up_i2cinitialize(_bus); + + if (_dev == nullptr) { + debug("failed to init I2C"); + ret = -ENOENT; + goto out; + } + + // call the probe function to check whether the device is present + ret = probe(); + + if (ret != OK) { + debug("probe failed"); + goto out; + } + + // do base class init, which will create device node, etc + ret = CDev::init(); + + if (ret != OK) { + debug("cdev init failed"); + goto out; + } + + // tell the world where we are + log("on bus %d at 0x%02x", _bus, _address); + +out: + return ret; +} + +int +I2C::probe() +{ + // Assume the device is too stupid to be discoverable. + return OK; +} + +int +I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + struct i2c_msg_s msgv[2]; + unsigned msgs; + int ret; + +// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + + msgs = 0; + + if (send_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = send; + msgv[msgs].length = send_len; + msgs++; + } + + if (recv_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } + + if (msgs == 0) + return -EINVAL; + + ret = I2C_TRANSFER(_dev, &msgv[0], msgs); + + return ret; +} + +} // namespace device
\ No newline at end of file diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h new file mode 100644 index 000000000..d84f7bd09 --- /dev/null +++ b/apps/drivers/device/i2c.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 Base class for devices connected via I2C. + */ + +#ifndef _DEVICE_I2C_H +#define _DEVICE_I2C_H + +#include "device.h" + +#include <nuttx/i2c.h> + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on I2C + */ +class __EXPORT I2C : public CDev +{ + +protected: + /** + * @ Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus I2C bus on which the device lives + * @param address I2C bus address, or zero if set_address will be used + * @param frequency I2C bus frequency for the device (currently not used) + * @param irq Interrupt assigned to the device (or zero if none) + */ + I2C(const char *name, + const char *devname, + int bus, + uint16_t address, + uint32_t frequency, + int irq = 0); + ~I2C(); + + virtual int init(); + + /** + * Check for the presence of the device on the bus. + */ + virtual int probe(); + + /** + * Perform an I2C transaction to the device. + * + * At least one of send_len and recv_len must be non-zero. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(uint8_t *send, unsigned send_len, + uint8_t *recv, unsigned recv_len); + + /** + * Change the bus address. + * + * Most often useful during probe() when the driver is testing + * several possible bus addresses. + * + * @param address The new bus address to set. + */ + void set_address(uint16_t address) { + _address = address; + } + +private: + int _bus; + uint16_t _address; + uint32_t _frequency; + struct i2c_dev_s *_dev; +}; + +} // namespace device + +#endif /* _DEVICE_I2C_H */ diff --git a/apps/drivers/device/pio.cpp b/apps/drivers/device/pio.cpp new file mode 100644 index 000000000..5179752b5 --- /dev/null +++ b/apps/drivers/device/pio.cpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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 Base class for devices accessed via PIO to registers. + */ + +#include "device.h" + +namespace device +{ + +PIO::PIO(const char *name, + const char *devname, + uint32_t base, + int irq) : + // base class + CDev(name, devname, irq), + // public + // protected + // private + _base(base) +{ +} + +PIO::~PIO() +{ + // nothing to do here... +} + +int +PIO::init() +{ + int ret = OK; + + // base class init first + ret = CDev::init(); + + return ret; +} + +} // namespace device
\ No newline at end of file diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp new file mode 100644 index 000000000..d0647bef6 --- /dev/null +++ b/apps/drivers/device/spi.cpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * 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 Base class for devices connected via SPI. + * + * @todo Work out if caching the mode/frequency would save any time. + */ + +#include "spi.h" + +#ifndef CONFIG_SPI_EXCHANGE +# error This driver requires CONFIG_SPI_EXCHANGE +#endif + +namespace device +{ + +SPI::SPI(const char *name, + const char *devname, + int bus, + enum spi_dev_e device, + enum spi_mode_e mode, + uint32_t frequency, + int irq) : + // base class + CDev(name, devname, irq), + // public + // protected + // private + _bus(bus), + _device(device), + _mode(mode), + _frequency(frequency), + _dev(nullptr) +{ +} + +SPI::~SPI() +{ + // XXX no way to let go of the bus... +} + +int +SPI::init() +{ + int ret = OK; + + // attach to the spi bus + if (_dev == nullptr) + _dev = up_spiinitialize(_bus); + + if (_dev == nullptr) { + debug("failed to init SPI"); + ret = -ENOENT; + goto out; + } + + // call the probe function to check whether the device is present + ret = probe(); + + if (ret != OK) { + debug("probe failed"); + goto out; + } + + // do base class init, which will create the device node, etc. + ret = CDev::init(); + + if (ret != OK) { + debug("cdev init failed"); + goto out; + } + + // tell the workd where we are + log("on bus %d at %d", _bus, _device); + +out: + return ret; +} + +int +SPI::probe() +{ + // assume the device is too stupid to be discoverable + return OK; +} + +int +SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + + if ((send == nullptr) && (recv == nullptr)) + return -EINVAL; + + /* do common setup */ + SPI_LOCK(_dev, true); + SPI_SETFREQUENCY(_dev, _frequency); + SPI_SETMODE(_dev, _mode); + SPI_SELECT(_dev, _device, true); + + /* do the transfer */ + SPI_EXCHANGE(_dev, send, recv, len); + + /* and clean up */ + SPI_SELECT(_dev, _device, false); + SPI_LOCK(_dev, false); + + return OK; +} + +} // namespace device
\ No newline at end of file diff --git a/apps/drivers/device/spi.h b/apps/drivers/device/spi.h new file mode 100644 index 000000000..ef382b03c --- /dev/null +++ b/apps/drivers/device/spi.h @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 Base class for devices connected via SPI. + */ + +#ifndef _DEVICE_SPI_H +#define _DEVICE_SPI_H + +#include "device.h" + +#include <nuttx/spi.h> + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on SPI + */ +class __EXPORT SPI : public CDev +{ +protected: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus SPI bus on which the device lives + * @param device Device handle (used by SPI_SELECT) + * @param mode SPI clock/data mode + * @param frequency SPI clock frequency + * @param irq Interrupt assigned to the device (or zero if none) + */ + SPI(const char *name, + const char *devname, + int bus, + enum spi_dev_e device, + enum spi_mode_e mode, + uint32_t frequency, + int irq = 0); + ~SPI(); + + virtual int init(); + + /** + * Check for the presence of the device on the bus. + */ + virtual int probe(); + + /** + * Perform a SPI transfer. + * + * At least one of send or recv must be non-null. + * + * @param send Bytes to send to the device, or nullptr if + * no data is to be sent. + * @param recv Buffer for receiving bytes from the device, + * or nullptr if no bytes are to be received. + * @param len Number of bytes to transfer. + * @return OK if the exchange was successful, -errno + * otherwise. + */ + int transfer(uint8_t *send, uint8_t *recv, unsigned len); + +private: + int _bus; + enum spi_dev_e _device; + enum spi_mode_e _mode; + uint32_t _frequency; + struct spi_dev_s *_dev; +}; + +} // namespace device + +#endif /* _DEVICE_SPI_H */ diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h new file mode 100644 index 000000000..bf13b2c32 --- /dev/null +++ b/apps/drivers/drv_accel.h @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 Accelerometer driver interface. + */ + +#ifndef _DRV_ACCEL_H +#define _DRV_ACCEL_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_orb_dev.h" + +#define ACCEL_DEVICE_PATH "/dev/accel" + +/** + * accel report structure. Reads from the device must be in multiples of this + * structure. + */ +struct accel_report { + float x; + float y; + float z; + uint64_t timestamp; +}; + +/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ +struct accel_scale { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; +}; + +/* + * ObjDev tag for raw accelerometer data. + */ +ORB_DECLARE(sensor_accel); + +/* + * ioctl() definitions + */ + +#define _ACCELIOCBASE (_SNIOCBASE + 0x20) +#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) + +/** set the driver polling rate to (arg) Hz, or one of the ACC_POLLRATE constants */ +#define ACCELIOCSPOLLRATE _ACCELIOC(0) + +#define ACC_POLLRATE_MANUAL 1000000 /**< poll when read */ +#define ACC_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ + +/** set the internal queue depth to (arg) entries, must be at least 1 */ +#define ACCELIOCSQUEUEDEPTH _ACCELIOC(1) + +/** set the accel internal sample rate to at least (arg) Hz */ +#define ACCELIOCSSAMPLERATE _ACCELIOC(2) + +/** set the accel internal lowpass filter to no lower than (arg) Hz */ +#define ACCELIOCSLOWPASS _ACCELIOC(3) + +/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */ +#define ACCELIOCSREPORTFORMAT _ACCELIOC(4) + +/** set the accel scaling constants to the structure pointed to by (arg) */ +#define ACCELIOCSSCALE _ACCELIOC(5) + +/** set the accel measurement range to handle at least (arg) g */ +#define ACCELIORANGE _ACCELIOC(6) + +#endif /* _DRV_ACCEL_H */ diff --git a/apps/drivers/drv_baro.h b/apps/drivers/drv_baro.h new file mode 100644 index 000000000..4cfb35454 --- /dev/null +++ b/apps/drivers/drv_baro.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 Barometric pressure sensor driver interface. + */ + +#ifndef _DRV_BARO_H +#define _DRV_BARO_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_orb_dev.h" + +#define BARO_DEVICE_PATH "/dev/baro" + +/** + * baro report structure. Reads from the device must be in multiples of this + * structure. + */ +struct baro_report { + float pressure; + float altitude; + float temperature; + uint64_t timestamp; +}; + +/* + * ObjDev tag for raw barometer data. + */ +ORB_DECLARE(sensor_baro); + +/* + * ioctl() definitions + */ + +#define _BAROIOCBASE (_SNIOCBASE + 0x10) +#define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n)) + +/** set the driver polling rate to (arg) Hz, or one of the BARO_POLLRATE constants */ +#define BAROIOCSPOLLRATE _BAROIOC(0) + +#define BARO_POLLRATE_MANUAL 1000000 /**< poll when read */ +#define BARO_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ + +/** set the internal queue depth to (arg) entries, must be at least 1 */ +#define BAROIOCSQUEUEDEPTH _BAROIOC(1) + +/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */ +#define BAROIOCSREPORTFORMAT _BAROIOC(2) + +#endif /* _DRV_BARO_H */ diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h new file mode 100644 index 000000000..b9684bce1 --- /dev/null +++ b/apps/drivers/drv_gpio.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 Generic GPIO ioctl interface. + */ + +#ifndef _DRV_GPIO_H +#define _DRV_GPIO_H + +#include <sys/ioctl.h> + +/* + * GPIO defines come from a board-specific header, as they are shared + * with board-specific logic. + * + * The board-specific header must define: + * GPIO_DEVICE_PATH + * GPIO_RESET + * GPIO_SET_OUTPUT + * GPIO_SET_INPUT + * GPIO_SET_ALT_1 + * GPIO_SET_ALT_2 + * GPIO_SET_ALT_3 + * GPIO_SET_ALT_4 + * GPIO_SET + * GPIO_CLEAR + * GPIO_GET + */ + +/* Include board-specific GPIO definitions as well. */ +#include <arch/board/drv_gpio.h> + +#endif /* _DRV_GPIO_H */
\ No newline at end of file diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h new file mode 100644 index 000000000..21f6493b1 --- /dev/null +++ b/apps/drivers/drv_gyro.h @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 Gyroscope driver interface. + */ + +#ifndef _DRV_GYRO_H +#define _DRV_GYRO_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_orb_dev.h" + +#define GYRO_DEVICE_PATH "/dev/gyro" + +/** + * gyro report structure. Reads from the device must be in multiples of this + * structure. + */ +struct gyro_report { + float x; + float y; + float z; + uint64_t timestamp; +}; + +/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */ +struct gyro_scale { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; +}; + +/* + * ObjDev tag for raw gyro data. + */ +ORB_DECLARE(sensor_gyro); + +/* + * ioctl() definitions + */ + +#define _GYROIOCBASE (_SNIOCBASE + 0x10) +#define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n)) + +/** set the driver polling rate to (arg) Hz, or one of the GYRO_POLLRATE constants */ +#define GYROIOCSPOLLRATE _GYROIOC(0) + +#define GYRO_POLLRATE_MANUAL 1000000 /**< poll when read */ +#define GYRO_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ + +/** set the internal queue depth to (arg) entries, must be at least 1 */ +#define GYROIOCSQUEUEDEPTH _GYROIOC(1) + +/** set the gyro internal sample rate to at least (arg) Hz */ +#define GYROIOCSSAMPLERATE _GYROIOC(2) + +/** set the gyro internal lowpass filter to no lower than (arg) Hz */ +#define GYROIOCSLOWPASS _GYROIOC(3) + +/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */ +#define GYROIOCSREPORTFORMAT _GYROIOC(4) + +/** set the gyro scaling constants to (arg) */ +#define GYROIOCSSCALE _GYROIOC(5) + +#endif /* _DRV_GYRO_H */ diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h new file mode 100644 index 000000000..7e90e9e46 --- /dev/null +++ b/apps/drivers/drv_mag.h @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 Magnetometer driver interface. + */ + +#ifndef _DRV_MAG_H +#define _DRV_MAG_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_orb_dev.h" + +#define MAG_DEVICE_PATH "/dev/mag" + +/** + * mag report structure. Reads from the device must be in multiples of this + * structure. + */ +struct mag_report { + float x; + float y; + float z; + uint64_t timestamp; +}; + +/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ +struct mag_scale { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; +}; + +/* + * ObjDev tag for raw magnetometer data. + */ +ORB_DECLARE(sensor_mag); + +/* + * ioctl() definitions + */ + +#define _MAGIOCBASE (_SNIOCBASE + 0x30) +#define _MAGIOC(_n) (_IOC(_MAGIOBASE, _n)) + +/** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */ +#define MAGIOCSPOLLRATE _MAGIOC(0) + +#define MAG_POLLRATE_MANUAL 1000000 /**< poll when read */ +#define MAG_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */ + +/** set the internal queue depth to (arg) entries, must be at least 1 */ +#define MAGIOCSQUEUEDEPTH _MAGIOC(1) + +/** set the mag internal sample rate to at least (arg) Hz */ +#define MAGIOCSSAMPLERATE _MAGIOC(2) + +/** set the mag internal lowpass filter to no lower than (arg) Hz */ +#define MAGIOCSLOWPASS _MAGIOC(3) + +/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */ +#define MAGIOCSREPORTFORMAT _MAGIOC(4) + +/** set the mag scaling constants to the structure pointed to by (arg) */ +#define MAGIOCSSCALE _MAGIOC(5) + +#endif /* _DRV_MAG_H */ diff --git a/apps/drivers/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h new file mode 100644 index 000000000..bacef1cd3 --- /dev/null +++ b/apps/drivers/drv_orb_dev.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef _DRV_UORB_H +#define _DRV_UORB_H + +/** + * @file uORB published object driver. + */ + +#include <sys/types.h> +#include <sys/ioctl.h> +#include <stdint.h> + +/* XXX for ORB_DECLARE used in many drivers */ +#include "../uORB/uORB.h" + +/* + * ioctl() definitions + */ + +/** path to the uORB control device for pub/sub topics */ +#define TOPIC_MASTER_DEVICE_PATH "/obj/_obj_" + +/** path to the uORB control device for parameter topics */ +#define PARAM_MASTER_DEVICE_PATH "/param/_param_" + +/** maximum ogbject name length */ +#define ORB_MAXNAME 32 + +#define _ORBIOCBASE (_DIOCBASE + 0x80) +#define _ORBIOC(_n) (_IOC(_ORBIOCBASE, _n)) + +/* + * IOCTLs for the uORB control device + */ + +/** Advertise a new topic described by *(uorb_metadata *)arg */ +#define ORBIOCADVERTISE _ORBIOC(0) + +/* + * IOCTLs for individual topics. + */ + +/** Fetch the time at which the topic was last updated into *(uint64_t *)arg */ +#define ORBIOCLASTUPDATE _ORBIOC(10) + +/** Check whether the topic has been updated since it was last read, sets *(bool *)arg */ +#define ORBIOCUPDATED _ORBIOC(11) + +/** Set the minimum interval at which the topic can be seen to be updated for this subscription */ +#define ORBIOCSETINTERVAL _ORBIOC(12) + +#endif /* _DRV_UORB_H */ diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h new file mode 100644 index 000000000..551f9b1a6 --- /dev/null +++ b/apps/drivers/drv_pwm_output.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 PWM servo output interface. + * + * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a + * pwm_output_values structure to the device, or by publishing to the + * output_pwm ObjDev. + * Writing a value of 0 to a channel suppresses any output for that + * channel. + */ + +#ifndef _DRV_PWM_OUTPUT_H +#define _DRV_PWM_OUTPUT_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_orb_dev.h" + +/** + * Path for the default PWM output device. + * + * Note that on systems with more than one PWM output path (e.g. + * PX4FMU with PX4IO connected) there may be other devices that + * respond to this protocol. + */ +#define PWM_OUTPUT_DEVICE_PATH "/dev/pwm_output" + +/** + * Maximum number of PWM output channels in the system. + */ +#define PWM_OUTPUT_MAX_CHANNELS 16 + +/** + * Servo output signal type, value is actual servo output pulse + * width in microseconds. + */ +typedef uint16_t servo_position_t; + +/** + * Servo output status structure. + * + * May be published to output_pwm, or written to a PWM output + * device. + */ +struct pwm_output_values { + /** desired servo update rate in Hz */ + uint32_t update_rate; + + /** desired pulse widths for each of the supported channels */ + servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; +}; + +/* + * ObjDev tag for PWM outputs. + */ +ORB_DECLARE(output_pwm); + +/* + * ioctl() definitions + * + * Note that ioctls and ObjDev updates should not be mixed, as the + * behaviour of the system in this case is not defined. + */ +#define _PWM_SERVO_BASE 0x7500 + +/** arm all servo outputs handle by this driver */ +#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) + +/** disarm all servo outputs (stop generating pulses) */ +#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) + +/** set a single servo to a specific value */ +#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) + +/** get a single specific servo value */ +#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) + + +#endif /* _DRV_PWM_OUTPUT_H */ diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h new file mode 100644 index 000000000..532e95fb5 --- /dev/null +++ b/apps/drivers/drv_rc_input.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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 R/C input interface. + * + */ + +#ifndef _DRV_RC_INPUT_H +#define _DRV_RC_INPUT_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_orb_dev.h" + +/** + * Path for the default R/C input device. + * + * Note that on systems with more than one R/C input path (e.g. + * PX4FMU with PX4IO connected) there may be other devices that + * respond to this protocol. + * + * Input data may be obtained by subscribing to the input_rc + * object, or by poll/reading from the device. + */ +#define RC_INPUT_DEVICE_PATH "/dev/input_rc" + +/** + * Maximum number of R/C input channels in the system. + */ +#define RC_INPUT_MAX_CHANNELS 16 + +/** + * Input signal type, value is a control position from zero to 100 + * percent. + */ +typedef uint8_t rc_input_t; + +/** + * R/C input status structure. + * + * Published to input_rc, may also be published to other names depending + * on the board involved. + */ +struct rc_input_values { + /** number of channels actually being seen */ + uint32_t channel_count; + + /** desired pulse widths for each of the supported channels */ + rc_input_t values[RC_INPUT_MAX_CHANNELS]; +}; + +/* + * ObjDev tag for R/C inputs. + */ +ORB_DECLARE(input_rc); + +#endif /* _DRV_RC_INPUT_H */ diff --git a/apps/drivers/ms5611/Makefile b/apps/drivers/ms5611/Makefile new file mode 100644 index 000000000..d8e67cba2 --- /dev/null +++ b/apps/drivers/ms5611/Makefile @@ -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. +# +############################################################################ + +# +# MS5611 driver +# + +APPNAME = ms5611 +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp new file mode 100644 index 000000000..d3c75f755 --- /dev/null +++ b/apps/drivers/ms5611/ms5611.cpp @@ -0,0 +1,944 @@ +/**************************************************************************** + * + * 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 Driver for the MS5611 barometric pressure sensor connected via I2C + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/up_hrt.h> + +#include <drivers/drv_baro.h> + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct ms5611_prom_s { + uint16_t factory_setup; + uint16_t c1_pressure_sens; + uint16_t c2_pressure_offset; + uint16_t c3_temp_coeff_pres_sens; + uint16_t c4_temp_coeff_pres_offset; + uint16_t c5_reference_temp; + uint16_t c6_temp_coeff_temp; + uint16_t serial_and_crc; +}; + +/** + * Grody hack for crc4() + */ +union ms5611_prom_u { + uint16_t c[8]; + struct ms5611_prom_s s; +}; +#pragma pack(pop) + +class MS5611 : public device::I2C +{ +public: + MS5611(int bus); + ~MS5611(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int open_first(struct file *filp); + virtual int close_last(struct file *filp); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + union ms5611_prom_u _prom; + + struct work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct baro_report *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + int32_t _dT; + int64_t _temp64; + + int _orbject; + + unsigned _reads; + unsigned _measure_errors; + unsigned _read_errors; + unsigned _buf_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + int measure(); + + /** + * Collect the result of the most recent measurement. + */ + int collect(); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int read_prom(); + + /** + * PROM CRC routine ported from MS5611 application note + * + * @param n_prom Pointer to words read from PROM. + * @return True if the CRC matches. + */ + bool crc4(uint16_t *n_prom); + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * MS5611 internal constants and data structures. + */ + +/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ +#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ + +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ +#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ + +#define ADDR_RESET_CMD 0x1E /* read from this address to reset chip (0b0011110 on bus) */ +#define ADDR_CMD_CONVERT_D1 0x48 /* 4096 samples to this address to start conversion (0b01001000 on bus) */ +#define ADDR_CMD_CONVERT_D2 0x58 /* 4096 samples */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); + + +MS5611::MS5611(int bus) : + I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), + _measure_ticks(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _dT(0), + _temp64(0), + _reads(0), + _measure_errors(0), + _read_errors(0), + _buf_overflows(0) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + _work.worker = nullptr; +} + +MS5611::~MS5611() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +MS5611::init() +{ + int ret; + + /* do I2C init (and probe) first */ + ret = I2C::init(); + + /* assuming we're good, advertise the object */ + if (ret == OK) { + struct baro_report b; + + /* if this fails (e.g. no object in the system) that's OK */ + memset(&b, 0, sizeof(b)); + _orbject = orb_advertise(ORB_ID(sensor_baro), &b); + + if (_orbject < 0) + debug("failed to create sensor_baro object"); + } + + return ret; +} + +int +MS5611::open_first(struct file *filp) +{ + /* reset to manual-poll mode */ + _measure_ticks = 0; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct baro_report[_num_reports]; + _oldest_report = _next_report = 0; + + return OK; +} + +int +MS5611::close_last(struct file *filp) +{ + /* stop measurement */ + stop(); + + /* free report buffers */ + if (_reports != nullptr) { + delete[] _reports; + _num_reports = 0; + } + + _measure_ticks = 0; + + return OK; +} + +int +MS5611::probe() +{ + if (OK == probe_address(MS5611_ADDRESS_1)) + return OK; + + if (OK == probe_address(MS5611_ADDRESS_2)) + return OK; + + return -EIO; +} + +int +MS5611::probe_address(uint8_t address) +{ + uint8_t cmd = ADDR_RESET_CMD; + + /* select the address we are going to try */ + set_address(address); + + /* send reset command */ + if (OK != transfer(&cmd, 1, nullptr, 0)) + return -EIO; + + /* wait for PROM contents to be in the device (2.8 ms) */ + usleep(3000); + + /* read PROM */ + if (OK != read_prom()) + return -EIO; + + return OK; +} + +ssize_t +MS5611::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct baro_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + _reads++; + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + _reads++; + + } while (0); + + return ret; +} + +int +MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case BAROIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case BARO_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case BARO_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case BAROIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct baro_report *buf = new struct baro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case BAROIOCSREPORTFORMAT: + return -EINVAL; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +void +MS5611::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(&_work, (worker_t)&MS5611::cycle_trampoline, this, 1); +} + +void +MS5611::stop() +{ + work_cancel(&_work); +} + +void +MS5611::cycle_trampoline(void *arg) +{ + MS5611 *dev = (MS5611 *)arg; + + dev->cycle(); +} + +void +MS5611::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("FATAL collection error - restarting\n"); + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(&_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("FATAL measure error - restarting\n"); + start(); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(&_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +int +MS5611::measure() +{ + int ret; + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + */ + ret = transfer(&cmd_data, 1, nullptr, 0); + + if (OK != ret) + _measure_errors++; + + return ret; +} + +int +MS5611::collect() +{ + uint8_t cmd; + uint8_t data[3]; + + /* read the most recent measurement */ + cmd = 0; + + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + + if (OK != transfer(&cmd, 1, &data[0], 3)) { + _read_errors++; + return -EIO; + } + + /* fetch the raw value */ + uint32_t raw = (((uint32_t)data[0]) << 16) | (((uint32_t)data[1]) << 8) | ((uint32_t)data[2]); + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature calculation */ + _dT = raw - (((int32_t)_prom.s.c5_reference_temp) * 256); + _temp64 = 2000 + (((int64_t)_dT) * _prom.s.c6_temp_coeff_temp) / 8388608; + + } else { + + /* pressure calculation */ + int64_t offset = (int64_t)_prom.s.c2_pressure_offset * 65536 + ((int64_t)_dT * _prom.s.c4_temp_coeff_pres_offset) / 128; + int64_t sens = (int64_t)_prom.s.c1_pressure_sens * 32768 + ((int64_t)_dT * _prom.s.c3_temp_coeff_pres_sens) / 256; + + /* it's pretty cold, second order temperature compensation needed */ + if (_temp64 < 2000) { + /* second order temperature compensation */ + int64_t temp2 = (((int64_t)_dT) * _dT) >> 31; + int64_t tmp_64 = (_temp64 - 2000) * (_temp64 - 2000); + int64_t offset2 = (5 * tmp_64) >> 1; + int64_t sens2 = (5 * tmp_64) >> 2; + _temp64 = _temp64 - temp2; + offset = offset - offset2; + sens = sens - sens2; + } + + int64_t press_int64 = (((raw * sens) / 2097152 - offset) / 32768); + + /* generate a new report */ + _reports[_next_report].temperature = _temp64 / 100.0f; + _reports[_next_report].pressure = press_int64 / 100.0f; + /* convert as double for max. precision, store as float (more than enough precision) */ + _reports[_next_report].altitude = (44330.0 * (1.0 - pow((press_int64 / 101325.0), 0.190295))); + + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _orbject, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + _buf_overflows++; + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); + + return OK; +} + +int +MS5611::read_prom() +{ + /* read PROM data */ + uint8_t prom_buf[2] = {255, 255}; + + for (int i = 0; i < 8; i++) { + uint8_t cmd = ADDR_PROM_SETUP + (i * 2); + + if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + break; + + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + _prom.c[i] = (((uint16_t)prom_buf[0]) << 8) | ((uint16_t)prom_buf[1]); + + } + + /* calculate CRC and return false */ + return crc4(&_prom.c[0]) ? OK : -EIO; +} + +bool +MS5611::crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + +void +MS5611::print_info() +{ + printf("reads: %u\n", _reads); + printf("measure errors: %u\n", _measure_errors); + printf("read errors: %u\n", _read_errors); + printf("read overflows: %u\n", _buf_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); + printf("dT/temp64: %d/%lld\n", _dT, _temp64); +} + +/** + * Local functions in support of the shell command. + */ +namespace +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MS5611 *g_dev; + +/* + * XXX this should just be part of the generic sensors test... + */ + + +int +test_fail(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "FAIL: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return ERROR; +} + +int +test_note(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "note: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return OK; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + * + * @param fd An open file descriptor on the driver. + */ +int +test(int fd) +{ + struct baro_report report; + ssize_t sz; + int ret; + + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + return test_fail("immediate read failed: %d", errno); + + test_note("single read"); + test_note("pressure: %u", (unsigned)report.pressure); + test_note("altitude: %u", (unsigned)report.altitude); + test_note("temperature: %u", (unsigned)report.temperature); + test_note("time: %lld", report.timestamp); + usleep(1000000); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, BAROIOCSQUEUEDEPTH, 10)) + return test_fail("failed to set queue depth"); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, BAROIOCSPOLLRATE, 2)) + return test_fail("failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + return test_fail("timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + return test_fail("periodic read failed: %d", errno); + + test_note("periodic read %u", i); + test_note("pressure: %u", (unsigned)report.pressure); + test_note("altitude: %u", (unsigned)report.altitude); + test_note("temperature: %u", (unsigned)report.temperature); + test_note("time: %lld", report.timestamp); + } + + return test_note("PASS"); + return OK; +} + +int +info() +{ + if (g_dev == nullptr) { + fprintf(stderr, "MS5611: driver not running\n"); + return -ENOENT; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return OK; +} + + +} // namespace + +int +ms5611_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + fprintf(stderr, "MS5611: already loaded\n"); + return -EBUSY; + } + + /* create the driver */ + /* XXX HORRIBLE hack - the bus number should not come from here */ + g_dev = new MS5611(2); + + if (g_dev == nullptr) { + fprintf(stderr, "MS5611: driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "MS5611: driver init failed\n"); + usleep(100000); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + return OK; + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + int fd, ret; + + fd = open(BARO_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + return test_fail("driver open failed: %d", errno); + + ret = test(fd); + close(fd); + return ret; + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + return info(); + + fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n"); + return -EINVAL; +} diff --git a/apps/examples/Kconfig b/apps/examples/Kconfig new file mode 100644 index 000000000..a20f7c2e7 --- /dev/null +++ b/apps/examples/Kconfig @@ -0,0 +1,200 @@ +# +# For a description of the syntax of this configuration file, +# 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/dhcpd/Kconfig" +endmenu + +menu "FTP client example" +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/hidkbd/Kconfig" +endmenu + +menu "IGMP example" +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/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 "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/wlan/Kconfig" +endmenu diff --git a/apps/examples/Make.defs b/apps/examples/Make.defs new file mode 100644 index 000000000..aa8d83733 --- /dev/null +++ b/apps/examples/Make.defs @@ -0,0 +1,223 @@ +############################################################################ +# apps/examples/Make.defs +# Adds selected applications to apps/ build +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +ifeq ($(CONFIG_EXAMPLES_ADC),y) +CONFIGURED_APPS += examples/adc +endif + +ifeq ($(CONFIG_EXAMPLES_BUTTONS),y) +CONFIGURED_APPS += examples/buttons +endif + +ifeq ($(CONFIG_EXAMPLES_CAN),y) +CONFIGURED_APPS += examples/can +endif + +ifeq ($(CONFIG_EXAMPLES_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +endif + +ifeq ($(CONFIG_EXAMPLES_COMPOSITE),y) +CONFIGURED_APPS += examples/composite +endif + +ifeq ($(CONFIG_EXAMPLES_DHCPD),y) +CONFIGURED_APPS += examples/dhcpd +endif + +ifeq ($(CONFIG_EXAMPLES_FTPC),y) +CONFIGURED_APPS += examples/ftpc +endif + +ifeq ($(CONFIG_EXAMPLES_FTPD),y) +CONFIGURED_APPS += examples/ftpd +endif + +ifeq ($(CONFIG_EXAMPLES_HELLO),y) +CONFIGURED_APPS += examples/hello +endif + +ifeq ($(CONFIG_EXAMPLES_HELLOXX),y) +CONFIGURED_APPS += examples/helloxx +endif + +ifeq ($(CONFIG_EXAMPLES_HIDKBD),y) +CONFIGURED_APPS += examples/hidkbd +endif + +ifeq ($(CONFIG_EXAMPLES_IGMP),y) +CONFIGURED_APPS += examples/igmp +endif + +ifeq ($(CONFIG_EXAMPLES_LCDRW),y) +CONFIGURED_APPS += examples/lcdrw +endif + +ifeq ($(CONFIG_EXAMPLES_MM),y) +CONFIGURED_APPS += examples/mm +endif + +ifeq ($(CONFIG_EXAMPLES_MOUNT),y) +CONFIGURED_APPS += examples/mount +endif + +ifeq ($(CONFIG_EXAMPLES_NETTEST),y) +CONFIGURED_APPS += examples/nettest +endif + +ifeq ($(CONFIG_EXAMPLES_NSH),y) +CONFIGURED_APPS += examples/nsh +endif + +ifeq ($(CONFIG_EXAMPLES_NULL),y) +CONFIGURED_APPS += examples/null +endif + +ifeq ($(CONFIG_EXAMPLES_NX),y) +CONFIGURED_APPS += examples/nx +endif + +ifeq ($(CONFIG_EXAMPLES_NXCONSOLE),y) +CONFIGURED_APPS += examples/nxconsole +endif + +ifeq ($(CONFIG_EXAMPLES_NXFFS),y) +CONFIGURED_APPS += examples/nxffs +endif + +ifeq ($(CONFIG_EXAMPLES_NXFLAT),y) +CONFIGURED_APPS += examples/nxflat +endif + +ifeq ($(CONFIG_EXAMPLES_NXHELLO),y) +CONFIGURED_APPS += examples/nxhello +endif + +ifeq ($(CONFIG_EXAMPLES_NXIMAGE),y) +CONFIGURED_APPS += examples/nximage +endif + +ifeq ($(CONFIG_EXAMPLES_NXLINES),y) +CONFIGURED_APPS += examples/nxlines +endif + +ifeq ($(CONFIG_EXAMPLES_NXTEXT),y) +CONFIGURED_APPS += examples/nxtext +endif + +ifeq ($(CONFIG_EXAMPLES_OSTEST),y) +CONFIGURED_APPS += examples/ostest +endif + +ifeq ($(CONFIG_EXAMPLES_PASHELLO),y) +CONFIGURED_APPS += examples/pashello +endif + +ifeq ($(CONFIG_EXAMPLES_PIPE),y) +CONFIGURED_APPS += examples/pipe +endif + +ifeq ($(CONFIG_EXAMPLES_POLL),y) +CONFIGURED_APPS += examples/poll +endif + +ifeq ($(CONFIG_EXAMPLES_PWM),y) +CONFIGURED_APPS += examples/pwm +endif + +ifeq ($(CONFIG_EXAMPLES_QENCODER),y) +CONFIGURED_APPS += examples/qencoder +endif + +ifeq ($(CONFIG_EXAMPLES_RGMP),y) +CONFIGURED_APPS += examples/rgmp +endif + +ifeq ($(CONFIG_EXAMPLES_ROMFS),y) +CONFIGURED_APPS += examples/romfs +endif + +ifeq ($(CONFIG_EXAMPLES_SENDMAIL),y) +CONFIGURED_APPS += examples/sendmail +endif + +ifeq ($(CONFIG_EXAMPLES_SERLOOP),y) +CONFIGURED_APPS += examples/serloop +endif + +ifeq ($(CONFIG_EXAMPLES_TELNETD),y) +CONFIGURED_APPS += examples/telnetd +endif + +ifeq ($(CONFIG_EXAMPLES_THTTPD),y) +CONFIGURED_APPS += examples/thttpd +endif + +ifeq ($(CONFIG_EXAMPLES_TIFF),y) +CONFIGURED_APPS += examples/tiff +endif + +ifeq ($(CONFIG_EXAMPLES_TOUCHSCREEN),y) +CONFIGURED_APPS += examples/touchscreen +endif + +ifeq ($(CONFIG_EXAMPLES_UDP),y) +CONFIGURED_APPS += examples/udp +endif + +ifeq ($(CONFIG_EXAMPLES_UIP),y) +CONFIGURED_APPS += examples/uip +endif + +ifeq ($(CONFIG_EXAMPLES_USBSERIAL),y) +CONFIGURED_APPS += examples/usbserial +endif + +ifeq ($(CONFIG_EXAMPLES_USBMSC),y) +CONFIGURED_APPS += examples/usbmsc +endif + +ifeq ($(CONFIG_EXAMPLES_USBTERM),y) +CONFIGURED_APPS += examples/usbterm +endif + +ifeq ($(CONFIG_EXAMPLES_WGET),y) +CONFIGURED_APPS += examples/wget +endif + +ifeq ($(CONFIG_EXAMPLES_WLAN),y) +CONFIGURED_APPS += examples/wlan +endif diff --git a/apps/examples/Makefile b/apps/examples/Makefile new file mode 100644 index 000000000..ad5be6497 --- /dev/null +++ b/apps/examples/Makefile @@ -0,0 +1,129 @@ +############################################################################ +# apps/examples/Makefile +# +# 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 +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config # Current configuration + +# Sub-directories + +SUBDIRS = adc buttons can cdcacm composite dhcpd ftpc ftpd hello helloxx \ + hidkbd igmp lcdrw mm modbus mount nettest nsh null nx nxconsole nxffs \ + nxflat nxhello nximage nxlines nxtext ostest pashello pipe poll pwm \ + qencoder rgmp romfs serloop telnetd thttpd tiff touchscreen udp uip \ + usbserial sendmail usbstorage usbterm watchdog wget wlan + +# Sub-directories that might need context setup. Directories may need +# context setup for a variety of reasons, but the most common is because +# the example may be built as an NSH built-in function. +# +# Directories that may be built as NSH built-in functions may have their +# own configuration setting (like CONFIG_EXAMPLES_HELLOXX_BUILTIN), but +# many only depend on the generic CONFIG_NSH_BUILTIN_APPS setting. And +# there a few which an ONLY be built as NSH built-in applications; these +# are included in the list unconditionally. + +CNTXTDIRS = pwm + +ifeq ($(CONFIG_NSH_BUILTIN_APPS),y) +CNTXTDIRS += adc can cdcacm composite ftpd dhcpd modbus nettest qencoder telnetd watchdog +endif + +ifeq ($(CONFIG_EXAMPLES_HELLO_BUILTIN),y) +CNTXTDIRS += hello +endif +ifeq ($(CONFIG_EXAMPLES_HELLOXX_BUILTIN),y) +CNTXTDIRS += helloxx +endif +ifeq ($(CONFIG_EXAMPLES_LCDRW_BUILTIN),y) +CNTXTDIRS += lcdrw +endif +ifeq ($(CONFIG_EXAMPLES_NX_BUILTIN),y) +CNTXTDIRS += nx +endif +ifeq ($(CONFIG_EXAMPLES_NXHELLO_BUILTIN),y) +CNTXTDIRS += nxhello +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 +ifeq ($(CONFIG_EXAMPLES_OSTEST_BUILTIN),y) +CNTXTDIRS += ostest +endif +ifeq ($(CONFIG_EXAMPLES_TIFF_BUILTIN),y) +CNTXTDIRS += tiff +endif +ifeq ($(CONFIG_EXAMPLES_TOUCHSCREEN_BUILTIN),y) +CNTXTDIRS += touchscreen +endif +ifeq ($(CONFIG_EXAMPLES_USBMSC_BUILTIN),y) +CNTXTDIRS += usbstorage +endif +ifeq ($(CONFIG_EXAMPLES_USBTERM_BUILTIN),y) +CNTXTDIRS += usbterm +endif + +all: nothing + +.PHONY: nothing context depend clean distclean + +nothing: + +context: + @for dir in $(CNTXTDIRS) ; do \ + $(MAKE) -C $$dir context TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + +depend: + @for dir in $(SUBDIRS) ; do \ + $(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + +clean: + @for dir in $(SUBDIRS) ; do \ + $(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + +distclean: clean + @for dir in $(SUBDIRS) ; do \ + $(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + +-include Make.dep + diff --git a/apps/examples/README.txt b/apps/examples/README.txt new file mode 100644 index 000000000..7d068f979 --- /dev/null +++ b/apps/examples/README.txt @@ -0,0 +1,1649 @@ +examples +^^^^^^^^ + + appconfig and CONFIG_APPS + + The examples directory contains several sample applications that + can be linked with NuttX. The specific example is selected in the + configs/<board-name>/appconfig file via the CONFIGURED_APPS setting. + This setting provides the path to the directory containing the + application Makefile (this path is a relative to the apps/ top- + level directory). For example, + + CONFIGURE_APPS += examples/ostest + + Selects the examples/ostest example. + + Built-In functions + + Some of the examples may be built as "built-in" functions that + can be executed at run time (rather than as NuttX "main" programs). + These "built-in" examples can be also be executed from the NuttShell + (NSH) command line. In order to configure these built-in NSH + functions, you have to set up the following: + + - CONFIG_NSH_BUILTIN_APPS - Enable support for external registered, + "named" applications that can be executed from the NSH + command line (see apps/README.txt for more information). + - CONFIG_EXAMPLES_XYZ_BUILTIN -- Build the XYZ example as a "built-in" + that can be executed from the NSH command line (where XYZ is + the specific example. See the following for examples that + support this option). + +examples/adc +^^^^^^^^^^^^ + + A mindlessly simple test of an ADC devices. It simply reads from the + ADC device and dumps the data to the console forever. + + This test depends on these specific ADC/NSH configurations settings (your + specific ADC settings might require additional settings). + + CONFIG_ADC - Enabled ADC support + CONFIG_NSH_BUILTIN_APPS - Build the ADC test as an NSH built-in function. + Default: Built as a standalone problem + + Specific configuration options for this example include: + + CONFIG_EXAMPLES_ADC_DEVPATH - The default path to the ADC device. Default: /dev/adc0 + CONFIG_EXAMPLES_ADC_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_ADC_GROUPSIZE - The number of samples to read at once. + Default: 4 + +examples/buttons +^^^^^^^^^^^^^^^^ + + This is a simple configuration that may be used to test the board- + specific button interfaces. Configuration options: + + CONFIG_ARCH_BUTTONS - Must be defined for button support + CONFIG_EXAMPLE_BUTTONS_MIN - Lowest button number (MIN=0) + CONFIG_EXAMPLE_BUTTONS_MAX - Highest button number (MAX=7) + + CONFIG_ARCH_IRQBUTTONS - Must be defined for interrupting button support + CONFIG_EXAMPLE_IRQBUTTONS_MIN - Lowest interrupting button number (MIN=0) + CONFIG_EXAMPLE_IRQBUTTONS_MAX - Highest interrupting button number (MAX=7) + + Name strings for buttons: + + CONFIG_EXAMPLE_BUTTONS_NAME0, CONFIG_EXAMPLE_BUTTONS_NAME1, + CONFIG_EXAMPLE_BUTTONS_NAME2, CONFIG_EXAMPLE_BUTTONS_NAME3, + CONFIG_EXAMPLE_BUTTONS_NAME4, CONFIG_EXAMPLE_BUTTONS_NAME5, + CONFIG_EXAMPLE_BUTTONS_NAME6, CONFIG_EXAMPLE_BUTTONS_NAME7, + + Additional architecture-/board- specific configuration settings may also + be required. + + NOTE: This test exercises internal button 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/can +^^^^^^^^^^^^ + + If the CAN device is configured in loopback mode, then this example can + be used to test the CAN device in loop back mode. It simple sinces a + sequence of CAN messages and verifies that those messages are returned + exactly as sent. + + This test depends on these specific CAN/NSH configurations settings (your + specific CAN settings might require additional settings). + + CONFIG_CAN - Enables CAN support. + CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback + mode for testing. The STM32 CAN driver does support loopback mode. + CONFIG_NSH_BUILTIN_APPS - Build the CAN test as an NSH built-in function. + Default: Built as a standalone problem + + Specific configuration options for this example include: + + CONFIG_EXAMPLES_CAN_DEVPATH - The path to the CAN device. Default: /dev/can0 + CONFIG_EXAMPLES_CAN_NMSGS - If CONFIG_NSH_BUILTIN_APPS + is defined, then the number of loops is provided on the command line + and this value is ignored. Otherwise, this number of CAN message is + collected and the program terminates. Default: If built as an NSH + built-in, the default is 32. Otherwise messages are sent and received + indefinitely. + + The default behavior assumes loopback mode. Messages are sent, then read + and verified. The behavior can be altered for other kinds of testing where + the test only sends or received (but does not verify) can messages. + + CONFIG_EXAMPLES_CAN_READONLY - Only receive messages + CONFIG_EXAMPLES_CAN_WRITEONLY - Only send messages + +examples/cdcacm +^^^^^^^^^^^^^^^ + + This very simple example shows how a USB CDC/ACM serial can be dynamically + connected and disconnected from a host. This example can only be used as + an NSH built-int command. If built-in, then two new NSH commands will be + supported: + + 1. sercon - Connect the CDC/ACM serial device + 2. serdis - Disconnect the CDC/ACM serial device + + Configuration prequisites (not complete): + + CONFIG_USBDEV=y : USB device support must be enabled + CONFIG_CDCACM=y : The CDC/ACM driver must be built + CONFIG_NSH_BUILTIN_APPS : NSH built-in application support must be enabled + + Configuration options specific to this example: + + CONFIG_EXAMPLES_CDCACM_DEVMINOR : The minor number of the CDC/ACM device. + : i.e., the 'x' in /dev/ttyACMx + + If CONFIG_USBDEV_TRACE is enabled (or CONFIG_DEBUG and CONFIG_DEBUG_USB, or + CONFIG_USBDEV_TRACE), then the example code will also initialize the USB trace + output. The amount of trace output can be controlled using: + + CONFIG_EXAMPLES_CDCACM_TRACEINIT + Show initialization events + CONFIG_EXAMPLES_CDCACM_TRACECLASS + Show class driver events + CONFIG_EXAMPLES_CDCACM_TRACETRANSFERS + Show data transfer events + CONFIG_EXAMPLES_CDCACM_TRACECONTROLLER + Show controller events + CONFIG_EXAMPLES_CDCACM_TRACEINTERRUPTS + Show interrupt-related events. + + Note: This example is only enables or disable USB CDC/ACM via the NSH + 'sercon' and 'serdis' command. It will enable and disable tracing per + the settings before enabling and after disabling the CDC/ACM device. It + will not, however, monitor buffered trace data in the interim. If + CONFIG_USBDEV_TRACE is defined (and the debug options are not), other + application logic will need to monitor the buffered trace data. + +examples/composite +^^^^^^^^^^^^^^^^^^ + + This example test a USB composite device. The only supported composite is + CDC/ACM serial with a USB mass storage device. + + Required overall configuration: + + CONFIG_USBDEV=y - USB device support + CONFIG_USBDEV_COMPOSITE=y - USB composite device support + CONFIG_COMPOSITE_IAD=y - Interface associate descriptor needed + + CONFIG_CDCACM=y - USB CDC/ACM serial device support + CONFIG_CDCACM_COMPOSITE=y - USB CDC/ACM serial composite device support + CONFIG_CDCACM_IFNOBASE=0 - CDC/ACM interfaces start with number 0 + CONFIG_CDCACM_STRBASE=4 - Base of string numbers (not really needed) + CONFIG_CDCACM_EPINTIN=1 - Endpoint numbers must be unique + CONFIG_CDCACM_EPBULKIN=2 + CONFIG_CDCACM_EPBULKOUT=3 + + CONFIG_USBMSC - USB mass storage device support + CONFIG_USBMSC_COMPOSITE=y - USB mass storage composite device support + CONFIG_USBMSC_IFNOBASE=2 - USB mass storage interfaces start with number 2 + CONFIG_USBMSC_STRBASE=4 - Base of string numbers (needed) + CONFIG_USBMSC_EPBULKOUT=4 - Endpoint numbers must be unique + CONFIG_USBMSC_EPBULKIN=5 + + CONFIG_NSH_BUILTIN_APPS + This example can be built as two NSH "built-in" commands if this option + is selected: 'conn' will connect the USB composite device; 'msdis' + will disconnect the USB composite device. + + Configuration options unique to this example: + + CONFIG_EXAMPLES_COMPOSITE_DEBUGMM + Enables some debug tests to check for memory usage and memory leaks. + + CONFIG_EXAMPLES_COMPOSITE_NLUNS + Defines the number of logical units (LUNs) exported by the USB storage + driver. Each LUN corresponds to one exported block driver (or partition + of a block driver). May be 1, 2, or 3. Default is 1. + CONFIG_EXAMPLES_COMPOSITE_DEVMINOR1 + The minor device number of the block driver for the first LUN. For + example, N in /dev/mmcsdN. Used for registering the block driver. Default + is zero. + CONFIG_EXAMPLES_COMPOSITE_DEVPATH1 + The full path to the registered block driver. Default is "/dev/mmcsd0" + CONFIG_EXAMPLES_COMPOSITE_DEVMINOR2 and CONFIG_EXAMPLES_COMPOSITE_DEVPATH2 + Similar parameters that would have to be provided if CONFIG_EXAMPLES_COMPOSITE_NLUNS + is 2 or 3. No defaults. + CONFIG_EXAMPLES_COMPOSITE_DEVMINOR3 and CONFIG_EXAMPLES_COMPOSITE_DEVPATH2 + Similar parameters that would have to be provided if CONFIG_EXAMPLES_COMPOSITE_NLUNS + is 3. No defaults. + CONFIG_EXAMPLES_COMPOSITE_BUFLEN. Default 256. + + CONFIG_EXAMPLES_COMPOSITE_TTYUSB - The minor number of the USB serial device. + Default is zero (corresponding to /dev/ttyUSB0 or /dev/ttyACM0). Default is zero. + CCONFIG_EXAMPLES_COMPOSITE_SERDEV - The string corresponding to + CONFIG_EXAMPLES_COMPOSITE_TTYUSB. The default is "/dev/ttyUSB0" (for the PL2303 + emulation) or "/dev/ttyACM0" (for the CDC/ACM serial device). + CONFIG_EXAMPLES_COMPOSITE_BUFSIZE - The size of the serial I/O buffer in + bytes. Default 256 bytes. + + If CONFIG_USBDEV_TRACE is enabled (or CONFIG_DEBUG and CONFIG_DEBUG_USB), then + the example code will also manage the USB trace output. The amount of trace output + can be controlled using: + + CONFIG_EXAMPLES_COMPOSITE_TRACEINIT + Show initialization events + CONFIG_EXAMPLES_COMPOSITE_TRACECLASS + Show class driver events + CONFIG_EXAMPLES_COMPOSITE_TRACETRANSFERS + Show data transfer events + CONFIG_EXAMPLES_COMPOSITE_TRACECONTROLLER + Show controller events + CONFIG_EXAMPLES_COMPOSITE_TRACEINTERRUPTS + Show interrupt-related events. + +examples/dhcpd +^^^^^^^^^^^^^^ + + This examples builds a tiny DCHP server for the target system. + + NOTE: For test purposes, this example can be built as a + host-based DHCPD server. This can be built as follows: + + cd examples/dhcpd + make -f Makefile.host TOPDIR=<nuttx-directory> + + NuttX configuration settings: + + CONFIG_NET=y - Of course + CONFIG_NSOCKET_DESCRIPTORS - And, of course, you must allocate some + socket descriptors. + CONFIG_NET_UDP=y - UDP support is required for DHCP + (as well as various other UDP-related + configuration settings) + CONFIG_NET_BROADCAST=y - UDP broadcast support is needed. + + CONFIG_EXAMPLE_DHCPD_NOMAC - (May be defined to use software assigned MAC) + CONFIG_EXAMPLE_DHCPD_IPADDR - Target IP address + CONFIG_EXAMPLE_DHCPD_DRIPADDR - Default router IP addess + CONFIG_EXAMPLE_DHCPD_NETMASK - Network mask + + See also CONFIG_NETUTILS_DHCPD_* settings described elsewhere + and used in netutils/dhcpd/dhcpd.c. These settings are required + to described the behavior of the daemon. + + Applications using this example will need to provide an appconfig + file in the configuration driver with instruction to build applications + like: + + CONFIGURED_APPS += uiplib + +examples/ftpc +^^^^^^^^^^^^^ + + This is a simple FTP client shell used to exercise the capabilities + of the FTPC library (apps/netutils/ftpc). This example is configured + to that it will only work as a "built-in" program that can be run from + NSH when CONFIG_NSH_BUILTIN_APPS is defined. + + From NSH, the startup command sequence is as follows. This is only + an example, your configration could have different mass storage devices, + mount paths, and FTP directories: + + nsh> mount -t vfat /dev/mmcsd0 /tmp # Mount the SD card at /tmp + nsh> cd /tmp # cd into the /tmp directory + nsh> ftpc xx.xx.xx.xx[:pp] # Start the FTP client + nfc> login <name> <password> # Log into the FTP server + nfc> help # See a list of FTP commands + + where xx.xx.xx.xx is the IP address of the FTP server and pp is an + optional port number. + + NOTE: By default, FTPC uses readline to get data from stdin. So your + appconfig file must have the following build path: + + CONFIGURED_APPS += system/readline + + NOTE: If you use the ftpc task over a telnet NSH connection, then you + should set the following configuration item: + + CONFIG_EXAMPLES_FTPC_FGETS=y + + By default, the FTPC client will use readline() to get characters from + the console. Readline includes and command-line editor and echos + characters received in stdin back through stdout. Neither of these + behaviors are desire-able if Telnet is used. + + You may also want to define the following in your configuration file. + Otherwise, you will have not feeback about what is going on: + + CONFIG_DEBUG=y + CONFIG_DEBUG_VERBOSE=y + CONFIG_DEBUG_FTPC=y + +examples/ftpd +^^^^^^^^^^^^^^ + + This example exercises the FTPD daemon at apps/netuils/ftpd. Below are + configurations specific to the FTPD example (the FTPD daemon itself may + require other configuration options as well). + + CONFIG_EXAMPLES_FTPD_PRIO - Priority of the FTP daemon. + Default: SCHED_PRIORITY_DEFAULT + CONFIG_EXAMPLES_FTPD_STACKSIZE - Stack size allocated for the + FTP daemon. Default: 2048 + CONFIG_EXAMPLES_FTPD_NONETINIT - Define to suppress configuration of the + network by apps/examples/ftpd. You would need to suppress network + configuration if the network is configuration prior to running the + example. + + NSH always initializes the network so if CONFIG_NSH_BUILTIN_APPS is + defined, so is CONFIG_EXAMPLES_FTPD_NONETINIT (se it does not explicitly + need to be defined in that case): + + CONFIG_NSH_BUILTIN_APPS - Build the FTPD daemon example test as an + NSH built-in function. By default the FTPD daemon will be built + as a standalone application. + + If CONFIG_EXAMPLES_FTPD_NONETINIT is not defined, then the following may + be specified to customized the network configuration: + + CONFIG_EXAMPLE_FTPD_NOMAC - If the hardware has no MAC address of its + own, define this =y to provide a bogus address for testing. + CONFIG_EXAMPLE_FTPD_IPADDR - The target IP address. Default 10.0.0.2 + CONFIG_EXAMPLE_FTPD_DRIPADDR - The default router address. Default + 10.0.0.1 + CONFIG_EXAMPLE_FTPD_NETMASK - The network mask. Default: 255.255.255.0 + + Other required configuration settings: Of course TCP networking support + is required. But here are a couple that are less obvious: + + CONFIG_DISABLE_PTHREAD - pthread support is required + CONFIG_DISABLE_POLL - poll() support is required + + Other FTPD configuration options thay may be of interest: + + CONFIG_FTPD_VENDORID - The vendor name to use in FTP communications. + Default: "NuttX" + CONFIG_FTPD_SERVERID - The server name to use in FTP communications. + Default: "NuttX FTP Server" + CONFIG_FTPD_CMDBUFFERSIZE - The maximum size of one command. Default: + 512 bytes. + CONFIG_FTPD_DATABUFFERSIZE - The size of the I/O buffer for data + transfers. Default: 2048 bytes. + CONFIG_FTPD_WORKERSTACKSIZE - The stacksize to allocate for each + FTP daemon worker thread. Default: 2048 bytes. + + The appconfig file (apps/.config) should include: + + CONFIGURED_APPS += examples/ftpd + CONFIGURED_APPS += netutils/uiplib + CONFIGURED_APPS += netutils/ftpd + +examples/hello +^^^^^^^^^^^^^^ + + This is the mandatory, "Hello, World!!" example. It is little more + than examples/null with a single printf statement. Really useful only + for bringing up new NuttX architectures. + + * CONFIG_EXAMPLES_HELLO_BUILTIN + Build the "Hello, World" example as an NSH built-in application. + +examples/helloxx +^^^^^^^^^^^^^^^^ + + This is C++ version of the "Hello, World!!" example. It is intended + only to verify that the C++ compiler is functional, that basic C++ + library suupport is available, and that class are instantiated + correctly. + + NuttX configuration prerequisites: + + CONFIG_HAVE_CXX -- Enable C++ Support + + Optional NuttX configuration settings: + + CONFIG_HAVE_CXXINITIALIZE -- Enable support for static constructors + (may not be available on all platforms). + + NuttX configuration settings specific to this examp;le: + + CONFIG_EXAMPLES_HELLOXX_BUILTIN -- Build the helloxx example as a + "built-in" that can be executed from the NSH command line. + CONFIG_EXAMPLES_HELLOXX_NOSTACKCONST - Set if the system does not + support construction of objects on the stack. + + Also needed: + + CONFIG_HAVE_CXX=y + + And you may have to tinker with the following to get libxx to compile + properly: + + CONFIG_CXX_NEWLONG=y or =n + + The argument of the 'new' operators should take a type of size_t. But size_t + has an unknown underlying. In the nuttx sys/types.h header file, size_t + is typed as uint32_t (which is determined by architecture-specific logic). + But the C++ compiler may believe that size_t is of a different type resulting + in compilation errors in the operator. Using the underlying integer type + Instead of size_t seems to resolve the compilation issues. + +examples/hidkbd +^^^^^^^^^^^^^^^^ + + This is a simple test to debug/verify the USB host HID keyboard class + driver. + + CONFIG_EXAMPLES_HIDKBD_DEFPRIO - Priority of "waiter" thread. + CONFIG_EXAMPLES_HIDKBD_STACKSIZE - Stacksize of "waiter" thread. + +examples/igmp +^^^^^^^^^^^^^ + + This is a trivial test of the NuttX IGMP capability. It present it + does not do much of value -- Much more is needed in order to verify + the IGMP features! + + * CONFIG_EXAMPLE_IGMP_NOMAC + Set if the hardware has no MAC address; one will be assigned + * CONFIG_EXAMPLE_IGMP_IPADDR + Target board IP address + * CONFIG_EXAMPLE_IGMP_DRIPADDR + Default router address + * CONFIG_EXAMPLE_IGMP_NETMASK + Network mask + * CONFIG_EXAMPLE_IGMP_GRPADDR + Multicast group address + + Applications using this example will need to provide an appconfig + file in the configuration driver with instruction to build applications + like: + + CONFIGURED_APPS += uiplib + +examples/lcdrw +^^^^^^^^^^^^^^ + + This example may be used to verify if you can or cannot read data + correctly from an LCD interface. At present, this supports only LCDs + with RGB565 color format. + + * CONFIG_EXAMPLES_LDCRW_DEVNO + LCD device number. Default: 0 + * CONFIG_EXAMPLES_LDCRW_XRES + LCD X resolution. Default: 240 + * CONFIG_EXAMPLES_LDCRW_YRES + LCD Y resolution. Default: 320 + +examples/mm +^^^^^^^^^^^ + + This is a simplified version of the "built-in" memory manager test of + mm/mm_test.c. It is simplified because it does not have access to the + internals of the memory manager as does mm/mm_test.c, but it has the + advantage that it runs in the actual NuttX tasking environment (the + mm/mm_test.c only runs in a PC simulation environment). + +examples/modbus +^^^^^^^^^^^^^^^ + + This is a port of the FreeModbus Linux demo. It derives from the + demos/LINUX directory of the FreeModBus version 1.5.0 (June 6, 2010) + that can be downloaded in its entirety from http://developer.berlios.de/project/showfiles.php?group_id=6120. + + CONFIG_EXAMPLES_MODBUS_PORT, Default 0 (for /dev/ttyS0) + CONFIG_EXAMPLES_MODBUS_BAUD, Default B38400 + CONFIG_EXAMPLES_MODBUS_PARITY, Default MB_PAR_EVEN + + CONFIG_EXAMPLES_MODBUS_REG_INPUT_START, Default 1000 + CONFIG_EXAMPLES_MODBUS_REG_INPUT_NREGS, Default 4 + CONFIG_EXAMPLES_MODBUS_REG_HOLDING_START, Default 2000 + CONFIG_EXAMPLES_MODBUS_REG_HOLDING_NREGS, Default 130 + + The FreeModBus library resides at apps/modbus. See apps/modbus/README.txt + for additional configuration information. + +examples/mount +^^^^^^^^^^^^^^ + + This contains a simple test of filesystem mountpoints. + + * CONFIG_EXAMPLES_MOUNT_DEVNAME + The name of the user-provided block device to mount. + If CONFIG_EXAMPLES_MOUNT_DEVNAME is not provided, then + a RAM disk will be configured. + + * CONFIG_EXAMPLES_MOUNT_NSECTORS + The number of "sectors" in the RAM disk used when + CONFIG_EXAMPLES_MOUNT_DEVNAME is not defined. + + * CONFIG_EXAMPLES_MOUNT_SECTORSIZE + The size of each sectors in the RAM disk used when + CONFIG_EXAMPLES_MOUNT_DEVNAME is not defined. + + * CONFIG_EXAMPLES_MOUNT_RAMDEVNO + The RAM device minor number used to mount the RAM disk used + when CONFIG_EXAMPLES_MOUNT_DEVNAME is not defined. The + default is zero (meaning that "/dev/ram0" will be used). + +examples/netttest +^^^^^^^^^^^^^^^^^ + + This is a simple network test for verifying client- and server- + functionality in a TCP/IP connection. + + Applications using this example will need to provide an appconfig + file in the configuration driver with instruction to build applications + like: + + CONFIGURED_APPS += uiplib + +examples/nsh +^^^^^^^^^^^^ + + This directory provides an example of how to configure and use + the NuttShell (NSH) application. NSH is a simple shell + application. NSH is described in its own README located at + apps/nshlib/README.txt + + Applications using this example will need to provide an appconfig + file in the configuration driver with instruction to build applications + like: + + CONFIGURED_APPS += nshlib + + NOTE: If the NSH serial console is used, then following is also + required to build the readline() library: + + CONFIGURED_APPS += system/readline + + And if networking is included: + + CONFIGURED_APPS += uiplib + CONFIGURED_APPS += dhcpc + CONFIGURED_APPS += resolv + CONFIGURED_APPS += tftp + CONFIGURED_APPS += webclient + + If the Telnet console is enabled, then the appconfig file (apps/.config) + should also include: + + CONFIGURED_APPS += netutils/telnetd + + Also if the Telnet console is enabled, make sure that you have the + following set in the NuttX configuration file or else the performance + will be very bad (because there will be only one character per TCP + transfer): + + CONFIG_STDIO_BUFFER_SIZE - Some value >= 64 + CONFIG_STDIO_LINEBUFFER=y + +examples/nx +^^^^^^^^^^^ + + This directory contains a simple test of a subset of the NX APIs + defined in include/nuttx/nx/nx.h. The following configuration options + can be selected: + + CONFIG_EXAMPLES_NX_BUILTIN -- Build the NX example as a "built-in" + that can be executed from the NSH command line + CONFIG_EXAMPLES_NX_VPLANE -- The plane to select from the frame- + buffer driver for use in the test. Default: 0 + CONFIG_EXAMPLES_NX_DEVNO - The LCD device to select from the LCD + driver for use in the test: Default: 0 + CONFIG_EXAMPLES_NX_BGCOLOR -- The color of the background. Default depends on + CONFIG_EXAMPLES_NX_BPP. + CONFIG_EXAMPLES_NX_COLOR1 -- The color of window 1. Default depends on + CONFIG_EXAMPLES_NX_BPP. + CONFIG_EXAMPLES_NX_COLOR2 -- The color of window 2. Default depends on + CONFIG_EXAMPLES_NX_BPP. + CONFIG_EXAMPLES_NX_TBCOLOR -- The color of the toolbar. Default depends on + CONFIG_EXAMPLES_NX_BPP. + CONFIG_EXAMPLES_NX_FONTID - Selects the font (see font ID numbers in + include/nuttx/nx/nxfonts.h) + CONFIG_EXAMPLES_NX_FONTCOLOR -- The color of the fonts. Default depends on + CONFIG_EXAMPLES_NX_BPP. + CONFIG_EXAMPLES_NX_BPP -- Pixels per pixel to use. Valid options + include 2, 4, 8, 16, 24, and 32. Default is 32. + CONFIG_EXAMPLES_NX_RAWWINDOWS -- Use raw windows; Default is to + use pretty, framed NXTK windows with toolbars. + CONFIG_EXAMPLES_NX_EXTERNINIT - The driver for the graphics device on + this platform requires some unusual initialization. This is the + for, for example, SPI LCD/OLED devices. If this configuration is + selected, then the platform code must provide an LCD initialization + function with a prototype like: + + #ifdef CONFIG_NX_LCDDRIVER + FAR struct lcd_dev_s *up_nxdrvinit(unsigned int devno); + #else + FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno); + #endif + + This test can be performed with either the single-user version of + NX or with the multiple user version of NX selected with CONFIG_NX_MULTIUSER. + If CONFIG_NX_MULTIUSER is defined, then the following configuration + options also apply: + + CONFIG_EXAMPLES_NX_STACKSIZE -- The stacksize to use when creating + the NX server. Default 2048 + CONFIG_EXAMPLES_NX_CLIENTPRIO -- The client priority. Default: 100 + CONFIG_EXAMPLES_NX_SERVERPRIO -- The server priority. Default: 120 + CONFIG_EXAMPLES_NX_LISTENERPRIO -- The priority of the event listener + thread. Default 80. + CONFIG_EXAMPLES_NX_NOTIFYSIGNO -- The signal number to use with + nx_eventnotify(). Default: 4 + + If CONFIG_NX_MULTIUSER is defined, then the example also expects the + following settings and will generate an error if they are not as expected: + + CONFIG_DISABLE_MQUEUE=n + CONFIG_DISABLE_SIGNALS=n + CONFIG_DISABLE_PTHREAD=n + CONFIG_NX_BLOCKING=y + +examples/nxconsole +^^^^^^^^^^^^^^^^^^ + + This directory contains yet another version of the NuttShell (NSH). This + version uses the NX console device defined in include/nuttx/nx/nxconsole.h + for output. the result is that the NSH input still come from the standard + console input (probably a serial console). But the text output will go to + an NX winbdow. Prerequisite configuration settings for this test include: + + CONFIG_NX=y -- NX graphics must be enabled + CONFIG_NXCONSOLE=y -- The NX console driver must be built + CONFIG_NX_MULTIUSER=y -- NX multi-user support must be enabled. + CONFIG_DISABLE_MQUEUE=n -- Message queue support must be available. + CONFIG_DISABLE_SIGNALS=n -- Signals are needed + CONFIG_DISABLE_PTHREAD=n -- pthreads are needed + CONFIG_NX_BLOCKING=y -- pthread APIs must be blocking + CONFIG_NSH_CONSOLE=y -- NSH must be configured to use a console. + + The following configuration options can be selected to customize the + test: + + CONFIG_EXAMPLES_NXCON_VPLANE -- The plane to select from the frame- + buffer driver for use in the test. Default: 0 + CONFIG_EXAMPLES_NXCON_DEVNO - The LCD device to select from the LCD + driver for use in the test: Default: 0 + CONFIG_EXAMPLES_NXCON_BGCOLOR -- The color of the background. Default + Default is a darker royal blue. + CONFIG_EXAMPLES_NXCON_WCOLOR -- The color of the window. Default is a light + slate blue. + CONFIG_EXAMPLES_NXCON_FONTID -- Selects the font (see font ID numbers in + include/nuttx/nx/nxfonts.h) + CONFIG_EXAMPLES_NXCON_FONTCOLOR -- The color of the fonts. Default is + black. + CONFIG_EXAMPLES_NXCON_BPP -- Pixels per pixel to use. Valid options + include 2, 4, 8, 16, 24, and 32. Default is 32. + CONFIG_EXAMPLES_NXCON_TOOLBAR_HEIGHT -- The height of the toolbar. + Default: 16 + CONFIG_EXAMPLES_NXCON_TBCOLOR -- The color of the toolbar. Default is + a medium grey. + CONFIG_EXAMPLES_NXCON_EXTERNINIT - The driver for the graphics device on + this platform requires some unusual initialization. This is the + for, for example, SPI LCD/OLED devices. If this configuration is + selected, then the platform code must provide an LCD initialization + function with a prototype like: + + #ifdef CONFIG_NX_LCDDRIVER + FAR struct lcd_dev_s *up_nxdrvinit(unsigned int devno); + #else + FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno); + #endif + + CONFIG_EXAMPLES_NXCON_MINOR -- The NX console device minor number. + Default is 0 corresponding to /dev/nxcon0 + CONFIG_EXAMPLES_NXCON_DEVNAME -- The quoated, full path to the + NX console device corresponding to CONFIG_EXAMPLES_NXCON_MINOR. + Default: "/dev/nxcon0" + CONFIG_EXAMPLES_NXCONSOLE_PRIO - Priority of the NxConsole task. + Default: SCHED_PRIORITY_DEFAULT + CONFIG_EXAMPLES_NXCONSOLE_STACKSIZE - Stack size allocated for the + NxConsole task. Default: 2048 + + The following configuration settings determine how to set up the NX + server (CONFIG_NX_MULTIUSER): + + CONFIG_EXAMPLES_NXCON_STACKSIZE -- The stacksize to use when creating + the NX server. Default 2048 + CONFIG_EXAMPLES_NXCON_CLIENTPRIO -- The client priority. Default: 100 + CONFIG_EXAMPLES_NXCON_SERVERPRIO -- The server priority. Default: 120 + CONFIG_EXAMPLES_NXCON_LISTENERPRIO -- The priority of the event listener + thread. Default 80. + CONFIG_EXAMPLES_NXCON_NOTIFYSIGNO -- The signal number to use with + nx_eventnotify(). Default: 4 + +examples/nxffs +^^^^^^^^^^^^^^ + + This is a test of the NuttX NXFFS FLASH file system. This is an NXFFS + stress test and beats on the file system very hard. It should only + be used in a simulation environment! Putting this NXFFS test on real + hardware will most likely destroy your FLASH. You have been warned. + +examples/nxflat +^^^^^^^^^^^^^^^ + + This example builds a small NXFLAT test case. This includes several + test programs under examples/nxflat tests. These tests are build using + the NXFLAT format and installed in a ROMFS file system. At run time, + each program in the ROMFS file system is executed. Requires CONFIG_NXFLAT. + +examplex/nxhello +^^^^^^^^^^^^^^^^ + + A very simple graphics example that just says "Hello, World!" in the + center of the display. + + The following configuration options can be selected: + + CONFIG_EXAMPLES_NXHELLO_BUILTIN -- Build the NXHELLO example as a "built-in" + that can be executed from the NSH command line + CONFIG_EXAMPLES_NXHELLO_VPLANE -- The plane to select from the frame- + buffer driver for use in the test. Default: 0 + CONFIG_EXAMPLES_NXHELLO_DEVNO - The LCD device to select from the LCD + driver for use in the test: Default: 0 + CONFIG_EXAMPLES_NXHELLO_BGCOLOR -- The color of the background. Default + depends on CONFIG_EXAMPLES_NXHELLO_BPP. + CONFIG_EXAMPLES_NXHELLO_FONTID - Selects the font (see font ID numbers in + include/nuttx/nx/nxfonts.h) + CONFIG_EXAMPLES_NXHELLO_FONTCOLOR -- The color of the fonts used in the + background window. Default depends on CONFIG_EXAMPLES_NXHELLO_BPP. + CONFIG_EXAMPLES_NXHELLO_BPP -- Pixels per pixel to use. Valid options + include 2, 4, 8, 16, 24, and 32. Default is 32. + CONFIG_EXAMPLES_NXHELLO_EXTERNINIT - The driver for the graphics device on + this platform requires some unusual initialization. This is the + for, for example, SPI LCD/OLED devices. If this configuration is + selected, then the platform code must provide an LCD initialization + function with a prototype like: + + #ifdef CONFIG_NX_LCDDRIVER + FAR struct lcd_dev_s *up_nxdrvinit(unsigned int devno); + #else + FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno); + #endif + +examples/nximage +^^^^^^^^^^^^^^^^ + + This is a simple example that just puts the NuttX logo image in the center + of the display. This only works for RGB23 (888), RGB16 (656), RGB8 (332), + and 8-bit greyscale for now. + + CONFIG_EXAMPLES_NXIMAGE_BUILTIN -- Build the NXIMAGE example as a "built-in" + that can be executed from the NSH command line + CONFIG_EXAMPLES_NXIMAGE_VPLANE -- The plane to select from the frame- + buffer driver for use in the test. Default: 0 + CONFIG_EXAMPLES_NXIMAGE_DEVNO - The LCD device to select from the LCD + driver for use in the test: Default: 0 + CONFIG_EXAMPLES_NXIMAGE_BPP -- Pixels per pixel to use. Valid options + include 8, 16, and 24. Default is 16. + CONFIG_EXAMPLES_NXIMAGE_XSCALEp5, CONFIG_EXAMPLES_NXIMAGE_XSCALE1p5, + CONFIG_EXAMPLES_NXIMAGE_XSCALE2p0 -- The logo image width is 160 columns. + One of these may be defined to rescale the image horizontally by .5, 1.5, + or 2.0. + CONFIG_EXAMPLES_NXIMAGE_YSCALEp5, CONFIG_EXAMPLES_NXIMAGE_YSCALE1p5, + CONFIG_EXAMPLES_NXIMAGE_YSCALE2p0 -- The logo image height is 160 rows. + One of these may be defined to rescale the image vertically by .5, 1.5, + or 2.0. + CONFIG_EXAMPLES_NXIMAGE_GREYSCALE -- Grey scale image. Default: RGB. + CONFIG_EXAMPLES_NXIMAGE_EXTERNINIT - The driver for the graphics device on + this platform requires some unusual initialization. This is the + for, for example, SPI LCD/OLED devices. If this configuration is + selected, then the platform code must provide an LCD initialization + function with a prototype like: + + #ifdef CONFIG_NX_LCDDRIVER + FAR struct lcd_dev_s *up_nxdrvinit(unsigned int devno); + #else + FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno); + #endif + + How was that run-length encoded image produced? + + a. I used GIMP output the image as a .c file. + b. I added som C logic to palette-ize the RGB image in the GIMP .c file + c. Then I add some simple run-length encoding to palette-ized image. + + NOTE: As of this writing, most of the pixel depth, scaling options, and + combinations thereof have not been tested. + +examplex/nxlines +^^^^^^^^^^^^^^^^ + + A very simple graphics example that just exercised the NX line drawing + logic. + + 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 + driver for use in the test: Default: 0 + CONFIG_EXAMPLES_NXLINES_BGCOLOR -- The color of the background. Default + depends on CONFIG_EXAMPLES_NXLINES_BPP. + CONFIG_EXAMPLES_NXLINES_LINEWIDTH - Selects the width of the lines in + pixels (default: 16) + CONFIG_EXAMPLES_NXLINES_LINECOLOR -- The color of the central lines drawn + in the background window. Default depends on CONFIG_EXAMPLES_NXLINES_BPP + (there really is no meaningful default). + CONFIG_EXAMPLES_NXLINES_BORDERWIDTH -- The width of the circular border + drawn in the background window. (default: 16). + CONFIG_EXAMPLES_NXLINES_BORDERCOLOR -- The color of the circular border + drawn in the background window. Default depends on CONFIG_EXAMPLES_NXLINES_BPP + (there really is no meaningful default). + CONFIG_EXAMPLES_NXLINES_CIRCLECOLOR -- The color of the circular region + filled in the background window. Default depends on CONFIG_EXAMPLES_NXLINES_BPP + (there really is no meaningful default). + CONFIG_EXAMPLES_NXLINES_BORDERCOLOR -- The color of the lines drawn in the + background window. Default depends on CONFIG_EXAMPLES_NXLINES_BPP (there + really is no meaningful default). + + CONFIG_EXAMPLES_NXLINES_BPP -- Pixels per pixel to use. Valid options + include 2, 4, 8, 16, 24, and 32. Default is 16. + CONFIG_EXAMPLES_NXLINES_EXTERNINIT - The driver for the graphics device on + this platform requires some unusual initialization. This is the + for, for example, SPI LCD/OLED devices. If this configuration is + selected, then the platform code must provide an LCD initialization + function with a prototype like: + + #ifdef CONFIG_NX_LCDDRIVER + FAR struct lcd_dev_s *up_nxdrvinit(unsigned int devno); + #else + FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno); + #endif + +examples/nxtext +^^^^^^^^^^^^^^^ + + This directory contains another simple test of a subset of the NX APIs + defined in include/nuttx/nx/nx.h. This text focuses on text displays on + the dispaly background combined with pop-up displays over the text. + The text display will continue to update while the pop-up is visible. + + NOTE: This example will *only* work with FB drivers and with LCD + drivers that support reading the contents of the internal LCD memory + *unless* you define CONFIG_EXAMPLES_NXTEXT_NOGETRUN. If you notice + garbage on the display or a failure at the point where the display + should scroll, it is probably because you have an LCD driver that is + write-only. + + The following configuration options can be selected: + + CONFIG_EXAMPLES_NXTEXT_BUILTIN -- Build the NXTEXT example as a "built-in" + that can be executed from the NSH command line + CONFIG_EXAMPLES_NXTEXT_VPLANE -- The plane to select from the frame- + buffer driver for use in the test. Default: 0 + CONFIG_EXAMPLES_NXTEXT_DEVNO - The LCD device to select from the LCD + driver for use in the test: Default: 0 + CONFIG_EXAMPLES_NXTEXT_BGCOLOR -- The color of the background. Default + depends on CONFIG_EXAMPLES_NXTEXT_BPP. + CONFIG_EXAMPLES_NXTEXT_BGFONTID - Selects the font to use in the + background text (see font ID numbers in include/nuttx/nx/nxfonts.h) + CONFIG_EXAMPLES_NXTEXT_BGFONTCOLOR -- The color of the fonts used in the + background window. Default depends on CONFIG_EXAMPLES_NXTEXT_BPP. + CONFIG_EXAMPLES_NXTEXT_PUCOLOR -- The color of the pop-up window. Default + depends on CONFIG_EXAMPLES_NXTEXT_BPP. + CONFIG_EXAMPLES_NXTEXT_PUFONTID - Selects the font to use in the pop-up + windows (see font ID numbers in include/nuttx/nx/nxfonts.h) + CONFIG_EXAMPLES_NXTEXT_PUFONTCOLOR -- The color of the fonts used in the + background window. Default depends on CONFIG_EXAMPLES_NXTEXT_BPP. + CONFIG_EXAMPLES_NXTEXT_BPP -- Pixels per pixel to use. Valid options + include 2, 4, 8, 16, 24, and 32. Default is 32. + CONFIG_EXAMPLES_NXTEXT_NOGETRUN -- If your display is read-only OR if + reading is not reliable, then select this configuration to avoid + reading from the display. + CONFIG_EXAMPLES_NXTEXT_EXTERNINIT - The driver for the graphics device on + this platform requires some unusual initialization. This is the + for, for example, SPI LCD/OLED devices. If this configuration is + selected, then the platform code must provide an LCD initialization + function with a prototype like: + + #ifdef CONFIG_NX_LCDDRIVER + FAR struct lcd_dev_s *up_nxdrvinit(unsigned int devno); + #else + FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno); + #endif + + CONFIG_EXAMPLES_NXTEXT_BMCACHE - The maximum number of characters that + can be put in the background window. Default is 128. + CONFIG_EXAMPLES_NXTEXT_GLCACHE - The maximum nuber of pre-rendered + fonts that can be retained for the background window. + + This test can be performed with either the single-user version of + NX or with the multiple user version of NX selected with CONFIG_NX_MULTIUSER. + If CONFIG_NX_MULTIUSER is defined, then the following configuration + options also apply: + + CONFIG_EXAMPLES_NXTEXT_STACKSIZE -- The stacksize to use when creating + the NX server. Default 2048 + CONFIG_EXAMPLES_NXTEXT_CLIENTPRIO -- The client priority. Default: 100 + CONFIG_EXAMPLES_NXTEXT_SERVERPRIO -- The server priority. Default: 120 + CONFIG_EXAMPLES_NXTEXT_LISTENERPRIO -- The priority of the event listener + thread. Default 80. + CONFIG_EXAMPLES_NXTEXT_NOTIFYSIGNO -- The signal number to use with + nx_eventnotify(). Default: 4 + + If CONFIG_NX_MULTIUSER is defined, then the example also expects the + following settings and will generate an error if they are not as expected: + + CONFIG_DISABLE_MQUEUE=n + CONFIG_DISABLE_SIGNALS=n + CONFIG_DISABLE_PTHREAD=n + CONFIG_NX_BLOCKING=y + +examples/null +^^^^^^^^^^^^^ + + This is the do nothing application. It is only used for bringing + up new NuttX architectures in the most minimal of environments. + +examples/ostest +^^^^^^^^^^^^^^^ + + This is the NuttX 'qualification' suite. It attempts to exercise + a broad set of OS functionality. Its coverage is not very extensive + as of this writing, but it is used to qualify each NuttX release. + + The behavior of the ostest can be modified with the following + settings in the configs/<board-name>/defconfig file: + + * CONFIG_EXAMPLES_OSTEST_BUILTIN + Build the OS test example as an NSH built-in application. + * CONFIG_EXAMPLES_OSTEST_LOOPS + Used to control the number of executions of the test. If + undefined, the test executes one time. If defined to be + zero, the test runs forever. + * CONFIG_EXAMPLES_OSTEST_STACKSIZE + Used to create the ostest task. Default is 8192. + * CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS + 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. + +examples/pashello +^^^^^^^^^^^^^^^^^ + + This is "Hello, World" implemented via the Pascal P-Code interpreter. In + order to use this example, you must first download and install the + NuttX pascal module. After unpacking the pascal module, you can find + installation instructions in pascal/nuttx/README.txt. + + The correct install location for the NuttX examples and build files is + apps/interpreters. + +examples/pipe +^^^^^^^^^^^^^ + + A test of the mkfifo() and pipe() APIs. + + * CONFIG_EXAMPLES_PIPE_STACKSIZE + Sets the size of the stack to use when creating the child tasks. + The default size is 1024. + +examples/poll +^^^^^^^^^^^^^ + + A test of the poll() and select() APIs using FIFOs and, if available, + stdin, and a TCP/IP socket. In order to build this test, you must the + following selected in your NuttX configuration file: + + CONFIG_NFILE_DESCRIPTORS - Defined to be greater than 0 + CONFIG_DISABLE_POLL - NOT defined + + In order to use the TCP/IP select test, you have also the following + additional things selected in your NuttX configuration file: + + CONFIG_NET - Defined for general network support + CONFIG_NET_TCP - Defined for TCP/IP support + CONFIG_NSOCKET_DESCRIPTORS - Defined to be greater than 0 + CONFIG_NET_NTCP_READAHEAD_BUFFERS - Defined to be greater than zero + + CONFIG_EXAMPLE_POLL_NOMAC - (May be defined to use software assigned MAC) + CONFIG_EXAMPLE_POLL_IPADDR - Target IP address + CONFIG_EXAMPLE_POLL_DRIPADDR - Default router IP addess + CONFIG_EXAMPLE_POLL_NETMASK - Network mask + + In order to for select to work with incoming connections, you + must also select: + + CONFIG_NET_TCPBACKLOG - Incoming connections pend in a backlog until accept() is called. + + In additional to the target device-side example, there is also + a host-side application in this directory. It can be compiled under + Linux or Cygwin as follows: + + cd examples/usbserial + make -f Makefile.host TOPDIR=<nuttx-directory> TARGETIP=<target-ip> + + Where <target-ip> is the IP address of your target board. + + This will generate a small program called 'host'. Usage: + + 1. Build the examples/poll target program with TCP/IP poll support + and start the target. + + 3. Then start the host application: + + ./host + + The host and target will exchange are variety of small messages. Each + message sent from the host should cause the select to return in target. + The target example should read the small message and send it back to + the host. The host should then receive the echo'ed message. + + If networking is enabled, applications using this example will need to + provide an appconfig file in the configuration driver with instruction + to build applications like: + + CONFIGURED_APPS += uiplib + +examples/pwm +^^^^^^^^^^^^ + + A test of a PWM device driver. It simply enables a pulsed output for + a specified frequency and duty for a specified period of time. This + example can ONLY be built as an NSH built-in function. + + This test depends on these specific PWM/NSH configurations settings (your + specific PWM settings might require additional settings). + + CONFIG_PWM - Enables PWM support. + CONFIG_EXAMPLES_PWM_COUNT - Enabled PWM pulse count support (if the + hardware supports it). + CONFIG_NSH_BUILTIN_APPS - Build the PWM test as an NSH built-in function. + Default: Not built! The example can only be used as an NSH built-in + application + + Specific configuration options for this example include: + + CONFIG_EXAMPLES_PWM_DEVPATH - The path to the default PWM device. Default: /dev/pwm0 + CONFIG_EXAMPLES_PWM_FREQUENCY - The initial PWM frequency. Default: 100 Hz + CONFIG_EXAMPLES_PWM_DUTYPCT - The initial PWM duty as a percentage. Default: 50% + CONFIG_EXAMPLES_PWM_DURATION - The initial PWM pulse train duration in seconds. + Used only if the current pulse count is zero (pulse count is only supported + if CONFIG_PWM_PULSECOUNT is defined). Default: 5 seconds + CONFIG_EXAMPLES_PWM_PULSECOUNT - The initial PWM pulse count. This option is + only available if CONFIG_PWM_PULSECOUNT is non-zero. Default: 0 (i.e., use + the duration, not the count). + +examples/qencoder +^^^^^^^^^^^^^^^^^ + + This example is a simple test of a Quadrature Encoder driver. It simply reads + positional data from the encoder and prints it., + + This test depends on these specific QE/NSH configurations settings (your + specific PWM settings might require additional settings). + + CONFIG_QENCODER - Enables quadrature encoder support (upper-half driver). + CONFIG_NSH_BUILTIN_APPS - Build the QE test as an NSH built-in function. + Default: Built as a standalone progrem. + + Additional configuration options will mostly likely be required for the board- + specific lower-half driver. See the README.txt file in your board configuration + directory. + + 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 + +examples/rgmp +^^^^^^^^^^^^^ + + RGMP stands for RTOS and GPOS on Multi-Processor. RGMP is a project for + running GPOS and RTOS simultaneously on multi-processor platforms. You can + port your favorite RTOS to RGMP together with an unmodified Linux to form a + hybrid operating system. This makes your application able to use both RTOS + and GPOS features. + + See http://rgmp.sourceforge.net/wiki/index.php/Main_Page for further + + At present, the RGMP example folder contains only an empty main.c file. + +examples/romfs +^^^^^^^^^^^^^^ + + This example exercises the romfs filesystem. Configuration options + include: + + * CONFIG_EXAMPLES_ROMFS_RAMDEVNO + The minor device number to use for the ROM disk. The default is + 1 (meaning /dev/ram1) + + * CONFIG_EXAMPLES_ROMFS_SECTORSIZE + The ROM disk sector size to use. Default is 64. + + * CONFIG_EXAMPLES_ROMFS_MOUNTPOINT + The location to mount the ROM disk. Deafault: "/usr/local/share" + +examples/sendmail +^^^^^^^^^^^^^^^^^ + + This examples exercises the uIP SMTP logic by sending a test message + to a selected recipient. This test can also be built to execute on + the Cygwin/Linux host environment: + + cd examples/sendmail + make -f Makefile.host TOPDIR=<nuttx-directory> + + Settings unique to this example include: + + CONFIG_EXAMPLE_SENDMAIL_NOMAC - May be defined to use software assigned MAC (optional) + CONFIG_EXAMPLE_SENDMAIL_IPADDR - Target IP address (required) + CONFIG_EXAMPLE_SENDMAIL_DRIPADDR - Default router IP addess (required) + CONFIG_EXAMPLE_SENDMAILT_NETMASK - Network mask (required) + CONFIG_EXAMPLE_SENDMAIL_RECIPIENT - The recipient of the email (required) + CONFIG_EXAMPLE_SENDMAIL_SENDER - Optional. Default: "nuttx-testing@example.com" + CONFIG_EXAMPLE_SENDMAIL_SUBJECT - Optional. Default: "Testing SMTP from NuttX" + CONFIG_EXAMPLE_SENDMAIL_BODY - Optional. Default: "Test message sent by NuttX" + + NOTE: This test has not been verified on the NuttX target environment. + As of this writing, unit-tested in the Cygwin/Linux host environment. + + NOTE 2: This sendmail example only works for the simplest of + environments. Virus protection software on your host may have + to be disabled to allow you to send messages. Only very open, + unprotected recipients can be used. Most will protect themselves + from this test email because it looks like SPAM. + + Applications using this example will need to provide an appconfig + file in the configuration driver with instruction to build applications + like: + + CONFIGURED_APPS += uiplib + CONFIGURED_APPS += smtp + +examples/serloop +^^^^^^^^^^^^^^^^ + + This is a mindlessly simple loopback test on the console. Useful + for testing new serial drivers. Configuration options include: + + * CONFIG_EXAMPLES_SERLOOP_BUFIO + Use C buffered I/O (getchar/putchar) vs. raw console I/O + (read/read). + +examples/telnetd +^^^^^^^^^^^^^^^^ + + This directory contains a functional port of the tiny uIP shell. In + the NuttX environment, the NuttShell (at apps/nshlib) supercedes this + tiny shell and also supports telnetd. + + CONFIG_EXAMPLES_TELNETD_DAEMONPRIO - Priority of the Telnet daemon. + Default: SCHED_PRIORITY_DEFAULT + CONFIG_EXAMPLES_TELNETD_DAEMONSTACKSIZE - Stack size allocated for the + Telnet daemon. Default: 2048 + CONFIG_EXAMPLES_TELNETD_CLIENTPRIO- Priority of the Telnet client. + Default: SCHED_PRIORITY_DEFAULT + CONFIG_EXAMPLES_TELNETD_CLIENTSTACKSIZE - Stack size allocated for the + Telnet client. Default: 2048 + CONFIG_EXAMPLE_TELNETD_NOMAC - If the hardware has no MAC address of its + own, define this =y to provide a bogus address for testing. + CONFIG_EXAMPLE_TELNETD_IPADDR - The target IP address. Default 10.0.0.2 + CONFIG_EXAMPLE_TELNETD_DRIPADDR - The default router address. Default + 10.0.0.1 + CONFIG_EXAMPLE_TELNETD_NETMASK - The network mask. Default: 255.255.255.0 + + The appconfig file (apps/.config) should include: + + CONFIGURED_APPS += examples/telnetd + CONFIGURED_APPS += netutils/uiplib + CONFIGURED_APPS += netutils/telnetd + + Also, make sure that you have the following set in the NuttX configuration + file or else the performance will be very bad (because there will be only + one character per TCP transfer): + + CONFIG_STDIO_BUFFER_SIZE - Some value >= 64 + CONFIG_STDIO_LINEBUFFER=y + +examples/thttpd +^^^^^^^^^^^^^^^ + + An example that builds netutils/thttpd with some simple NXFLAT + CGI programs. see configs/README.txt for most THTTPD settings. + In addition to those, this example accepts: + + CONFIG_EXAMPLE_THTTPD_NOMAC - (May be defined to use software assigned MAC) + CONFIG_EXAMPLE_THTTPD_DRIPADDR - Default router IP addess + CONFIG_EXAMPLE_THTTPD_NETMASK - Network mask + + Applications using this example will need to provide an appconfig + file in the configuration directory with instruction to build applications + like: + + CONFIGURED_APPS += uiplib + CONFIGURED_APPS += thttpd + +examples/tiff +^^^^^^^^^^^^^ + + This is a simple unit test for the TIFF creation library at apps/graphic/tiff. + It is configured to work in the Linux user-mode simulation and has not been + tested in any other environment. Since the example also depends on some + other logic to mount a file system, currently it will only work as an NSH + built-on, i.e., if the following is defined: + + CONFIG_NSH_BUILTIN_APPS=y + CONFIG_EXAMPLES_TIFF_BUILTIN=y + + At a miniumum, to run in an embedded environment, you will probably have to + change the configured paths to the TIFF files defined in the example. + + CONFIG_EXAMPLES_TIFF_OUTFILE - Name of the resulting TIFF file. Default is + "/tmp/result.tif" + CONFIG_EXAMPLES_TIFF_TMPFILE1/2 - Names of two temporaries files that + will be used in the file creation. Defaults are "/tmp/tmpfile1.dat" and + "/tmp/tmpfile2.dat" + + The following must also be defined in your apps/ configuration file: + + CONFIGURED_APPS += examples/tiff + CONFIGURED_APPS += graphics/tiff + +examples/touchscreen +^^^^^^^^^^^^^^^^^^^^ + + This configuration implements a simple touchscreen test at + apps/examples/touchscreen. This test will create an empty X11 window + and will print the touchscreen output as it is received from the + simulated touchscreen driver. + + CONFIG_EXAMPLES_TOUCHSCREEN_BUILTIN - Build the touchscreen test as + an NSH built-in function. Default: Built as a standalone problem + CONFIG_EXAMPLES_TOUCHSCREEN_MINOR - The minor device number. Minor=N + correspnds to touchscreen device /dev/input0. Note this value must + with CONFIG_EXAMPLES_TOUCHSCREEN_DEVPATH. Default 0. + CONFIG_EXAMPLES_TOUCHSCREEN_DEVPATH - The path to the touchscreen + device. This must be consistent with CONFIG_EXAMPLES_TOUCHSCREEN_MINOR. + Default: "/dev/input0" + CONFIG_EXAMPLES_TOUCHSCREEN_NSAMPLES - If CONFIG_EXAMPLES_TOUCHSCREEN_BUILTIN + 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. + + The following additional configurations must be set in the NuttX + configuration file: + + CONFIG_INPUTP=y + (Plus any touchscreen-specific settings). + + The following must also be defined in your apps configuration file: + + CONFIGURED_APPS += examples/tiff + CONFIGURED_APPS += graphics/tiff + + The board-specific logic must provide the following interfaces that will + be called by the example in order to initialize and uninitialize the + touchscreen hardware: + + int arch_tcinitialize(int minor); + int arch_tcuninitialize(void); + +examples/udp +^^^^^^^^^^^^ + + This is a simple network test for verifying client- and server- + functionality over UDP. + + Applications using this example will need to provide an appconfig + file in the configuration driver with instruction to build applications + like: + + CONFIGURED_APPS += uiplib + +examples/uip +^^^^^^^^^^^^ + + This is a port of uIP tiny webserver example application. Settings + specific to this example include: + + CONFIG_EXAMPLE_UIP_NOMAC - (May be defined to use software assigned MAC) + CONFIG_EXAMPLE_UIP_IPADDR - Target IP address + CONFIG_EXAMPLE_UIP_DRIPADDR - Default router IP addess + CONFIG_EXAMPLE_UIP_NETMASK - Network mask + CONFIG_EXAMPLE_UIP_DHCPC - Select to get IP address via DHCP + + If you use DHCPC, then some special configuration network options are + required. These include: + + CONFIG_NET=y - Of course + CONFIG_NSOCKET_DESCRIPTORS - And, of course, you must allocate some + socket descriptors. + CONFIG_NET_UDP=y - UDP support is required for DHCP + (as well as various other UDP-related + configuration settings). + CONFIG_NET_BROADCAST=y - UDP broadcast support is needed. + CONFIG_NET_BUFSIZE=650 - Per RFC2131 (p. 9), the DHCP client must be + (or larger) prepared to receive DHCP messages of up to + 576 bytes (excluding Ethernet, IP, or UDP + headers and FCS). + + Other configuration items apply also to the selected webserver net utility. + Additional relevant settings for the uIP webserver net utility are: + + CONFIG_NETUTILS_HTTPDSTACKSIZE + CONFIG_NETUTILS_HTTPDFILESTATS + CONFIG_NETUTILS_HTTPDNETSTATS + + Applications using this example will need to provide an appconfig + file in the configuration driver with instruction to build applications + like: + + CONFIGURED_APPS += uiplib + CONFIGURED_APPS += dhcpc + CONFIGURED_APPS += resolv + CONFIGURED_APPS += webserver + +examples/usbserial +^^^^^^^^^^^^^^^^^^ + + TARGET CONFIGURATION: + + This is another implementation of "Hello, World" but this one uses + a USB serial driver. Configuration options can be used to simply + the test. These options include: + + CONFIG_EXAMPLES_USBSERIAL_INONLY + Only verify IN (device-to-host) data transfers. Default: both + CONFIG_EXAMPLES_USBSERIAL_OUTONLY + Only verify OUT (host-to-device) data transfers. Default: both + CONFIG_EXAMPLES_USBSERIAL_ONLYSMALL + Send only small, single packet messages. Default: Send large and small. + CONFIG_EXAMPLES_USBSERIAL_ONLYBIG + Send only large, multi-packet messages. Default: Send large and small. + + If CONFIG_USBDEV_TRACE is enabled (or CONFIG_DEBUG and CONFIG_DEBUG_USB), then + the example code will also manage the USB trace output. The amount of trace output + can be controlled using: + + CONFIG_EXAMPLES_USBSERIAL_TRACEINIT + Show initialization events + CONFIG_EXAMPLES_USBSERIAL_TRACECLASS + Show class driver events + CONFIG_EXAMPLES_USBSERIAL_TRACETRANSFERS + Show data transfer events + CONFIG_EXAMPLES_USBSERIAL_TRACECONTROLLER + Show controller events + CONFIG_EXAMPLES_USBSERIAL_TRACEINTERRUPTS + Show interrupt-related events. + + Error results are always shown in the trace output + + HOST-SIDE TEST PROGRAM + + In additional to the target device-side example, there is also a + host-side application in this directory. This host side application + must be executed on a Linux host in order to perform the USBSERIAL + test. The host application can be compiled under Linux (or Cygwin?) + as follows: + + cd examples/usbserial + make -f Makefile.host TOPDIR=<nuttx-directory> + + RUNNING THE TEST + + This will generate a small program called 'host'. Usage: + + 1. Build the examples/usbserial target program and start the target. + + 2. Wait a bit, then do enter: + + dmesg + + At the end of the dmesg output, you should see the serial + device was successfully idenfied and assigned to a tty device, + probably /dev/ttyUSB0 or /dev/ttyACM0 (depending on the configured + USB serial driver). + + 3. Then start the host application: + + ./host [<tty-dev>] + + Where: + + <tty-dev> is the USB TTY device to use. The default is + "/dev/ttyUSB0" (for the PL2303 emulation) or "/dev/ttyACM0" (for + the CDC/ACM serial device). + + The host and target will exchange are variety of very small and very large + serial messages. + +examples/usbstorage +^^^^^^^^^^^^^^^^^^^ + + This example registers a block device driver, then exports the block + the device using the USB storage class driver. In order to use this + example, your board-specific logic must provide the function: + + void usbmsc_archinitialize(void); + + This function will be called by the example/usbstorage in order to + do the actual registration of the block device drivers. For examples + of the implementation of usbmsc_archinitialize() see + configs/mcu123-lpc124x/src/up_usbmsc.c or + configs/stm3210e-eval/src/usbmsc.c + + Configuration options: + + CONFIG_EXAMPLES_USBMSC_BUILTIN + This example can be built as two NSH "built-in" commands if this option + is selected: 'msconn' will connect the USB mass storage device; 'msdis' + will disconnect the USB storage device. + CONFIG_EXAMPLES_USBMSC_NLUNS + Defines the number of logical units (LUNs) exported by the USB storage + driver. Each LUN corresponds to one exported block driver (or partition + of a block driver). May be 1, 2, or 3. Default is 1. + CONFIG_EXAMPLES_USBMSC_DEVMINOR1 + The minor device number of the block driver for the first LUN. For + example, N in /dev/mmcsdN. Used for registering the block driver. Default + is zero. + CONFIG_EXAMPLES_USBMSC_DEVPATH1 + The full path to the registered block driver. Default is "/dev/mmcsd0" + CONFIG_EXAMPLES_USBMSC_DEVMINOR2 and CONFIG_EXAMPLES_USBMSC_DEVPATH2 + Similar parameters that would have to be provided if CONFIG_EXAMPLES_USBMSC_NLUNS + is 2 or 3. No defaults. + CONFIG_EXAMPLES_USBMSC_DEVMINOR3 and CONFIG_EXAMPLES_USBMSC_DEVPATH3 + Similar parameters that would have to be provided if CONFIG_EXAMPLES_USBMSC_NLUNS + is 3. No defaults. + CONFIG_EXAMPLES_USBMSC_DEBUGMM + Enables some debug tests to check for memory usage and memory leaks. + + If CONFIG_USBDEV_TRACE is enabled (or CONFIG_DEBUG and CONFIG_DEBUG_USB), then + the example code will also manage the USB trace output. The amount of trace output + can be controlled using: + + CONFIG_EXAMPLES_USBMSC_TRACEINIT + Show initialization events + CONFIG_EXAMPLES_USBMSC_TRACECLASS + Show class driver events + CONFIG_EXAMPLES_USBMSC_TRACETRANSFERS + Show data transfer events + CONFIG_EXAMPLES_USBMSC_TRACECONTROLLER + Show controller events + CONFIG_EXAMPLES_USBMSC_TRACEINTERRUPTS + Show interrupt-related events. + + Error results are always shown in the trace output + + NOTE 1: When built as an NSH add-on command (CONFIG_EXAMPLES_USBMSC_BUILTIN=y), + Caution should be used to assure that the SD drive (or other storage device) is + not in use when the USB storage device is configured. Specifically, the SD + driver should be unmounted like: + + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Card is mounted in NSH + ... + nsh> umount /mnd/sdcard # Unmount before connecting USB!!! + nsh> msconn # Connect the USB storage device + ... + nsh> msdis # Disconnect USB storate device + nsh> mount -t vfat /dev/mmcsd0 /mnt/sdcard # Restore the mount + + Failure to do this could result in corruption of the SD card format. + + NOTE 2: This test exercises internal USB device 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/usbterm +^^^^^^^^^^^^^^^^ + + This example implements a little USB terminal.. more of a USB "chat" + edited lines are received from the remote host connected via USB + serial and echoed out the target serial console. Edited lines from + the local target serial console are received and forwarded to the + remote host via USB serial. + + Usage: + - Build the example and load into the target FLASH + - Connect on terminal to the target RS-232 connect and configure + for 115200 8N1. For example, suppose this Tera Term on a Windows + box. + - Power up the target board + - Connect the USB to a Linux box. Use the Linux dmesg command to + assure that the connect was successful. The USB CDC ACM device + should appear as /dev/ttyACM0 + - On the Linux box, open minicom with tty=/dev/ttyACM0. + Configure minicom so that (1) local characters are echoed and (2) + so that no CR is required. + - Now what you type on the target Tera Term window should echo on + the Linux minicom window and, conversely, what you type on the + minicom winow should be echo in the target Tera Term window. + + Configuration options: + + CONFIG_EXAMPLES_USBTERM_BUILTIN - Build the usbterm example as an NSH + built-in command. NOTE: This is not fully functional as of this + writing.. It should work, but there is no mechanism in place yet + to exit the USB terminal program and return to NSH. + CONFIG_EXAMPLES_USBTERM_DEVINIT - If defined, then the example will + call a user provided function as part of its initialization: + int usbterm_devinit(void); + And another user provided function at termination: + void usbterm_devuninit(void); + CONFIG_EXAMPLES_USBTERM_BUFLEN - The size of the input and output + buffers used for receiving data. Default 256 bytes. + + If CONFIG_USBDEV_TRACE is enabled (or CONFIG_DEBUG and CONFIG_DEBUG_USB, or + CONFIG_USBDEV_TRACE), then the example code will also manage the USB trace + output. The amount of trace output can be controlled using: + + CONFIG_EXAMPLES_USBTERM_TRACEINIT + Show initialization events + CONFIG_EXAMPLES_USBTERM_TRACECLASS + Show class driver events + CONFIG_EXAMPLES_USBTERM_TRACETRANSFERS + Show data transfer events + CONFIG_EXAMPLES_USBTERM_TRACECONTROLLER + Show controller events + CONFIG_EXAMPLES_USBTERM_TRACEINTERRUPTS + Show interrupt-related events. + + NOTE: By default, USBterm uses readline to get data from stdin. So your + appconfig file must have the following build path: + + CONFIGURED_APPS += system/readline + + NOTE: If you use the USBterm task over a telnet NSH connection, then you + should set the following configuration item: + + CONFIG_EXAMPLES_USBTERM_FGETS=y + + By default, the USBterm client will use readline() to get characters from + the console. Readline includes and command-line editor and echos + characters received in stdin back through stdout. Neither of these + behaviors are desire-able if Telnet is used. + + Error results are always shown in the trace output + + Other relevant configuration options: CONFIG_CDCACM selected by the + Prolifics emulation (not defined) and the CDC serial implementation + (when defined). CONFIG_USBDEV_TRACE_INITIALIDSET. + +examples/watchdog +^^^^^^^^^^^^^^^^^ + + A simple test of a watchdog timer driver. Initializes starts the watchdog + timer. It pings the watchdog timer for a period of time then lets the + watchdog timer expire... resetting the CPU is successful. This + example can ONLY be built as an NSH built-in function. + + This test depends on these specific Watchdog/NSH configurations settings (your + specific watchdog hardware settings might require additional settings). + + CONFIG_WATCHDOG- Enables watchdog timer support support. + CONFIG_NSH_BUILTIN_APPS - Build the watchdog time test as an NSH + built-in function. Default: Not built! The example can only be used + as an NSH built-in application + + Specific configuration options for this example include: + + CONFIG_EXAMPLES_WATCHDOG_DEVPATH - The path to the Watchdog device. + Default: /dev/watchdog0 + CONFIG_EXAMPLES_WATCHDOG_PINGTIME - Time in milliseconds that the example + will ping the watchdog before letting the watchdog expire. Default: 5000 + milliseconds + CONFIG_EXAMPLES_WATCHDOG_PINGDELAY - Time delay between pings in + milliseconds. Default: 500 milliseconds. + CONFIG_EXAMPLES_WATCHDOG_TIMEOUT - The watchdog timeout value in + milliseconds before the watchdog timer expires. Default: 2000 + milliseconds. + +examples/wget +^^^^^^^^^^^^^ + + A simple web client example. It will obtain a file from a server using the HTTP + protocol. Settings unique to this example include: + + CONFIG_EXAMPLE_WGET_URL - The URL of the file to get + CONFIG_EXAMPLE_WGET_NOMAC - (May be defined to use software assigned MAC) + CONFIG_EXAMPLE_WGET_IPADDR - Target IP address + CONFIG_EXAMPLE_WGET_DRIPADDR - Default router IP addess + CONFIG_EXAMPLE_WGET_NETMASK - Network mask + + This example uses netutils/webclient. Additional configuration settings apply + to that code as follows (but built-in defaults are probably OK): + + CONFIG_WEBCLIENT_GETMIMETYPE, CONFIG_WEBCLIENT_MAXHTTPLINE, + CONFIG_WEBCLIENT_MAXMIMESIZE, CONFIG_WEBCLIENT_MAXHOSTNAME, + CONFIG_WEBCLIENT_MAXFILENAME + + Of course, the example also requires other settings including CONFIG_NET and + CONFIG_NET_TCP. The example also uses the uIP resolver which requires CONFIG_UDP. + + WARNNG: As of this writing, wget is untested on the target platform. At present + it has been tested only in the host-based configuration described in the following + note. The primary difference is that the target version will rely on the also + untested uIP name resolver. + + NOTE: For test purposes, this example can be built as a host-based wget function. + This can be built as follows: + + cd examples/wget + make -f Makefile.host + + Applications using this example will need to provide an appconfig + file in the configuration driver with instruction to build applications + like: + + CONFIGURED_APPS += uiplib + CONFIGURED_APPS += resolv + CONFIGURED_APPS += webclient + + diff --git a/apps/examples/adc/Kconfig b/apps/examples/adc/Kconfig new file mode 100644 index 000000000..b6dca047c --- /dev/null +++ b/apps/examples/adc/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_ADC + bool "ADC example" + default n + ---help--- + Enable the ADC example + +if EXAMPLES_ADC +endif diff --git a/apps/examples/adc/Makefile b/apps/examples/adc/Makefile new file mode 100644 index 000000000..6357dfc3d --- /dev/null +++ b/apps/examples/adc/Makefile @@ -0,0 +1,105 @@ +############################################################################ +# apps/examples/adc/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# NuttX NX Graphics Example. + +ASRCS = +CSRCS = adc_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Touchscreen built-in application info + +APPNAME = adc +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: context clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: +ifeq ($(CONFIG_NSH_BUILTIN_APPS),y) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +endif + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/adc/adc.h b/apps/examples/adc/adc.h new file mode 100644 index 000000000..9f79db92a --- /dev/null +++ b/apps/examples/adc/adc.h @@ -0,0 +1,125 @@ +/**************************************************************************** + * examples/examples/adc/adc.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_EXAMPLES_ADC_ADC_H +#define __APPS_EXAMPLES_ADC_ADC_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* CONFIG_NSH_BUILTIN_APPS - Build the ADC test as an NSH built-in function. + * Default: Built as a standalone problem + * CONFIG_EXAMPLES_ADC_DEVPATH - The default path to the ADC device. Default: /dev/adc0 + * CONFIG_EXAMPLES_ADC_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_ADC_GROUPSIZE - The number of samples to read at once. + * Default: 4 + */ + +#ifndef CONFIG_ADC +# error "ADC device support is not enabled (CONFIG_ADC)" +#endif + +#ifndef CONFIG_EXAMPLES_ADC_DEVPATH +# define CONFIG_EXAMPLES_ADC_DEVPATH "/dev/adc0" +#endif + +#ifndef CONFIG_EXAMPLES_ADC_GROUPSIZE +# define CONFIG_EXAMPLES_ADC_GROUPSIZE 4 +#endif + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct adc_state_s +{ + bool initialized; + FAR char *devpath; +#if defined(CONFIG_NSH_BUILTIN_APPS) || defined(CONFIG_EXAMPLES_ADC_NSAMPLES) + int count; +#endif +}; + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: adc_devinit() + * + * Description: + * Perform architecuture-specific initialization of the ADC hardware. This + * interface must be provided by all configurations using apps/examples/adc + * + ****************************************************************************/ + +int adc_devinit(void); + +#endif /* __APPS_EXAMPLES_ADC_ADC_H */ diff --git a/apps/examples/adc/adc_main.c b/apps/examples/adc/adc_main.c new file mode 100644 index 000000000..b88032654 --- /dev/null +++ b/apps/examples/adc/adc_main.c @@ -0,0 +1,367 @@ +/**************************************************************************** + * examples/adc/adc_main.c + * + * 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 + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/analog/adc.h> + +#include "adc.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +# define MAIN_NAME adc_main +# define MAIN_STRING "adc_main: " +#else +# define MAIN_NAME user_start +# define MAIN_STRING "user_start: " +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct adc_state_s g_adcstate; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: adc_devpath + ****************************************************************************/ + +static void adc_devpath(FAR struct adc_state_s *adc, FAR const char *devpath) +{ + /* Get rid of any old device path */ + + if (adc->devpath) + { + free(adc->devpath); + } + + /* Then set-up the new device path by copying the string */ + + adc->devpath = strdup(devpath); +} + +/**************************************************************************** + * Name: adc_help + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static void adc_help(FAR struct adc_state_s *adc) +{ + message("Usage: adc [OPTIONS]\n"); + message("\nArguments are \"sticky\". For example, once the ADC device is\n"); + message("specified, that device will be re-used until it is changed.\n"); + message("\n\"sticky\" OPTIONS include:\n"); + message(" [-p devpath] selects the ADC device. " + "Default: %s Current: %s\n", + CONFIG_EXAMPLES_ADC_DEVPATH, g_adcstate.devpath ? g_adcstate.devpath : "NONE"); + message(" [-n count] selects the samples to collect. " + "Default: 1 Current: %d\n", adc->count); + message(" [-h] shows this message and exits\n"); +} +#endif + +/**************************************************************************** + * Name: arg_string + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static int arg_string(FAR char **arg, FAR char **value) +{ + FAR char *ptr = *arg; + + if (ptr[2] == '\0') + { + *value = arg[1]; + return 2; + } + else + { + *value = &ptr[2]; + return 1; + } +} +#endif + +/**************************************************************************** + * Name: arg_decimal + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static int arg_decimal(FAR char **arg, FAR long *value) +{ + FAR char *string; + int ret; + + ret = arg_string(arg, &string); + *value = strtol(string, NULL, 10); + return ret; +} +#endif + +/**************************************************************************** + * Name: parse_args + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static void parse_args(FAR struct adc_state_s *adc, int argc, FAR char **argv) +{ + FAR char *ptr; + FAR char *str; + long value; + int index; + int nargs; + + for (index = 1; index < argc; ) + { + ptr = argv[index]; + if (ptr[0] != '-') + { + message("Invalid options format: %s\n", ptr); + exit(0); + } + + switch (ptr[1]) + { + case 'n': + nargs = arg_decimal(&argv[index], &value); + if (value < 0) + { + message("Count must be non-negative: %ld\n", value); + exit(1); + } + + adc->count = (uint32_t)value; + index += nargs; + break; + + case 'p': + nargs = arg_string(&argv[index], &str); + adc_devpath(adc, str); + index += nargs; + break; + + case 'h': + adc_help(adc); + exit(0); + + default: + message("Unsupported option: %s\n", ptr); + adc_help(adc); + exit(1); + } + } +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start/adc_main + ****************************************************************************/ + +int MAIN_NAME(int argc, char *argv[]) +{ + struct adc_msg_s sample[CONFIG_EXAMPLES_ADC_GROUPSIZE]; + size_t readsize; + ssize_t nbytes; + int fd; + int errval = 0; + int ret; + int i; + + /* Check if we have initialized */ + + if (!g_adcstate.initialized) + { + /* Initialization of the ADC hardware is performed by logic external to + * this test. + */ + + message(MAIN_STRING "Initializing external ADC device\n"); + ret = adc_devinit(); + if (ret != OK) + { + message(MAIN_STRING "adc_devinit failed: %d\n", ret); + errval = 1; + goto errout; + } + + /* Set the default values */ + + adc_devpath(&g_adcstate, CONFIG_EXAMPLES_ADC_DEVPATH); + +#ifdef CONFIG_EXAMPLES_ADC_NSAMPLES + g_adcstate.count = CONFIG_EXAMPLES_ADC_NSAMPLES; +#else + g_adcstate.count = 1; +#endif + g_adcstate.initialized = true; + } + + /* Parse the command line */ + +#ifdef CONFIG_NSH_BUILTIN_APPS + parse_args(&g_adcstate, argc, argv); +#endif + + /* If this example is configured as an NX add-on, then limit the number of + * samples that we collect before returning. Otherwise, we never return + */ + +#if defined(CONFIG_NSH_BUILTIN_APPS) || defined(CONFIG_EXAMPLES_ADC_NSAMPLES) + message(MAIN_STRING "g_adcstate.count: %d\n", g_adcstate.count); +#endif + + /* Open the ADC device for reading */ + + message(MAIN_STRING "Hardware initialized. Opening the ADC device: %s\n", + g_adcstate.devpath); + + fd = open(g_adcstate.devpath, O_RDONLY); + if (fd < 0) + { + message(MAIN_STRING "open %s failed: %d\n", g_adcstate.devpath, errno); + errval = 2; + goto errout_with_dev; + } + + /* Now loop the appropriate number of times, displaying the collected + * ADC samples. + */ + +#if defined(CONFIG_NSH_BUILTIN_APPS) + for (; g_adcstate.count > 0; g_adcstate.count--) +#elif defined(CONFIG_EXAMPLES_ADC_NSAMPLES) + for (g_adcstate.count = 0; g_adcstate.count < CONFIG_EXAMPLES_ADC_NSAMPLES; g_adcstate.count++) +#else + for (;;) +#endif + { + /* Flush any output before the loop entered or from the previous pass + * through the loop. + */ + + msgflush(); + + /* Read CONFIG_EXAMPLES_ADC_GROUPSIZE samples */ + + readsize = CONFIG_EXAMPLES_ADC_GROUPSIZE * sizeof(struct adc_msg_s); + nbytes = read(fd, sample, readsize); + + /* Handle unexpected return values */ + + if (nbytes < 0) + { + errval = errno; + if (errval != EINTR) + { + message(MAIN_STRING "read %s failed: %d\n", + g_adcstate.devpath, errval); + errval = 3; + goto errout_with_dev; + } + + message(MAIN_STRING "Interrupted read...\n"); + } + else if (nbytes == 0) + { + message(MAIN_STRING "No data read, Ignoring\n"); + } + + /* Print the sample data on successful return */ + + else + { + int nsamples = nbytes / sizeof(struct adc_msg_s); + if (nsamples * sizeof(struct adc_msg_s) != nbytes) + { + message(MAIN_STRING "read size=%d is not a multiple of sample size=%d, Ignoring\n", + nbytes, sizeof(struct adc_msg_s)); + } + else + { + message("Sample:\n"); + for (i = 0; i < nsamples ; i++) + { + message("%d: channel: %d value: %d\n", + i, sample[i].am_channel, sample[i].am_data); + } + } + } + } + +errout_with_dev: + close(fd); + +errout: + message("Terminating!\n"); + msgflush(); + return errval; +} diff --git a/apps/examples/buttons/Kconfig b/apps/examples/buttons/Kconfig new file mode 100644 index 000000000..9c34b37bc --- /dev/null +++ b/apps/examples/buttons/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_BUTTONS + bool "Buttons example" + default n + ---help--- + Enable the buttons example + +if EXAMPLES_BUTTONS +endif diff --git a/apps/examples/buttons/Makefile b/apps/examples/buttons/Makefile new file mode 100644 index 000000000..9c0587199 --- /dev/null +++ b/apps/examples/buttons/Makefile @@ -0,0 +1,105 @@ +############################################################################ +# apps/examples/buttons/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# Hello, World! Example + +ASRCS = +CSRCS = main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Buttons built-in application info + +APPNAME = buttons +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: context clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: +ifeq ($(CONFIG_NSH_BUILTIN_APPS),y) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +endif + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/buttons/main.c b/apps/examples/buttons/main.c new file mode 100644 index 000000000..fe447ca6b --- /dev/null +++ b/apps/examples/buttons/main.c @@ -0,0 +1,509 @@ +/**************************************************************************** + * examples/buttons/main.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * NOTE: This test exercises internal button driver interfaces. As such, it + * 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). + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/arch.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <debug.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ + +#ifndef CONFIG_ARCH_BUTTONS +# error "CONFIG_ARCH_BUTTONS is not defined in the configuration" +#endif + +#ifndef CONFIG_EXAMPLE_BUTTONS_NAME0 +# define CONFIG_EXAMPLE_BUTTONS_NAME0 "BUTTON0" +#endif +#ifndef CONFIG_EXAMPLE_BUTTONS_NAME1 +# define CONFIG_EXAMPLE_BUTTONS_NAME1 "BUTTON1" +#endif +#ifndef CONFIG_EXAMPLE_BUTTONS_NAME2 +# define CONFIG_EXAMPLE_BUTTONS_NAME2 "BUTTON2" +#endif +#ifndef CONFIG_EXAMPLE_BUTTONS_NAME3 +# define CONFIG_EXAMPLE_BUTTONS_NAME3 "BUTTON3" +#endif +#ifndef CONFIG_EXAMPLE_BUTTONS_NAME4 +# define CONFIG_EXAMPLE_BUTTONS_NAME4 "BUTTON4" +#endif +#ifndef CONFIG_EXAMPLE_BUTTONS_NAME5 +# define CONFIG_EXAMPLE_BUTTONS_NAME5 "BUTTON5" +#endif +#ifndef CONFIG_EXAMPLE_BUTTONS_NAME6 +# define CONFIG_EXAMPLE_BUTTONS_NAME6 "BUTTON6" +#endif +#ifndef CONFIG_EXAMPLE_BUTTONS_NAME7 +# define CONFIG_EXAMPLE_BUTTONS_NAME7 "BUTTON7" +#endif + +#define BUTTON_MIN 0 +#define BUTTON_MAX 7 + +#ifndef CONFIG_EXAMPLE_BUTTONS_MIN +# define CONFIG_EXAMPLE_BUTTONS_MIN BUTTON_MIN +#endif +#ifndef CONFIG_EXAMPLE_BUTTONS_MAX +# define CONFIG_EXAMPLE_BUTTONS_MAX BUTTON_MAX +#endif + +#if CONFIG_EXAMPLE_BUTTONS_MIN > CONFIG_EXAMPLE_BUTTONS_MAX +# error "CONFIG_EXAMPLE_BUTTONS_MIN > CONFIG_EXAMPLE_BUTTONS_MAX" +#endif +#if CONFIG_EXAMPLE_BUTTONS_MAX > 7 +# error "CONFIG_EXAMPLE_BUTTONS_MAX > 7" +#endif + +#ifndef CONFIG_EXAMPLE_IRQBUTTONS_MIN +# define CONFIG_EXAMPLE_IRQBUTTONS_MIN CONFIG_EXAMPLE_BUTTONS_MIN +#endif +#ifndef CONFIG_EXAMPLE_IRQBUTTONS_MAX +# define CONFIG_EXAMPLE_IRQBUTTONS_MAX CONFIG_EXAMPLE_BUTTONS_MAX +#endif + +#if CONFIG_EXAMPLE_IRQBUTTONS_MIN > CONFIG_EXAMPLE_IRQBUTTONS_MAX +# error "CONFIG_EXAMPLE_IRQBUTTONS_MIN > CONFIG_EXAMPLE_IRQBUTTONS_MAX" +#endif +#if CONFIG_EXAMPLE_IRQBUTTONS_MAX > 7 +# error "CONFIG_EXAMPLE_IRQBUTTONS_MAX > 7" +#endif + +#ifndef MIN +# define MIN(a,b) (a < b ? a : b) +#endif +#ifndef MAX +# define MAX(a,b) (a > b ? a : b) +#endif + +#define MIN_BUTTON MIN(CONFIG_EXAMPLE_BUTTONS_MIN, CONFIG_EXAMPLE_IRQBUTTONS_MIN) +#define MAX_BUTTON MAX(CONFIG_EXAMPLE_BUTTONS_MAX, CONFIG_EXAMPLE_IRQBUTTONS_MAX) + +#define NUM_BUTTONS (MAX_BUTTON - MIN_BUTTON + 1) +#define BUTTON_INDEX(b) ((b)-MIN_BUTTON) + +/* Is this being built as an NSH built-in application? */ + +#ifdef CONFIG_NSH_BUILTIN_APPS +# define MAIN_NAME buttons_main +# define MAIN_STRING "buttons_main: " +#else +# define MAIN_NAME user_start +# define MAIN_STRING "user_start: " +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct button_info_s +{ + FAR const char *name; /* Name for the button */ +#ifdef CONFIG_ARCH_IRQBUTTONS + xcpt_t handler; /* Button interrupt handler */ +#endif +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static void show_buttons(uint8_t oldset, uint8_t newset); + +#ifdef CONFIG_ARCH_IRQBUTTONS +static void button_handler(int id, int irq); + +#if MIN_BUTTON < 1 +static int button0_handler(int irq, FAR void *context); +#endif +#if MIN_BUTTON < 2 && MAX_BUTTON > 0 +static int button1_handler(int irq, FAR void *context); +#endif +#if MIN_BUTTON < 3 && MAX_BUTTON > 1 +static int button2_handler(int irq, FAR void *context); +#endif +#if MIN_BUTTON < 4 && MAX_BUTTON > 2 +static int button3_handler(int irq, FAR void *context); +#endif +#if MIN_BUTTON < 5 && MAX_BUTTON > 3 +static int button4_handler(int irq, FAR void *context); +#endif +#if MIN_BUTTON < 6 && MAX_BUTTON > 4 +static int button5_handler(int irq, FAR void *context); +#endif +#if MIN_BUTTON < 7 && MAX_BUTTON > 5 +static int button6_handler(int irq, FAR void *context); +#endif +#if MAX_BUTTON > 6 +static int button7_handler(int irq, FAR void *context); +#endif +#endif /* CONFIG_ARCH_IRQBUTTONS */ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + + /* Button Names */ + +static const struct button_info_s g_buttoninfo[NUM_BUTTONS] = +{ +#if MIN_BUTTON < 1 + { + CONFIG_EXAMPLE_BUTTONS_NAME0, +#ifdef CONFIG_ARCH_IRQBUTTONS + button0_handler +#endif + }, +#endif +#if MIN_BUTTON < 2 && MAX_BUTTON > 0 + { + CONFIG_EXAMPLE_BUTTONS_NAME1, +#ifdef CONFIG_ARCH_IRQBUTTONS + button1_handler +#endif + }, +#endif +#if MIN_BUTTON < 3 && MAX_BUTTON > 1 + { + CONFIG_EXAMPLE_BUTTONS_NAME2, +#ifdef CONFIG_ARCH_IRQBUTTONS + button2_handler +#endif + }, +#endif +#if MIN_BUTTON < 4 && MAX_BUTTON > 2 + { + CONFIG_EXAMPLE_BUTTONS_NAME3, +#ifdef CONFIG_ARCH_IRQBUTTONS + button3_handler +#endif + }, +#endif +#if MIN_BUTTON < 5 && MAX_BUTTON > 3 + { + CONFIG_EXAMPLE_BUTTONS_NAME4, +#ifdef CONFIG_ARCH_IRQBUTTONS + button4_handler +#endif + }, +#endif +#if MIN_BUTTON < 6 && MAX_BUTTON > 4 + { + CONFIG_EXAMPLE_BUTTONS_NAME5, +#ifdef CONFIG_ARCH_IRQBUTTONS + button5_handler +#endif + }, +#endif +#if MIN_BUTTON < 7 && MAX_BUTTON > 5 + { + CONFIG_EXAMPLE_BUTTONS_NAME6, +#ifdef CONFIG_ARCH_IRQBUTTONS + button6_handler +#endif + }, +#endif +#if MAX_BUTTON > 6 + { + CONFIG_EXAMPLE_BUTTONS_NAME7, +#ifdef CONFIG_ARCH_IRQBUTTONS + button7_handler +#endif + } +#endif +}; + +/* Last sampled button set */ + +static uint8_t g_oldset; + +/* Used to limit the number of button presses */ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static volatile long g_nbuttons; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void show_buttons(uint8_t oldset, uint8_t newset) +{ + uint8_t chgset = oldset ^ newset; + int i; + + /* Update the count of button presses shown */ + +#ifdef CONFIG_NSH_BUILTIN_APPS + if ((chgset & newset) != 0) + { + g_nbuttons++; + } +#endif + + /* Show each button state change */ + + for (i = MIN_BUTTON; i <= MAX_BUTTON; i++) + { + uint8_t mask = (1 << i); + if ((chgset & mask) != 0) + { + FAR const char *state; + + /* Get the button state */ + + if ((newset & mask) != 0) + { + state = "depressed"; + } + else + { + state = "released"; + } + + /* Use lib_lowprintf() because we make be executing from an + * interrupt handler. + */ + + lib_lowprintf(" %s %s\n", g_buttoninfo[BUTTON_INDEX(i)].name, state); + } + } +} + +#ifdef CONFIG_ARCH_IRQBUTTONS +static void button_handler(int id, int irq) +{ + uint8_t newset = up_buttons(); + + lib_lowprintf("IRQ:%d Button %d:%s SET:%02x:\n", + irq, id, g_buttoninfo[BUTTON_INDEX(id)].name, newset); + show_buttons(g_oldset, newset); + g_oldset = newset; +} + +#if MIN_BUTTON < 1 +static int button0_handler(int irq, FAR void *context) +{ + button_handler(0, irq); + return OK; +} +#endif + +#if MIN_BUTTON < 2 && MAX_BUTTON > 0 +static int button1_handler(int irq, FAR void *context) +{ + button_handler(1, irq); + return OK; +} +#endif + +#if MIN_BUTTON < 3 && MAX_BUTTON > 1 +static int button2_handler(int irq, FAR void *context) +{ + button_handler(2, irq); + return OK; +} +#endif + +#if MIN_BUTTON < 4 && MAX_BUTTON > 2 +static int button3_handler(int irq, FAR void *context) +{ + button_handler(3, irq); + return OK; +} +#endif + +#if MIN_BUTTON < 5 && MAX_BUTTON > 3 +static int button4_handler(int irq, FAR void *context) +{ + button_handler(4, irq); + return OK; +} +#endif + +#if MIN_BUTTON < 6 && MAX_BUTTON > 4 +static int button5_handler(int irq, FAR void *context) +{ + button_handler(5, irq); + return OK; +} +#endif + +#if MIN_BUTTON < 7 && MAX_BUTTON > 5 +static int button6_handler(int irq, FAR void *context) +{ + button_handler(6, irq); + return OK; +} +#endif + +#if MAX_BUTTON > 6 +static int button7_handler(int irq, FAR void *context) +{ + button_handler(7, irq); + return OK; +} +#endif +#endif /* CONFIG_ARCH_IRQBUTTONS */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * user_start/buttons_main + ****************************************************************************/ + +int MAIN_NAME(int argc, char *argv[]) +{ + uint8_t newset; + irqstate_t flags; + int i; + + /* If this example is configured as an NX add-on, then limit the number of + * samples that we collect before returning. Otherwise, we never return + */ + +#ifdef CONFIG_NSH_BUILTIN_APPS + long maxbuttons = 1; + g_nbuttons = 0; + if (argc > 1) + { + maxbuttons = strtol(argv[1], NULL, 10); + } + lib_lowprintf("maxbuttons: %d\n", maxbuttons); +#endif + + /* Initialize the button GPIOs */ + + up_buttoninit(); + + /* Register to recieve button interrupts */ + +#ifdef CONFIG_ARCH_IRQBUTTONS + for (i = CONFIG_EXAMPLE_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLE_IRQBUTTONS_MAX; i++) + { + xcpt_t oldhandler = up_irqbutton(i, g_buttoninfo[BUTTON_INDEX(i)].handler); + + /* Use lib_lowprintf() for compatibility with interrrupt handler output. */ + + lib_lowprintf("Attached handler at %p to button %d [%s], oldhandler:%p\n", + g_buttoninfo[BUTTON_INDEX(i)].handler, i, + g_buttoninfo[BUTTON_INDEX(i)].name, oldhandler); + + /* Some hardware multiplexes different GPIO button sources to the same + * physical interrupt. If we register multiple such multiplexed button + * interrupts, then the second registration will overwrite the first. In + * this case, the first button interrupts may be aliased to the second + * interrupt handler (or worse, could be lost). + */ + + if (oldhandler != NULL) + { + lib_lowprintf("WARNING: oldhandler:%p is not NULL! " + "Button events may be lost or aliased!\n", + oldhandler); + } + } +#endif + + /* Poll button state */ + + g_oldset = up_buttons(); +#ifdef CONFIG_NSH_BUILTIN_APPS + while (g_nbuttons < maxbuttons) +#else + for (;;) +#endif + { + /* Get the set of pressed and release buttons. */ + + newset = up_buttons(); + + /* Any changes from the last sample? */ + + if (newset != g_oldset) + { + /* Disable interrupts so that output here will not collide with + * output from an interrupt handler. + */ + + flags = irqsave(); + + /* Use lib_lowprintf() for compatibility with interrrupt handler + * output. + */ + + lib_lowprintf("POLL SET:%02x:\n", newset); + show_buttons(g_oldset, newset); + g_oldset = newset; + irqrestore(flags); + } + + /* Sleep a little... but not long. This will determine how fast we + * poll for button changes. + */ + + usleep(150000); /* 150 Milliseconds */ + } + + /* Un-register button handlers */ + +#if defined(CONFIG_ARCH_IRQBUTTONS) && defined(CONFIG_NSH_BUILTIN_APPS) + for (i = CONFIG_EXAMPLE_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLE_IRQBUTTONS_MAX; i++) + { + (void)up_irqbutton(i, NULL); + } +#endif + + return 0; +} + diff --git a/apps/examples/can/Kconfig b/apps/examples/can/Kconfig new file mode 100644 index 000000000..2b4504d68 --- /dev/null +++ b/apps/examples/can/Kconfig @@ -0,0 +1,14 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_CAN + bool "CAN example" + default n + ---help--- + Enable the CAN example + +if EXAMPLES_CAN +endif + diff --git a/apps/examples/can/Makefile b/apps/examples/can/Makefile new file mode 100644 index 000000000..c6dc5af84 --- /dev/null +++ b/apps/examples/can/Makefile @@ -0,0 +1,105 @@ +############################################################################ +# apps/examples/can/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# NuttX NX Graphics Example. + +ASRCS = +CSRCS = can_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Touchscreen built-in application info + +APPNAME = can +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: context clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: +ifeq ($(CONFIG_NSH_BUILTIN_APPS),y) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +endif + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/can/can.h b/apps/examples/can/can.h new file mode 100644 index 000000000..53a6b63ea --- /dev/null +++ b/apps/examples/can/can.h @@ -0,0 +1,131 @@ +/**************************************************************************** + * examples/examples/can/can.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_EXAMPLES_CAN_CAN_H +#define __APPS_EXAMPLES_CAN_CAN_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* This test depends on these specific CAN configurations settings (your + * specific CAN settings might require additional settings). + * + * CONFIG_CAN - Enables CAN support. + * CONFIG_CAN_LOOPBACK - A CAN driver may or may not support a loopback + * mode for testing. The STM32 CAN driver does support loopback mode. + * + * Specific configuration options for this example include: + * + * CONFIG_NSH_BUILTIN_APPS - Build the CAN test as an NSH built-in function. + * Default: Built as a standalone problem + * CONFIG_CAN_LOOPBACK + * CONFIG_EXAMPLES_CAN_DEVPATH - The path to the CAN device. Default: /dev/can0 + * CONFIG_EXAMPLES_CAN_NMSGS - If CONFIG_NSH_BUILTIN_APPS + * is defined, then the number of loops is provided on the command line + * and this value is ignored. Otherwise, this number of CAN message is + * collected and the program terminates. Default: If built as an NSH + * built-in, the default is 32. Otherwise messages are sent and received + * indefinitely. + * CONFIG_EXAMPLES_CAN_READONLY - Only receive messages + * CONFIG_EXAMPLES_CAN_WRITEONLY - Only send messages + */ + +#ifndef CONFIG_CAN +# error "CAN device support is not enabled (CONFIG_CAN)" +#endif + +#ifndef CONFIG_CAN_LOOPBACK +# warning "CAN loopback is not enabled (CONFIG_CAN_LOOPBACK)" +#endif + +#ifndef CONFIG_EXAMPLES_CAN_DEVPATH +# define CONFIG_EXAMPLES_CAN_DEVPATH "/dev/can0" +#endif + +#if defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_EXAMPLES_CAN_NMSGS) +# define CONFIG_EXAMPLES_CAN_NMSGS 32 +#endif + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: can_devinit() + * + * Description: + * Perform architecuture-specific initialization of the CAN hardware. This + * interface must be provided by all configurations using apps/examples/can + * + ****************************************************************************/ + +int can_devinit(void); + +#endif /* __APPS_EXAMPLES_CAN_CAN_H */ diff --git a/apps/examples/can/can_main.c b/apps/examples/can/can_main.c new file mode 100644 index 000000000..0ea729e5e --- /dev/null +++ b/apps/examples/can/can_main.c @@ -0,0 +1,309 @@ +/**************************************************************************** + * examples/can/can_main.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/can.h> + +#include "can.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#if defined(CONFIG_EXAMPLES_CAN_READONLY) +# undef CONFIG_EXAMPLES_CAN_WRITEONLY +# undef CONFIG_EXAMPLES_CAN_READWRITE +# define CAN_OFLAGS O_RDONLY +#elif defined(CONFIG_EXAMPLES_CAN_WRITEONLY) +# undef CONFIG_EXAMPLES_CAN_READWRITE +# define CAN_OFLAGS O_WRONLY +#else +# undef CONFIG_EXAMPLES_CAN_READWRITE +# define CONFIG_EXAMPLES_CAN_READWRITE 1 +# define CAN_OFLAGS O_RDWR +#endif + +#ifdef CONFIG_CAN_EXTID +# define MAX_ID (1 << 29) +#else +# define MAX_ID (1 << 11) +#endif + +#ifdef CONFIG_NSH_BUILTIN_APPS +# define MAIN_NAME can_main +# define MAIN_STRING "can_main: " +#else +# define MAIN_NAME user_start +# define MAIN_STRING "user_start: " +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start/can_main + ****************************************************************************/ + +int MAIN_NAME(int argc, char *argv[]) +{ +#ifndef CONFIG_EXAMPLES_CAN_READONLY + struct can_msg_s txmsg; +#ifdef CONFIG_CAN_EXTID + uint32_t msgid; +#else + uint16_t msgid; +#endif + int msgdlc; + uint8_t msgdata; +#endif + +#ifndef CONFIG_EXAMPLES_CAN_WRITEONLY + struct can_msg_s rxmsg; +#endif + + size_t msgsize; + ssize_t nbytes; +#if defined(CONFIG_NSH_BUILTIN_APPS) || defined(CONFIG_EXAMPLES_CAN_NMSGS) + long nmsgs; +#endif + + int fd; + int errval = 0; + int ret; + int i; + + /* If this example is configured as an NX add-on, then limit the number of + * samples that we collect before returning. Otherwise, we never return + */ + +#if defined(CONFIG_NSH_BUILTIN_APPS) + nmsgs = CONFIG_EXAMPLES_CAN_NMSGS; + if (argc > 1) + { + nmsgs = strtol(argv[1], NULL, 10); + } + message(MAIN_STRING "nmsgs: %d\n", nmsgs); +#elif defined(CONFIG_EXAMPLES_CAN_NMSGS) + message(MAIN_STRING "nmsgs: %d\n", CONFIG_EXAMPLES_CAN_NMSGS); +#endif + + /* Initialization of the CAN hardware is performed by logic external to + * this test. + */ + + message(MAIN_STRING "Initializing external CAN device\n"); + ret = can_devinit(); + if (ret != OK) + { + message(MAIN_STRING "can_devinit failed: %d\n", ret); + errval = 1; + goto errout; + } + + /* Open the CAN device for reading */ + + message(MAIN_STRING "Hardware initialized. Opening the CAN device\n"); + fd = open(CONFIG_EXAMPLES_CAN_DEVPATH, CAN_OFLAGS); + if (fd < 0) + { + message(MAIN_STRING "open %s failed: %d\n", + CONFIG_EXAMPLES_CAN_DEVPATH, errno); + errval = 2; + goto errout_with_dev; + } + + /* Now loop the appropriate number of times, performing one loopback test + * on each pass. + */ + +#ifndef CONFIG_EXAMPLES_CAN_READONLY + msgdlc = 1; + msgid = 1; + msgdata = 0; +#endif + +#if defined(CONFIG_NSH_BUILTIN_APPS) + for (; nmsgs > 0; nmsgs--) +#elif defined(CONFIG_EXAMPLES_CAN_NMSGS) + for (nmsgs = 0; nmsgs < CONFIG_EXAMPLES_CAN_NMSGS; nmsgs++) +#else + for (;;) +#endif + { + /* Flush any output before the loop entered or from the previous pass + * through the loop. + */ + + msgflush(); + + /* Construct the next TX message */ + +#ifndef CONFIG_EXAMPLES_CAN_READONLY + txmsg.cm_hdr.ch_id = msgid; + txmsg.cm_hdr.ch_rtr = false; + txmsg.cm_hdr.ch_dlc = msgdlc; +#ifdef CONFIG_CAN_EXTID + txmsg.cm_hdr.ch_extid = true; +#endif + + for (i = 0; i < msgdlc; i++) + { + txmsg.cm_data[i] = msgdata + i; + } + + /* Send the TX message */ + + msgsize = CAN_MSGLEN(msgdlc); + nbytes = write(fd, &txmsg, msgsize); + if (nbytes != msgsize) + { + message("ERROR: write(%d) returned %d\n", msgsize, nbytes); + errval = 3; + goto errout_with_dev; + } +#endif + +#ifdef CONFIG_EXAMPLES_CAN_WRITEONLY + message(" ID: %4d DLC: %d\n", msgid, msgdlc); +#endif + + /* Read the RX message */ + +#ifndef CONFIG_EXAMPLES_CAN_WRITEONLY + msgsize = sizeof(struct can_msg_s); + nbytes = read(fd, &rxmsg, msgsize); + if (nbytes < CAN_MSGLEN(0) || nbytes > msgsize) + { + message("ERROR: read(%d) returned %d\n", msgsize, nbytes); + errval = 4; + goto errout_with_dev; + } +#endif + +#ifndef CONFIG_EXAMPLES_CAN_READONLY + message(" ID: %4d DLC: %d\n", rxmsg.cm_hdr.id, rxmsg.cm_hdr.dlc); +#endif + + /* Verify that the received messages are the same */ + +#ifdef CONFIG_EXAMPLES_CAN_READWRITE + if (memcmp(&txmsg.cm_hdr, &rxmsg.cm_hdr, sizeof(struct can_hdr_s)) != 0) + { + message("ERROR: Sent header does not match received header:\n"); + lib_dumpbuffer("Sent header", (FAR const uint8_t*)&txmsg.cm_hdr, + sizeof(struct can_hdr_s)); + lib_dumpbuffer("Received header", (FAR const uint8_t*)&rxmsg.cm_hdr, + sizeof(struct can_hdr_s)); + errval = 4; + goto errout_with_dev; + } + + if (memcmp(txmsg.cm_data, rxmsg.cm_data, msgdlc) != 0) + { + message("ERROR: Data does not match. DLC=%d\n", msgdlc); + for (i = 0; i < msgdlc; i++) + { + message(" %d: TX %02x RX %02x\n", i, txmsg.cm_data[i], rxmsg.cm_data[i]); + errval = 5; + goto errout_with_dev; + } + } + + /* Report success */ + + message(" ID: %4d DLC: %d -- OK\n", msgid, msgdlc); +#endif + + /* Set up for the next pass */ + +#ifndef CONFIG_EXAMPLES_CAN_READONLY + msgdata += msgdlc; + + if (++msgid >= MAX_ID) + { + msgid = 1; + } + + if (++msgdlc > CAN_MAXDATALEN) + { + msgdlc = 1; + } +#endif + } + +errout_with_dev: + close(fd); + +errout: + message("Terminating!\n"); + msgflush(); + return errval; +} diff --git a/apps/examples/cdcacm/.context b/apps/examples/cdcacm/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/examples/cdcacm/.context diff --git a/apps/examples/cdcacm/Kconfig b/apps/examples/cdcacm/Kconfig new file mode 100644 index 000000000..8cd9c6e99 --- /dev/null +++ b/apps/examples/cdcacm/Kconfig @@ -0,0 +1,14 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_CDCACM + bool "CAN example" + default n + ---help--- + Enable the USB CDC/ACM class driver example + +if EXAMPLES_CDCACM +endif + diff --git a/apps/examples/cdcacm/Makefile b/apps/examples/cdcacm/Makefile new file mode 100644 index 000000000..3fa886d56 --- /dev/null +++ b/apps/examples/cdcacm/Makefile @@ -0,0 +1,109 @@ +############################################################################ +# apps/examples/cdcacm/Makefile +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# USB CDC/ACM serial mass storage example + +ASRCS = +CSRCS = cdcacm_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# USB CDC/ACM built-in application info + +APPNAME1 = sercon +PRIORITY1 = SCHED_PRIORITY_DEFAULT +STACKSIZE1 = 2048 + +APPNAME2 = serdis +PRIORITY2 = SCHED_PRIORITY_DEFAULT +STACKSIZE2 = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: context clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: + $(call REGISTER,$(APPNAME1),$(PRIORITY1),$(STACKSIZE1),$(APPNAME1)_main) + $(call REGISTER,$(APPNAME2),$(PRIORITY2),$(STACKSIZE2),$(APPNAME2)_main) + @touch $@ + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep + diff --git a/apps/examples/cdcacm/cdcacm.h b/apps/examples/cdcacm/cdcacm.h new file mode 100644 index 000000000..18570bff0 --- /dev/null +++ b/apps/examples/cdcacm/cdcacm.h @@ -0,0 +1,165 @@ +/**************************************************************************** + * examples/cdcacm/cdcacm.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __EXAMPLES_CDCACM_CDCACM_H +#define __EXAMPLES_CDCACM_CDCACM_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> + +#include <nuttx/usb/usbdev_trace.h> + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* Prerequisites */ + +#ifndef CONFIG_USBDEV +# error "CONFIG_USBDEV is not defined" +#endif + +#ifndef CONFIG_CDCACM +# error "CONFIG_CDCACM is not defined" +#endif + +#ifndef CONFIG_NSH_BUILTIN_APPS +# error "This example can only be built as an NSH built-in application" +#endif + +/* Default configuration values */ + +#ifndef CONFIG_EXAMPLES_CDCACM_DEVMINOR +# define CONFIG_EXAMPLES_CDCACM_DEVMINOR 0 +#endif + +/* Trace Configuration ******************************************************/ + +#ifdef CONFIG_EXAMPLES_CDCACM_TRACEINIT +# define TRACE_INIT_BITS (TRACE_INIT_BIT) +#else +# define TRACE_INIT_BITS (0) +#endif + +#define TRACE_ERROR_BITS (TRACE_DEVERROR_BIT|TRACE_CLSERROR_BIT) + +#ifdef CONFIG_EXAMPLES_CDCACM_TRACECLASS +# define TRACE_CLASS_BITS (TRACE_CLASS_BIT|TRACE_CLASSAPI_BIT|TRACE_CLASSSTATE_BIT) +#else +# define TRACE_CLASS_BITS (0) +#endif + +#ifdef CONFIG_EXAMPLES_CDCACM_TRACETRANSFERS +# define TRACE_TRANSFER_BITS (TRACE_OUTREQQUEUED_BIT|TRACE_INREQQUEUED_BIT|TRACE_READ_BIT|\ + TRACE_WRITE_BIT|TRACE_COMPLETE_BIT) +#else +# define TRACE_TRANSFER_BITS (0) +#endif + +#ifdef CONFIG_EXAMPLES_CDCACM_TRACECONTROLLER +# define TRACE_CONTROLLER_BITS (TRACE_EP_BIT|TRACE_DEV_BIT) +#else +# define TRACE_CONTROLLER_BITS (0) +#endif + +#ifdef CONFIG_EXAMPLES_CDCACM_TRACEINTERRUPTS +# define TRACE_INTERRUPT_BITS (TRACE_INTENTRY_BIT|TRACE_INTDECODE_BIT|TRACE_INTEXIT_BIT) +#else +# define TRACE_INTERRUPT_BITS (0) +#endif + +#define TRACE_BITSET (TRACE_INIT_BITS|TRACE_ERROR_BITS|TRACE_CLASS_BITS|\ + TRACE_TRANSFER_BITS|TRACE_CONTROLLER_BITS|TRACE_INTERRUPT_BITS) + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_lowprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_lowprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* All global variables used by this example are packed into a structure in + * order to avoid name collisions. + */ + +struct cdcacm_state_s +{ + /* This is the handle that references to this particular USB storage driver + * instance. It is only needed if the USB mass storage device example is + * built using CONFIG_NSH_BUILTIN_APPS. In this case, the value + * of the driver handle must be remembered between the 'sercon' and 'msdis' + * commands. + */ + + FAR void *handle; +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* All global variables used by this example are packed into a structure in + * order to avoid name collisions. + */ + +extern struct cdcacm_state_s g_cdcacm; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +#endif /* __EXAMPLES_CDCACM_CDCACM_H */ diff --git a/apps/examples/cdcacm/cdcacm_main.c b/apps/examples/cdcacm/cdcacm_main.c new file mode 100644 index 000000000..aeb7a9e74 --- /dev/null +++ b/apps/examples/cdcacm/cdcacm_main.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * examples/cdcacm/cdcacm_main.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <debug.h> + +#include <nuttx/usb/usbdev.h> +#include <nuttx/usb/cdcacm.h> + +#include "cdcacm.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* All global variables used by this example are packed into a structure in + * order to avoid name collisions. + */ + +struct cdcacm_state_s g_cdcacm; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * sercon_main + * + * Description: + * This is the main program that configures the CDC/ACM serial device. + * + ****************************************************************************/ + +int sercon_main(int argc, char *argv[]) +{ + int ret; + + /* Check if there is a non-NULL USB mass storage device handle (meaning that the + * USB mass storage device is already configured). + */ + + if (g_cdcacm.handle) + { + message("sercon:: ERROR: Already connected\n"); + return EXIT_FAILURE; + } + + /* Then, in any event, enable trace data collection as configured BEFORE + * enabling the CDC/ACM device. + */ + + usbtrace_enable(TRACE_BITSET); + + /* Initialize the USB CDC/ACM serial driver */ + + message("sercon: Registering CDC/ACM serial driver\n"); + ret = cdcacm_initialize(CONFIG_EXAMPLES_CDCACM_DEVMINOR, &g_cdcacm.handle); + if (ret < 0) + { + message("sercon: ERROR: Failed to create the CDC/ACM serial device: %d\n", -ret); + return EXIT_FAILURE; + } + + message("sercon: Successfully registered the CDC/ACM serial driver\n"); + return EXIT_SUCCESS; +} + +/**************************************************************************** + * serdis_main + * + * Description: + * This is a program entry point that will disconnect the CDC/ACM serial + * device. + * + ****************************************************************************/ + +int serdis_main(int argc, char *argv[]) +{ + /* First check if the USB mass storage device is already connected */ + + if (!g_cdcacm.handle) + { + message("serdis: ERROR: Not connected\n"); + return EXIT_FAILURE; + } + + /* Then, in any event, disable trace data collection as configured BEFORE + * enabling the CDC/ACM device. + */ + + usbtrace_enable(0); + + /* Then disconnect the device and uninitialize the USB mass storage driver */ + + cdcacm_uninitialize(g_cdcacm.handle); + g_cdcacm.handle = NULL; + message("serdis: Disconnected\n"); + return EXIT_SUCCESS; +} diff --git a/apps/examples/hello/Kconfig b/apps/examples/hello/Kconfig new file mode 100644 index 000000000..d697daa8a --- /dev/null +++ b/apps/examples/hello/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_HELLO + bool "\"Hello, World!\" example" + default n + ---help--- + Enable the \"Hello, World!\" example + +if EXAMPLES_HELLO +endif diff --git a/apps/examples/hello/Makefile b/apps/examples/hello/Makefile new file mode 100644 index 000000000..9c3cda894 --- /dev/null +++ b/apps/examples/hello/Makefile @@ -0,0 +1,105 @@ +############################################################################ +# apps/examples/hello/Makefile +# +# 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 +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# Hello, World! built-in application info + +APPNAME = hello +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Hello, World! Example + +ASRCS = +CSRCS = main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: +ifeq ($(CONFIG_EXAMPLES_HELLO_BUILTIN),y) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +endif + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/hello/main.c b/apps/examples/hello/main.c new file mode 100644 index 000000000..7934dc34b --- /dev/null +++ b/apps/examples/hello/main.c @@ -0,0 +1,70 @@ +/**************************************************************************** + * examples/hello/main.c + * + * Copyright (C) 2008, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <stdio.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * user_start/hello_main + ****************************************************************************/ + +#ifdef CONFIG_EXAMPLES_HELLO_BUILTIN +# define MAIN_NAME hello_main +#else +# define MAIN_NAME user_start +#endif + +int MAIN_NAME(int argc, char *argv[]) +{ + printf("Hello, World!!\n"); + return 0; +} + diff --git a/apps/examples/helloxx/Kconfig b/apps/examples/helloxx/Kconfig new file mode 100644 index 000000000..336389d24 --- /dev/null +++ b/apps/examples/helloxx/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_HELLOXX + bool "\"Hello, World!\" C++ example" + default n + ---help--- + Enable the \"Hello, World!\" C++ example + +if EXAMPLES_HELLOXX +endif diff --git a/apps/examples/helloxx/Makefile b/apps/examples/helloxx/Makefile new file mode 100644 index 000000000..c34378d24 --- /dev/null +++ b/apps/examples/helloxx/Makefile @@ -0,0 +1,122 @@ +############################################################################ +# apps/examples/helloxx/Makefile +# +# Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# Hello, World! C++ Example + +ASRCS = +CSRCS = +CXXSRCS = main.cxx + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) +CXXOBJS = $(CXXSRCS:.cxx=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) $(CXXSRCS) +OBJS = $(AOBJS) $(COBJS) $(CXXOBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# helloxx built-in application info + +APPNAME = helloxx +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend disclean chkcxx + +chkcxx: +ifneq ($(CONFIG_HAVE_CXX),y) + @echo "" + @echo "In order to use this example, you toolchain must support must" + @echo "" + @echo " (1) Explicitly select CONFIG_HAVE_CXX to build in C++ support" + @echo " (2) Define CXX, CXXFLAGS, and COMPILEXX in the Make.defs file" + @echo " of the configuration that you are using." + @echo "" + @exit 1 +endif + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +$(CXXOBJS): %$(OBJEXT): %.cxx + $(call COMPILEXX, $<, $@) + +.built: chkcxx $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: +ifeq ($(CONFIG_EXAMPLES_HELLOXX_BUILTIN),y) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +endif + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/helloxx/main.cxx b/apps/examples/helloxx/main.cxx new file mode 100644 index 000000000..8514fead2 --- /dev/null +++ b/apps/examples/helloxx/main.cxx @@ -0,0 +1,178 @@ +//*************************************************************************** +// examples/helloxx/main.cxx +// +// Copyright (C) 2009, 2011-2012 Gregory Nutt. All rights reserved. +// Author: Gregory Nutt <gnutt@nuttx.org> +// +// 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 NuttX 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. +// +//*************************************************************************** + +//*************************************************************************** +// Included Files +//*************************************************************************** + +#include <nuttx/config.h> + +#include <cstdio> +#include <debug.h> + +#include <nuttx/init.h> +#include <nuttx/arch.h> + +//*************************************************************************** +// Definitions +//*************************************************************************** +// Debug ******************************************************************** +// Non-standard debug that may be enabled just for testing the constructors + +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_CXX +#endif + +#ifdef CONFIG_DEBUG_CXX +# define cxxdbg dbg +# define cxxlldbg lldbg +# ifdef CONFIG_DEBUG_VERBOSE +# define cxxvdbg vdbg +# define cxxllvdbg llvdbg +# else +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +# endif +#else +# define cxxdbg(x...) +# define cxxlldbg(x...) +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +#endif + +//*************************************************************************** +// Private Classes +//*************************************************************************** + +class CHelloWorld +{ + public: + CHelloWorld(void) : mSecret(42) + { + cxxdbg("Constructor: mSecret=%d\n", mSecret); + } + + ~CHelloWorld(void) + { + cxxdbg("Destructor\n"); + } + + bool HelloWorld(void) + { + cxxdbg("HelloWorld: mSecret=%d\n", mSecret); + + if (mSecret != 42) + { + printf("CHelloWorld::HelloWorld: CONSTRUCTION FAILED!\n"); + return false; + } + else + { + printf("CHelloWorld::HelloWorld: Hello, World!!\n"); + return true; + } + } + + private: + int mSecret; +}; + +//*************************************************************************** +// Private Data +//*************************************************************************** + +// Define a statically constructed CHellowWorld instance if C++ static +// initializers are supported by the platform + +#ifdef CONFIG_HAVE_CXXINITIALIZE +static CHelloWorld g_HelloWorld; +#endif + +//*************************************************************************** +// Public Functions +//*************************************************************************** + +//*************************************************************************** +// user_start +//*************************************************************************** + +/**************************************************************************** + * Name: user_start/nxhello_main + ****************************************************************************/ + +#ifdef CONFIG_EXAMPLES_HELLOXX_BUILTIN +extern "C" int helloxx_main(int argc, char *argv[]); +# define MAIN_NAME helloxx_main +# define MAIN_STRING "helloxx_main: " +#else +# define MAIN_NAME user_start +# define MAIN_STRING "user_start: " +#endif + +int MAIN_NAME(int argc, char *argv[]) +{ + // If C++ initialization for static constructors is supported, then do + // that first + +#ifdef CONFIG_HAVE_CXXINITIALIZE + up_cxxinitialize(); +#endif + + // Exercise an explictly instantiated C++ object + + CHelloWorld *pHelloWorld = new CHelloWorld; + printf(MAIN_STRING "Saying hello from the dynamically constructed instance\n"); + pHelloWorld->HelloWorld(); + + // Exercise an C++ object instantiated on the stack + +#ifndef CONFIG_EXAMPLES_HELLOXX_NOSTACKCONST + CHelloWorld HelloWorld; + + printf(MAIN_STRING "Saying hello from the instance constructed on the stack\n"); + HelloWorld.HelloWorld(); +#endif + + // Exercise an statically constructed C++ object + +#ifdef CONFIG_HAVE_CXXINITIALIZE + printf(MAIN_STRING "Saying hello from the statically constructed instance\n"); + g_HelloWorld.HelloWorld(); +#endif + + delete pHelloWorld; + return 0; +} + diff --git a/apps/examples/lcdrw/Kconfig b/apps/examples/lcdrw/Kconfig new file mode 100644 index 000000000..2308ddc60 --- /dev/null +++ b/apps/examples/lcdrw/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_LCDRW + bool "LCD read/write example" + default n + ---help--- + Enable the LCD read/write example + +if EXAMPLES_LCDRW +endif diff --git a/apps/examples/lcdrw/Makefile b/apps/examples/lcdrw/Makefile new file mode 100644 index 000000000..053c62670 --- /dev/null +++ b/apps/examples/lcdrw/Makefile @@ -0,0 +1,105 @@ +############################################################################ +# apps/examples/lcdrw/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# LCD Read/Write Test + +ASRCS = +CSRCS = lcdrw_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# LCD R/W built-in application info + +APPNAME = lcdrw +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: +ifeq ($(CONFIG_EXAMPLES_LCDRW_BUILTIN),y) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +endif + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/lcdrw/lcdrw_main.c b/apps/examples/lcdrw/lcdrw_main.c new file mode 100644 index 000000000..c366743f4 --- /dev/null +++ b/apps/examples/lcdrw/lcdrw_main.c @@ -0,0 +1,259 @@ +/**************************************************************************** + * examples/lcdrw/lcdrw_main.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> + +#include <nuttx/lcd/lcd.h> +#include <nuttx/nx/nxglib.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* Most of the NX configuration settings are probbably *not* needed by this + * example. But, presumeably you are using NX too and so the checks might + * be good for you. + */ + +#ifndef CONFIG_NX +# error "CONFIG_NX must be defined to use this test" +#endif + +#ifndef CONFIG_NX_LCDDRIVER +# error "CONFIG_NX_LCDDRIVER must be defined to use this test" +#endif + +#ifndef CONFIG_EXAMPLES_LCDRW_BPP +# define CONFIG_EXAMPLES_LCDRW_BPP 16 +#endif + +#if CONFIG_EXAMPLES_LCDRW_BPP != 16 +# error "Currently only RGB565 is supported -- feel free to extend" +#endif + +#ifdef CONFIG_NX_DISABLE_16BPP +# error "CONFIG_NX_DISABLE_16BPP disables 16-bit support" +#endif + +#ifndef CONFIG_EXAMPLES_LDCRW_DEVNO +# define CONFIG_EXAMPLES_LDCRW_DEVNO 0 +#endif + +#ifndef CONFIG_EXAMPLES_LDCRW_XRES +# define CONFIG_EXAMPLES_LDCRW_XRES 240 +#endif + +#ifndef CONFIG_EXAMPLES_LDCRW_YRES +# define CONFIG_EXAMPLES_LDCRW_YRES 320 +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct lcdrw_instance_s +{ + /* LCD device handle and planeinfo */ + + FAR struct lcd_dev_s *dev; + struct lcd_planeinfo_s pinfo; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ +/**************************************************************************** + * Name: lcdrw_initialize + ****************************************************************************/ + +static inline int lcdrw_initialize(FAR struct lcdrw_instance_s *inst) +{ + int ret; + + /* Initialize the LCD device */ + + printf("screens_initialize: Initializing LCD\n"); + ret = up_lcdinitialize(); + if (ret < 0) + { + fprintf(stderr, "screens_initialize: up_lcdinitialize failed: %d\n", -ret); + return ret; + } + + /* Get the device instance. */ + + printf("Get LCD instance\n"); + inst->dev = up_lcdgetdev(CONFIG_EXAMPLES_LDCRW_DEVNO); + if (!inst->dev) + { + fprintf(stderr, "up_lcdgetdev failed, devno=%d\n", CONFIG_EXAMPLES_LDCRW_DEVNO); + return ret; + } + + /* Turn the LCD on at 75% power. This should not be necessary. */ + + (void)inst->dev->setpower(inst->dev, ((3*CONFIG_LCD_MAXPOWER + 3)/4)); + + /* Get the planeinfo structure */ + + ret = inst->dev->getplaneinfo(inst->dev, 0, &inst->pinfo); + if (ret < 0) + { + fprintf(stderr, "getplaneinfo failed: %d\n", ret); + } + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: lcdrw_main/user_start + ****************************************************************************/ + +#ifdef CONFIG_EXAMPLES_LCDRW_BUILTIN +# define MAIN_NAME lcdrw_main +#else +# define MAIN_NAME user_start +#endif + +int MAIN_NAME(int argc, char *argv[]) +{ + struct lcdrw_instance_s inst; + nxgl_coord_t row; + nxgl_coord_t col; + uint16_t value; + uint32_t offset; + FAR uint16_t *ptr; + int ret; + + /* Initialize the LCD driver */ + + ret = lcdrw_initialize(&inst); + if (ret < 0) + { + exit(1); + } + + /* Loop, writing all possible values to the LCD */ + + value = 0; + for (row = 0; row < CONFIG_EXAMPLES_LDCRW_YRES; row++) + { + /* Create a dummy row. The important thing is to try all + * bit combinations in a predictable way. + */ + + ptr = (FAR uint16_t*)inst.pinfo.buffer; + for (col = 0; col < CONFIG_EXAMPLES_LDCRW_XRES; col++) + { + *ptr++ = value++; + } + + /* Write the row to the LCD */ + + ret = inst.pinfo.putrun(row, 0, inst.pinfo.buffer, + CONFIG_EXAMPLES_LDCRW_XRES); + if (ret < 0) + { + fprintf(stderr, "putrun failed: %d\n", ret); + exit(1); + } + } + + /* Print a header */ + + printf(" "); + for (col = 0; col < 15; col++) + { + printf("---%x ", col); + } + printf("---f\n"); + + /* Then read each line back from the LCD. */ + + offset = 0; + for (row = 0; row < CONFIG_EXAMPLES_LDCRW_YRES; row++) + { + /* Read the row */ + + ret = inst.pinfo.getrun(row, 0, inst.pinfo.buffer, + CONFIG_EXAMPLES_LDCRW_XRES); + if (ret < 0) + { + fprintf(stderr, "getrun failed: %d\n", ret); + exit(1); + } + + /* Then dump the row to the display */ + + ptr = (FAR uint16_t*)inst.pinfo.buffer; + for (col = 0; col < CONFIG_EXAMPLES_LDCRW_XRES; col++) + { + if ((offset & 15) == 0) + { + printf("%06x ", offset); + } + + value = *ptr++; + offset++; + + if ((offset & 15) == 0) + { + printf("%04x\n", value); + } + else + { + printf("%04x ", value); + } + } + } + fflush(stdout); + + return 0; +} + diff --git a/apps/examples/mm/Kconfig b/apps/examples/mm/Kconfig new file mode 100644 index 000000000..81ce4c453 --- /dev/null +++ b/apps/examples/mm/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_MM + bool "Memory management example" + default n + ---help--- + Enable the memory management example + +if EXAMPLES_MM +endif diff --git a/apps/examples/mm/Makefile b/apps/examples/mm/Makefile new file mode 100644 index 000000000..e5d9ffb4c --- /dev/null +++ b/apps/examples/mm/Makefile @@ -0,0 +1,93 @@ +############################################################################ +# apps/examples/mm/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# Memory Management Test + +ASRCS = +CSRCS = mm_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/mm/mm_main.c b/apps/examples/mm/mm_main.c new file mode 100644 index 000000000..036c39047 --- /dev/null +++ b/apps/examples/mm/mm_main.c @@ -0,0 +1,300 @@ +/**************************************************************************** + * examples/mm/mm_main.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define NTEST_ALLOCS 32 + +/* #define STOP_ON_ERRORS do{}while(0) */ +#define STOP_ON_ERRORS exit(1) + +/* All other definitions derive from these two */ + +#define MM_MIN_SHIFT 4 /* 16 bytes */ +#define MM_MIN_CHUNK (1 << MM_MIN_SHIFT) +#define MM_GRAN_MASK (MM_MIN_CHUNK-1) +#define MM_ALIGN_UP(a) (((a) + MM_GRAN_MASK) & ~MM_GRAN_MASK) +#define MM_ALIGN_DOWN(a) ((a) & ~MM_GRAN_MASK) + +#ifdef CONFIG_SMALL_MEMORY +# define SIZEOF_MM_ALLOCNODE 4 +#else +# define SIZEOF_MM_ALLOCNODE 8 +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ +/* Test allocations */ + +static const int alloc_sizes[NTEST_ALLOCS] = +{ + 1024, 12, 962, 5692, 10254, 111, 9932, 601, + 222, 2746, 3, 124321, 68, 776, 6750, 852, + 4732, 28, 901, 480, 5011, 1536, 2011, 81647, + 646, 1646, 69179, 194, 2590, 7, 969, 70 +}; + +static const int realloc_sizes[NTEST_ALLOCS] = +{ + 18, 3088, 963, 123, 511, 11666, 3723, 42, + 9374, 1990, 1412, 6, 592, 4088, 11, 5040, + 8663, 91255, 28, 4346, 9172, 168, 229, 4734, + 59139, 221, 7830, 30421, 1666, 4, 812, 416 +}; + +static const int random1[NTEST_ALLOCS] = +{ + 20, 11, 3, 31, 9, 29, 7, 17, + 21, 2, 26, 18, 14, 25, 0, 10, + 27, 19, 22, 28, 8, 30, 12, 15, + 4, 1, 24, 6, 16, 13, 5, 23 +}; + +static const int random2[NTEST_ALLOCS] = +{ + 2, 19, 12, 23, 30, 11, 27, 4, + 20, 7, 0, 16, 28, 15, 5, 24, + 10, 17, 25, 31, 8, 29, 3, 26, + 9, 18, 22, 13, 1, 21, 14, 6 +}; + +static const int random3[NTEST_ALLOCS] = +{ + 8, 17, 3, 18, 26, 23, 30, 11, + 12, 22, 4, 20, 25, 10, 27, 1, + 29, 14, 19, 21, 0, 31, 7, 24, + 9, 15, 2, 28, 16, 6, 13, 5 +}; + +static const int alignment[NTEST_ALLOCS/2] = +{ + 128, 2048, 131072, 8192, 32, 32768, 16384 , 262144, + 512, 4096, 65536, 8, 64, 1024, 16, 4 +}; + +static void *allocs[NTEST_ALLOCS]; +static struct mallinfo alloc_info; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void mm_showmallinfo(void) +{ + alloc_info = mallinfo(); + printf(" mallinfo:\n"); + printf(" Total space allocated from system = %ld\n", + alloc_info.arena); + printf(" Number of non-inuse chunks = %ld\n", + alloc_info.ordblks); + printf(" Largest non-inuse chunk = %ld\n", + alloc_info.mxordblk); + printf(" Total allocated space = %ld\n", + alloc_info.uordblks); + printf(" Total non-inuse space = %ld\n", + alloc_info.fordblks); +} + +static void do_mallocs(void **mem, const int *size, const int *seq, int n) +{ + int i; + int j; + + for (i = 0; i < n; i++) + { + j = seq[i]; + if (!mem[j]) + { + printf("(%d)Allocating %d bytes\n", i, size[j]); + mem[j] = malloc(size[j]); + printf("(%d)Memory allocated at %p\n", i, mem[j]); + if (mem[j] == NULL) + { + int allocsize = MM_ALIGN_UP(size[j] + SIZEOF_MM_ALLOCNODE); + fprintf(stderr, "(%d)malloc failed for allocsize=%d\n", i, allocsize); + if (allocsize > alloc_info.mxordblk) + { + fprintf(stderr, " Normal, largest free block is only %ld\n", alloc_info.mxordblk); + } + else + { + fprintf(stderr, " ERROR largest free block is %ld\n", alloc_info.mxordblk); + exit(1); + } + } + else + { + memset(mem[j], 0xAA, size[j]); + } + + mm_showmallinfo(); + } + } +} + +static void do_reallocs(void **mem, const int *oldsize, const int *newsize, const int *seq, int n) +{ + int i; + int j; + + for (i = 0; i < n; i++) + { + j = seq[i]; + printf("(%d)Re-allocating at %p from %d to %d bytes\n", + i, mem[j], oldsize[j], newsize[j]); + mem[j] = realloc(mem[j], newsize[j]); + printf("(%d)Memory re-allocated at %p\n", i, mem[j]); + if (mem[j] == NULL) + { + int allocsize = MM_ALIGN_UP(newsize[j] + SIZEOF_MM_ALLOCNODE); + fprintf(stderr, "(%d)realloc failed for allocsize=%d\n", i, allocsize); + if (allocsize > alloc_info.mxordblk) + { + fprintf(stderr, " Normal, largest free block is only %ld\n", alloc_info.mxordblk); + } + else + { + fprintf(stderr, " ERROR largest free block is %ld\n", alloc_info.mxordblk); + exit(1); + } + } + else + { + memset(mem[j], 0x55, newsize[j]); + } + + mm_showmallinfo(); + } +} + +static void do_memaligns(void **mem, const int *size, const int *align, const int *seq, int n) +{ + int i; + int j; + + for (i = 0; i < n; i++) + { + j = seq[i]; + printf("(%d)Allocating %d bytes aligned to 0x%08x\n", + i, size[j], align[i]); + mem[j] = memalign(align[i], size[j]); + printf("(%d)Memory allocated at %p\n", i, mem[j]); + if (mem[j] == NULL) + { + int allocsize = MM_ALIGN_UP(size[j] + SIZEOF_MM_ALLOCNODE) + 2*align[i]; + fprintf(stderr, "(%d)memalign failed for allocsize=%d\n", i, allocsize); + if (allocsize > alloc_info.mxordblk) + { + fprintf(stderr, " Normal, largest free block is only %ld\n", alloc_info.mxordblk); + } + else + { + fprintf(stderr, " ERROR largest free block is %ld\n", alloc_info.mxordblk); + exit(1); + } + } + else + { + memset(mem[j], 0x33, size[j]); + } + + mm_showmallinfo(); + } +} + +static void do_frees(void **mem, const int *size, const int *seq, int n) +{ + int i; + int j; + + for (i = 0; i < n; i++) + { + j = seq[i]; + printf("(%d)Releasing memory at %p (size=%d bytes)\n", + i, mem[j], size[j]); + free(mem[j]); + mem[j] = NULL; + + mm_showmallinfo(); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start + ****************************************************************************/ + +int user_start(int argc, char *argv[]) +{ + mm_showmallinfo(); + + /* Allocate some memory */ + + do_mallocs(allocs, alloc_sizes, random1, NTEST_ALLOCS); + + /* Re-allocate the memory */ + + do_reallocs(allocs, alloc_sizes, realloc_sizes, random2, NTEST_ALLOCS); + + /* Release the memory */ + + do_frees(allocs, realloc_sizes, random3, NTEST_ALLOCS); + + /* Allocate aligned memory */ + + do_memaligns(allocs, alloc_sizes, alignment, random2, NTEST_ALLOCS/2); + do_memaligns(allocs, alloc_sizes, alignment, &random2[NTEST_ALLOCS/2], NTEST_ALLOCS/2); + + /* Release aligned memory */ + + do_frees(allocs, alloc_sizes, random1, NTEST_ALLOCS); + + printf("TEST COMPLETE\n"); + return 0; +} diff --git a/apps/examples/mount/Kconfig b/apps/examples/mount/Kconfig new file mode 100644 index 000000000..b38c4763c --- /dev/null +++ b/apps/examples/mount/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_MOUNT + bool "File system mount example" + default n + ---help--- + Enable the file system mount example + +if EXAMPLES_MOUNT +endif diff --git a/apps/examples/mount/Makefile b/apps/examples/mount/Makefile new file mode 100644 index 000000000..7e48ea44a --- /dev/null +++ b/apps/examples/mount/Makefile @@ -0,0 +1,93 @@ +############################################################################ +# apps/Makefile +# +# Copyright (C) 2007-2008, 2010-2010 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# mount() test + +ASRCS = +CSRCS = mount_main.c ramdisk.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/mount/mount.h b/apps/examples/mount/mount.h new file mode 100644 index 000000000..68a03674d --- /dev/null +++ b/apps/examples/mount/mount.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * examples/mount/mount.h + * + * Copyright (C) 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __EXAMPLES_MOUNT_MOUNT_H +#define __EXAMPLES_MOUNT_MOUNT_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* Configure the test */ + +#if defined(CONFIG_EXAMPLES_MOUNT_DEVNAME) +# if !defined(CONFIG_FS_WRITABLE) +# error "Writable filesystem required in this configuration" +# endif +# undef CONFIG_EXAMPLES_MOUNT_NSECTORS +# undef CONFIG_EXAMPLES_MOUNT_SECTORSIZE +# undef CONFIG_EXAMPLES_MOUNT_RAMDEVNO +# define MOUNT_DEVNAME CONFIG_EXAMPLES_MOUNT_DEVNAME +#else +# if !defined(CONFIG_FS_FAT) +# error "CONFIG_FS_FAT required in this configuration" +# endif +# if !defined(CONFIG_EXAMPLES_MOUNT_SECTORSIZE) +# define CONFIG_EXAMPLES_MOUNT_SECTORSIZE 512 +# endif +# if !defined(CONFIG_EXAMPLES_MOUNT_NSECTORS) +# define CONFIG_EXAMPLES_MOUNT_NSECTORS 2048 +# endif +# if !defined(CONFIG_EXAMPLES_MOUNT_RAMDEVNO) +# define CONFIG_EXAMPLES_MOUNT_RAMDEVNO 0 +# endif +# define STR_RAMDEVNO(m) #m +# define MKMOUNT_DEVNAME(m) "/dev/ram" STR_RAMDEVNO(m) +# define MOUNT_DEVNAME MKMOUNT_DEVNAME(CONFIG_EXAMPLES_MOUNT_RAMDEVNO) +#endif + + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +extern const char g_source[]; /* Mount 'source' path */ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifndef CONFIG_EXAMPLES_MOUNT_DEVNAME +extern int create_ramdisk(void); +#endif + +#endif /* __EXAMPLES_MOUNT_MOUNT_H */ diff --git a/apps/examples/mount/mount_main.c b/apps/examples/mount/mount_main.c new file mode 100644 index 000000000..00070b94c --- /dev/null +++ b/apps/examples/mount/mount_main.c @@ -0,0 +1,754 @@ +/**************************************************************************** + * examples/mount/mount_main.c + * + * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <sys/mount.h> +#include <sys/stat.h> +#include <sys/statfs.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <fcntl.h> +#include <dirent.h> +#include <errno.h> + +#include "mount.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define TEST_USE_STAT 1 +#define TEST_SHOW_DIRECTORIES 1 +#define TEST_USE_STATFS 1 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const char g_mntdir[] = "/mnt"; +static const char g_target[] = "/mnt/fs"; +static const char g_filesystemtype[] = "vfat"; + +static const char g_testdir1[] = "/mnt/fs/TestDir"; +static const char g_testdir2[] = "/mnt/fs/NewDir1"; +static const char g_testdir3[] = "/mnt/fs/NewDir2"; +static const char g_testdir4[] = "/mnt/fs/NewDir3"; +#ifdef CONFIG_EXAMPLES_MOUNT_DEVNAME +static const char g_testfile1[] = "/mnt/fs/TestDir/TestFile.txt"; +#endif +static const char g_testfile2[] = "/mnt/fs/TestDir/WrTest1.txt"; +static const char g_testfile3[] = "/mnt/fs/NewDir1/WrTest2.txt"; +static const char g_testfile4[] = "/mnt/fs/NewDir3/Renamed.txt"; +static const char g_testmsg[] = "This is a write test"; + +static int g_nerrors = 0; + +static char g_namebuffer[256]; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + + const char g_source[] = MOUNT_DEVNAME; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +#ifdef TEST_USE_STAT +static void show_stat(const char *path, struct stat *ps) +{ + printf("%s stat:\n", path); + printf("\tmode : %08x\n", ps->st_mode); + if (S_ISREG(ps->st_mode)) + { + printf("\ttype : File\n"); + } + else if (S_ISDIR(ps->st_mode)) + { + printf("\ttype : Directory\n"); + } + else if (S_ISCHR(ps->st_mode)) + { + printf("\ttype : Character driver\n"); + } + else if (S_ISBLK(ps->st_mode)) + { + printf("\ttype : Block driver\n"); + } + else + { + printf("\ttype : Unknown\n"); + } + + printf("\tsize : %d (bytes)\n", ps->st_size); + printf("\tblock size : %d (bytes)\n", ps->st_blksize); + printf("\tsize : %d (blocks)\n", ps->st_blocks); + printf("\taccess time : %d\n", ps->st_atime); + printf("\tmodify time : %d\n", ps->st_mtime); + printf("\tchange time : %d\n", ps->st_ctime); +} +#endif + +/**************************************************************************** + * Name: show_statfs + ****************************************************************************/ + +#ifdef TEST_USE_STATFS +static void show_statfs(const char *path) +{ + struct statfs buf; + int ret; + + /* Try stat() against a file or directory. It should fail with expectederror */ + + printf("show_statfs: Try statfs(%s)\n", path); + ret = statfs(path, &buf); + if (ret == 0) + { + printf("show_statfs: statfs(%s) succeeded\n", path); + printf("\tFS Type : %0x\n", buf.f_type); + printf("\tBlock size : %d\n", buf.f_bsize); + printf("\tNumber of blocks : %d\n", buf.f_blocks); + printf("\tFree blocks : %d\n", buf.f_bfree); + printf("\tFree user blocks : %d\n", buf.f_bavail); + printf("\tNumber file nodes : %d\n", buf.f_files); + printf("\tFree file nodes : %d\n", buf.f_ffree); + printf("\tFile name length : %d\n", buf.f_namelen); + } + else + { + printf("show_statfs: ERROR statfs(%s) failed with errno=%d\n", + path, errno); + g_nerrors++; + } +} +#else +# define show_statfs(p) +#endif + +/**************************************************************************** + * Name: show_directories + ****************************************************************************/ + +#ifdef TEST_SHOW_DIRECTORIES +static void show_directories(const char *path, int indent) +{ + DIR *dirp; + struct dirent *direntry; + int i; + + dirp = opendir(path); + if ( !dirp ) + { + printf("show_directories: ERROR opendir(\"%s\") failed with errno=%d\n", + path, errno); + g_nerrors++; + return; + } + + for (direntry = readdir(dirp); direntry; direntry = readdir(dirp)) + { + for (i = 0; i < 2*indent; i++) + { + putchar(' '); + } + if (DIRENT_ISDIRECTORY(direntry->d_type)) + { + char *subdir; + printf("%s/\n", direntry->d_name); + sprintf(g_namebuffer, "%s/%s", path, direntry->d_name); + subdir = strdup(g_namebuffer); + show_directories( subdir, indent + 1); + free(subdir); + } + else + { + printf("%s\n", direntry->d_name); + } + } + + closedir(dirp); +} +#else +# define show_directories(p,i) +#endif + +/**************************************************************************** + * Name: fail_read_open + ****************************************************************************/ +#ifdef CONFIG_EXAMPLES_MOUNT_DEVNAME +static void fail_read_open(const char *path, int expectederror) +{ + int fd; + + printf("fail_read_open: Try open(%s) for reading\n", path); + + fd = open(path, O_RDONLY); + if (fd >= 0) + { + printf("fail_read_open: ERROR open(%s) succeeded\n", path); + g_nerrors++; + close(fd); + } + else if (errno != expectederror) + { + printf("fail_read_open: ERROR open(%s) failed with errno=%d (expected %d)\n", + path, errno, expectederror); + g_nerrors++; + } +} +#endif + +/**************************************************************************** + * Name: read_test_file + ****************************************************************************/ + +static void read_test_file(const char *path) +{ + char buffer[128]; + int nbytes; + int fd; + + /* Read a test file that is already on the test file system image */ + + printf("read_test_file: opening %s for reading\n", path); + + fd = open(path, O_RDONLY); + if (fd < 0) + { + printf("read_test_file: ERROR failed to open %s, errno=%d\n", + path, errno); + g_nerrors++; + } + else + { + memset(buffer, 0, 128); + nbytes = read(fd, buffer, 128); + if (nbytes < 0) + { + printf("read_test_file: ERROR failed to read from %s, errno=%d\n", + path, errno); + g_nerrors++; + } + else + { + buffer[127]='\0'; + printf("read_test_file: Read \"%s\" from %s\n", buffer, path); + } + close(fd); + } +} + +/**************************************************************************** + * Name: write_test_file + ****************************************************************************/ + +static void write_test_file(const char *path) +{ + int fd; + + /* Write a test file into a pre-existing file on the test file system */ + + printf("write_test_file: opening %s for writing\n", path); + + fd = open(path, O_WRONLY|O_CREAT|O_TRUNC, 0644); + if (fd < 0) + { + printf("write_test_file: ERROR failed to open %s for writing, errno=%d\n", + path, errno); + g_nerrors++; + } + else + { + int nbytes = write(fd, g_testmsg, strlen(g_testmsg)); + if (nbytes < 0) + { + printf("write_test_file: ERROR failed to write to %s, errno=%d\n", + path, errno); + g_nerrors++; + } + else + { + printf("write_test_file: wrote %d bytes to %s\n", nbytes, path); + } + close(fd); + } +} + +/**************************************************************************** + * Name: fail_mkdir + ****************************************************************************/ + +static void fail_mkdir(const char *path, int expectederror) +{ + int ret; + + /* Try mkdir() against a file or directory. It should fail with expectederror */ + + printf("fail_mkdir: Try mkdir(%s)\n", path); + + ret = mkdir(path, 0666); + if (ret == 0) + { + printf("fail_mkdir: ERROR mkdir(%s) succeeded\n", path); + g_nerrors++; + } + else if (errno != expectederror) + { + printf("fail_mkdir: ERROR mkdir(%s) failed with errno=%d (expected %d)\n", + path, errno, expectederror); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: succeed_mkdir + ****************************************************************************/ + +static void succeed_mkdir(const char *path) +{ + int ret; + + printf("succeed_mkdir: Try mkdir(%s)\n", path); + + ret = mkdir(path, 0666); + if (ret != 0) + { + printf("succeed_mkdir: ERROR mkdir(%s) failed with errno=%d\n", + path, errno); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: fail_rmdir + ****************************************************************************/ + +static void fail_rmdir(const char *path, int expectederror) +{ + int ret; + + /* Try rmdir() against a file or directory. It should fail with expectederror */ + + printf("fail_rmdir: Try rmdir(%s)\n", path); + + ret = rmdir(path); + if (ret == 0) + { + printf("fail_rmdir: ERROR rmdir(%s) succeeded\n", path); + g_nerrors++; + } + else if (errno != expectederror) + { + printf("fail_rmdir: ERROR rmdir(%s) failed with errno=%d (expected %d)\n", + path, errno, expectederror); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: succeed_rmdir + ****************************************************************************/ + +static void succeed_rmdir(const char *path) +{ + int ret; + + printf("succeed_rmdir: Try rmdir(%s)\n", path); + + ret = rmdir(path); + if (ret != 0) + { + printf("succeed_rmdir: ERROR rmdir(%s) failed with errno=%d\n", + path, errno); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: fail_unlink + ****************************************************************************/ + +static void fail_unlink(const char *path, int expectederror) +{ + int ret; + + /* Try unlink() against a file or directory. It should fail with expectederror */ + + printf("fail_unlink: Try unlink(%s)\n", path); + + ret = unlink(path); + if (ret == 0) + { + printf("fail_unlink: ERROR unlink(%s) succeeded\n", path); + g_nerrors++; + } + else if (errno != expectederror) + { + printf("fail_unlink: ERROR unlink(%s) failed with errno=%d (expected %d)\n", + path, errno, expectederror); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: succeed_unlink + ****************************************************************************/ + +static void succeed_unlink(const char *path) +{ + int ret; + + /* Try unlink() against the test file. It should succeed. */ + + printf("succeed_unlink: Try unlink(%s)\n", path); + + ret = unlink(path); + if (ret != 0) + { + printf("succeed_unlink: ERROR unlink(%s) failed with errno=%d\n", + path, errno); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: fail_rename + ****************************************************************************/ + +static void fail_rename(const char *oldpath, const char *newpath, int expectederror) +{ + int ret; + + /* Try rename() against a file or directory. It should fail with expectederror */ + + printf("fail_rename: Try rename(%s->%s)\n", oldpath, newpath); + + ret = rename(oldpath, newpath); + if (ret == 0) + { + printf("fail_rename: ERROR rename(%s->%s) succeeded\n", + oldpath, newpath); + g_nerrors++; + } + else if (errno != expectederror) + { + printf("fail_rename: ERROR rename(%s->%s) failed with errno=%d (expected %d)\n", + oldpath, newpath, errno, expectederror); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: succeed_rename + ****************************************************************************/ + +static void succeed_rename(const char *oldpath, const char *newpath) +{ + int ret; + + printf("succeed_rename: Try rename(%s->%s)\n", oldpath, newpath); + + ret = rename(oldpath, newpath); + if (ret != 0) + { + printf("succeed_rename: ERROR rename(%s->%s) failed with errno=%d\n", + oldpath, newpath, errno); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: fail_stat + ****************************************************************************/ + +#ifdef TEST_USE_STAT +static void fail_stat(const char *path, int expectederror) +{ + struct stat buf; + int ret; + + /* Try stat() against a file or directory. It should fail with expectederror */ + + printf("fail_stat: Try stat(%s)\n", path); + + ret = stat(path, &buf); + if (ret == 0) + { + printf("fail_stat: ERROR stat(%s) succeeded\n", path); + show_stat(path, &buf); + g_nerrors++; + } + else if (errno != expectederror) + { + printf("fail_stat: ERROR stat(%s) failed with errno=%d (expected %d)\n", + path, errno, expectederror); + g_nerrors++; + } +} +#else +# define fail_stat(p,e); +#endif + +/**************************************************************************** + * Name: succeed_stat + ****************************************************************************/ + +#ifdef TEST_USE_STAT +static void succeed_stat(const char *path) +{ + struct stat buf; + int ret; + + printf("succeed_stat: Try stat(%s)\n", path); + + ret = stat(path, &buf); + if (ret != 0) + { + printf("succeed_stat: ERROR stat(%s) failed with errno=%d\n", + path, errno); + g_nerrors++; + } + else + { + printf("succeed_stat: stat(%s) succeeded\n", path); + show_stat(path, &buf); + } +} +#else +#define succeed_stat(p) +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start + ****************************************************************************/ + +int user_start(int argc, char *argv[]) +{ + int ret; + +#ifndef CONFIG_EXAMPLES_MOUNT_DEVNAME + /* Create a RAM disk for the test */ + + ret = create_ramdisk(); + if (ret < 0) + { + printf("user_start: ERROR failed to create RAM disk\n"); + return 1; + } +#endif + + /* Mount the test file system (see arch/sim/src/up_deviceimage.c */ + + printf("user_start: mounting %s filesystem at target=%s with source=%s\n", + g_filesystemtype, g_target, g_source); + + ret = mount(g_source, g_target, g_filesystemtype, 0, NULL); + printf("user_start: mount() returned %d\n", ret); + + if (ret == 0) + { + show_statfs(g_mntdir); + show_statfs(g_target); + +#ifdef CONFIG_EXAMPLES_MOUNT_DEVNAME + /* Read a test file that is already on the test file system image */ + + show_directories("", 0); + succeed_stat(g_testfile1); + show_statfs(g_testfile1); + read_test_file(g_testfile1); +#else + /* Create the test directory that would have been on the canned filesystem */ + + succeed_mkdir(g_testdir1); + show_directories("", 0); + succeed_stat(g_testdir1); + show_statfs(g_testdir1); +#endif + + /* Write a test file into a pre-existing directory on the test file system */ + + fail_stat(g_testfile2, ENOENT); + write_test_file(g_testfile2); + show_directories("", 0); + succeed_stat(g_testfile2); + show_statfs(g_testfile2); + + /* Read the file that we just wrote */ + + read_test_file(g_testfile2); + + /* Try rmdir() against a file on the directory. It should fail with ENOTDIR */ +#ifdef CONFIG_EXAMPLES_MOUNT_DEVNAME + fail_rmdir(g_testfile1, ENOTDIR); +#endif + + /* Try rmdir() against the test directory. It should fail with ENOTEMPTY */ + + fail_rmdir(g_testdir1, ENOTEMPTY); + + /* Try unlink() against the test directory. It should fail with EISDIR */ + + fail_unlink(g_testdir1, EISDIR); + + /* Try unlink() against the test file1. It should succeed. */ +#ifdef CONFIG_EXAMPLES_MOUNT_DEVNAME + succeed_unlink(g_testfile1); + fail_stat(g_testfile1, ENOENT); + show_directories("", 0); +#endif + + /* Attempt to open testfile1 should fail with ENOENT */ +#ifdef CONFIG_EXAMPLES_MOUNT_DEVNAME + fail_read_open(g_testfile1, ENOENT); +#endif + /* Try rmdir() against the test directory. It should still fail with ENOTEMPTY */ + + fail_rmdir(g_testdir1, ENOTEMPTY); + + /* Try mkdir() against the test file2. It should fail with EEXIST. */ + + fail_mkdir(g_testfile2, EEXIST); + + /* Try unlink() against the test file2. It should succeed. */ + + succeed_unlink(g_testfile2); + show_directories("", 0); + fail_stat(g_testfile2, ENOENT); + + /* Try mkdir() against the test dir1. It should fail with EEXIST. */ + + fail_mkdir(g_testdir1, EEXIST); + + /* Try rmdir() against the test directory. mkdir should now succeed. */ + + succeed_rmdir(g_testdir1); + show_directories("", 0); + fail_stat(g_testdir1, ENOENT); + + /* Try mkdir() against the test dir2. It should succeed */ + + succeed_mkdir(g_testdir2); + show_directories("", 0); + succeed_stat(g_testdir2); + show_statfs(g_testdir2); + + /* Try mkdir() against the test dir2. It should fail with EXIST */ + + fail_mkdir(g_testdir2, EEXIST); + + /* Write a test file into a new directory on the test file system */ + + fail_stat(g_testfile3, ENOENT); + write_test_file(g_testfile3); + show_directories("", 0); + succeed_stat(g_testfile3); + show_statfs(g_testfile3); + + /* Read the file that we just wrote */ + + read_test_file(g_testfile3); + + /* Use mkdir() to create test dir3. It should succeed */ + + fail_stat(g_testdir3, ENOENT); + succeed_mkdir(g_testdir3); + show_directories("", 0); + succeed_stat(g_testdir3); + show_statfs(g_testdir3); + + /* Try rename() on the root directory. Should fail with EXDEV*/ + + fail_rename(g_target, g_testdir4, EXDEV); + + /* Try rename() to an existing directory. Should fail with EEXIST */ + + fail_rename(g_testdir2, g_testdir3, EEXIST); + + /* Try rename() to a non-existing directory. Should succeed */ + + fail_stat(g_testdir4, ENOENT); + succeed_rename(g_testdir3, g_testdir4); + show_directories("", 0); + fail_stat(g_testdir3, ENOENT); + succeed_stat(g_testdir4); + show_statfs(g_testdir4); + + /* Try rename() of file. Should work. */ + + fail_stat(g_testfile4, ENOENT); + succeed_rename(g_testfile3, g_testfile4); + show_directories("", 0); + fail_stat(g_testfile3, ENOENT); + succeed_stat(g_testfile4); + show_statfs(g_testfile4); + + /* Make sure that we can still read the renamed file */ + + read_test_file(g_testfile4); + + /* Unmount the file system */ + + printf("user_start: Try unmount(%s)\n", g_target); + + ret = umount(g_target); + if (ret != 0) + { + printf("user_start: ERROR umount() failed, errno %d\n", errno); + g_nerrors++; + } + + printf("user_start: %d errors reported\n", g_nerrors); + } + + fflush(stdout); + return 0; +} diff --git a/apps/examples/mount/ramdisk.c b/apps/examples/mount/ramdisk.c new file mode 100644 index 000000000..9688580c0 --- /dev/null +++ b/apps/examples/mount/ramdisk.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * examples/mount/ramdisk.c + * + * Copyright (C) 2008-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <errno.h> + +#include <nuttx/ramdisk.h> +#include <nuttx/fs/mkfatfs.h> + +#include "mount.h" + +#ifndef CONFIG_EXAMPLES_MOUNT_DEVNAME + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +#define BUFFER_SIZE (CONFIG_EXAMPLES_MOUNT_NSECTORS*CONFIG_EXAMPLES_MOUNT_SECTORSIZE) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct fat_format_s g_fmt = FAT_FORMAT_INITIALIZER; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: create_ramdisk + * + * Description: + * Create a RAM disk of the specified size formatting with a FAT file + * system + * + * Input Parameters: + * None + * + * Return: + * Zero on success, a negated errno on failure. + * + ****************************************************************************/ + +int create_ramdisk(void) +{ + char *pbuffer; + int ret; + + /* Allocate a buffer to hold the file system image. */ + + pbuffer = (char*)malloc(BUFFER_SIZE); + if (!pbuffer) + { + printf("create_ramdisk: Failed to allocate ramdisk of size %d\n", + BUFFER_SIZE); + return -ENOMEM; + } + + /* Register a RAMDISK device to manage this RAM image */ + + ret = ramdisk_register(CONFIG_EXAMPLES_MOUNT_RAMDEVNO, + pbuffer, + CONFIG_EXAMPLES_MOUNT_NSECTORS, + CONFIG_EXAMPLES_MOUNT_SECTORSIZE, + true); + if (ret < 0) + { + printf("create_ramdisk: Failed to register ramdisk at %s: %d\n", + g_source, -ret); + free(pbuffer); + return ret; + } + + /* Create a FAT filesystem on the ramdisk */ + + ret = mkfatfs(g_source, &g_fmt); + if (ret < 0) + { + printf("create_ramdisk: Failed to create FAT filesystem on ramdisk at %s\n", + g_source); + /* free(pbuffer); -- RAM disk is registered */ + return ret; + } + + return 0; +} +#endif /* !CONFIG_EXAMPLES_MOUNT_DEVNAME */ diff --git a/apps/examples/nsh/Kconfig b/apps/examples/nsh/Kconfig new file mode 100644 index 000000000..289c7e515 --- /dev/null +++ b/apps/examples/nsh/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_NSH + bool "NuttShell (NSH) example" + default n + ---help--- + Enable the NuttShell (NSH) example + +if EXAMPLES_NSH +endif diff --git a/apps/examples/nsh/Makefile b/apps/examples/nsh/Makefile new file mode 100644 index 000000000..b5844f9ed --- /dev/null +++ b/apps/examples/nsh/Makefile @@ -0,0 +1,93 @@ +############################################################################ +# apps/examples/nsh/Makefile +# +# Copyright (C) 2007-2008, 2010-2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# NuttShell (NSH) Example + +ASRCS = +CSRCS = nsh_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/nsh/nsh_main.c b/apps/examples/nsh/nsh_main.c new file mode 100644 index 000000000..c5b671ab1 --- /dev/null +++ b/apps/examples/nsh/nsh_main.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * examples/nsh/nsh_main.c + * + * Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/stat.h> +#include <stdint.h> +#include <stdio.h> +#include <sched.h> +#include <errno.h> + +#include <nuttx/arch.h> + +#include <apps/nsh.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* The NSH telnet console requires networking support (and TCP/IP) */ + +#ifndef CONFIG_NET +# undef CONFIG_NSH_TELNET +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start + ****************************************************************************/ + +int user_start(int argc, char *argv[]) +{ + int exitval = 0; + int ret; + + /* Call all C++ static constructors */ + +#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE) + up_cxxinitialize(); +#endif + + /* Initialize the NSH library */ + + nsh_initialize(); + + /* If the Telnet console is selected as a front-end, then start the + * Telnet daemon. + */ + +#ifdef CONFIG_NSH_TELNET + ret = nsh_telnetstart(); + if (ret < 0) + { + /* The daemon is NOT running. Report the the error then fail... + * either with the serial console up or just exiting. + */ + + fprintf(stderr, "ERROR: Failed to start TELNET daemon: %d\n", ret); + exitval = 1; + } +#endif + + /* If the serial console front end is selected, then run it on this thread */ + +#ifdef CONFIG_NSH_CONSOLE + ret = nsh_consolemain(0, NULL); + + /* nsh_consolemain() should not return. So if we get here, something + * is wrong. + */ + + fprintf(stderr, "ERROR: nsh_consolemain() returned: %d\n", ret); + exitval = 1; +#endif + + return exitval; +} diff --git a/apps/examples/null/Kconfig b/apps/examples/null/Kconfig new file mode 100644 index 000000000..1f19dfd2c --- /dev/null +++ b/apps/examples/null/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_NULL + bool "NULL example" + default n + ---help--- + Enable the NULL example + +if EXAMPLES_NULL +endif diff --git a/apps/examples/null/Makefile b/apps/examples/null/Makefile new file mode 100644 index 000000000..3938eb171 --- /dev/null +++ b/apps/examples/null/Makefile @@ -0,0 +1,93 @@ +############################################################################ +# examples/null/Makefile +# +# Copyright (C) 2007-2008, 2010-2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# The smallest thing you can build -- the NULL example. + +ASRCS = +CSRCS = null_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/null/null_main.c b/apps/examples/null/null_main.c new file mode 100644 index 000000000..10fc1bf1e --- /dev/null +++ b/apps/examples/null/null_main.c @@ -0,0 +1,67 @@ +/**************************************************************************** + * examples/null/null_main.c + * + * Copyright (C) 2007, 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start + ****************************************************************************/ + +int user_start(int argc, char *argv[]) +{ + return 0; +} diff --git a/apps/examples/ostest/Kconfig b/apps/examples/ostest/Kconfig new file mode 100644 index 000000000..ca8957993 --- /dev/null +++ b/apps/examples/ostest/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_OSTEST + bool "OS test example" + default n + ---help--- + Enable the OS test example + +if EXAMPLES_OSTEST +endif diff --git a/apps/examples/ostest/Makefile b/apps/examples/ostest/Makefile new file mode 100644 index 000000000..eab1db8b3 --- /dev/null +++ b/apps/examples/ostest/Makefile @@ -0,0 +1,149 @@ +############################################################################ +# apps/examples/ostest/Makefile +# +# Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# ostest built-in application info + +APPNAME = ostest +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# NuttX OS Test + +ASRCS = +CSRCS = main.c dev_null.c + +ifeq ($(CONFIG_ARCH_FPU),y) +CSRCS += fpu.c +endif + +ifneq ($(CONFIG_DISABLE_PTHREAD),y) +CSRCS += cancel.c cond.c mutex.c sem.c barrier.c +ifneq ($(CONFIG_RR_INTERVAL),0) +CSRCS += roundrobin.c +endif # CONFIG_RR_INTERVAL +ifeq ($(CONFIG_MUTEX_TYPES),y) +CSRCS += rmutex.c +endif # CONFIG_MUTEX_TYPES +endif # CONFIG_DISABLE_PTHREAD + +ifneq ($(CONFIG_DISABLE_SIGNALS),y) +CSRCS += sighand.c +ifneq ($(CONFIG_DISABLE_PTHREAD),y) +ifneq ($(CONFIG_DISABLE_CLOCK),y) +CSRCS += timedwait.c +endif # CONFIG_DISABLE_CLOCK +endif # CONFIG_DISABLE_PTHREAD +endif # CONFIG_DISABLE_SIGNALS + +ifneq ($(CONFIG_DISABLE_MQUEUE),y) +ifneq ($(CONFIG_DISABLE_PTHREAD),y) +CSRCS += mqueue.c +ifneq ($(CONFIG_DISABLE_CLOCK),y) +CSRCS += timedmqueue.c +endif # CONFIG_DISABLE_CLOCK +endif # CONFIG_DISABLE_PTHREAD +endif # CONFIG_DISABLE_MQUEUE + +ifneq ($(CONFIG_DISABLE_POSIX_TIMERS),y) +CSRCS += posixtimer.c +endif + +ifneq ($(CONFIG_DISABLE_SIGNALS),y) +ifneq ($(CONFIG_DISABLE_PTHREAD),y) +ifeq ($(CONFIG_PRIORITY_INHERITANCE),y) +CSRCS += prioinherit.c +endif # CONFIG_PRIORITY_INHERITANCE +endif # CONFIG_DISABLE_PTHREAD +endif # CONFIG_DISABLE_SIGNALS + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: +ifeq ($(CONFIG_EXAMPLES_OSTEST_BUILTIN),y) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +endif + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/ostest/barrier.c b/apps/examples/ostest/barrier.c new file mode 100644 index 000000000..da1301dc3 --- /dev/null +++ b/apps/examples/ostest/barrier.c @@ -0,0 +1,208 @@ +/**************************************************************************** + * examples/ostest/barrier.c + * + * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <stdio.h> +#include <unistd.h> +#include <pthread.h> + +#include "ostest.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define HALF_SECOND 500000L + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static pthread_barrier_t barrier; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: barrier_func + ****************************************************************************/ + +static void *barrier_func(void *parameter) +{ + int id = (int)parameter; + int status; + + printf("barrier_func: Thread %d started\n", id); +#ifndef CONFIG_DISABLE_SIGNALS + usleep(HALF_SECOND); +#endif + + /* Wait at the barrier until all threads are synchronized. */ + + printf("barrier_func: Thread %d calling pthread_barrier_wait()\n", + id); + FFLUSH(); + status = pthread_barrier_wait(&barrier); + if (status == 0) + { + printf("barrier_func: Thread %d, back with " + "status=0 (I am not special)\n", + id, status); + } + else if (status == PTHREAD_BARRIER_SERIAL_THREAD) + { + printf("barrier_func: Thread %d, back with " + "status=PTHREAD_BARRIER_SERIAL_THREAD (I AM SPECIAL)\n", + id, status); + } + else + { + printf("barrier_func: ERROR thread %d could not get semaphore value\n", + id); + } + FFLUSH(); + +#ifndef CONFIG_DISABLE_SIGNALS + usleep(HALF_SECOND); +#endif + printf("barrier_func: Thread %d done\n", id); + FFLUSH(); + return NULL; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: barrier_test + ****************************************************************************/ + +void barrier_test(void) +{ + pthread_t barrier_thread[CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS]; + pthread_addr_t result; + pthread_attr_t attr; + pthread_barrierattr_t barrierattr; + int status; + int i; + + printf("barrier_test: Initializing barrier\n"); + + status = pthread_barrierattr_init(&barrierattr); + if (status != OK) + { + printf("barrier_test: pthread_barrierattr_init failed, status=%d\n", + status); + } + + status = pthread_barrier_init(&barrier, &barrierattr, + CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS); + if (status != OK) + { + printf("barrier_test: pthread_barrierattr_init failed, status=%d\n", + status); + } + + /* Create the barrier */ + + status = pthread_barrierattr_init(&barrierattr); + + /* Start CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS thread instances */ + + status = pthread_attr_init(&attr); + if (status != OK) + { + printf("barrier_test: pthread_attr_init failed, status=%d\n", + status); + } + + for (i = 0; i < CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS; i++) + { + status = pthread_create(&barrier_thread[i], &attr, barrier_func, + (pthread_addr_t)i); + if (status != 0) + { + printf("barrier_test: Error in thread %d create, status=%d\n", + i, status); + printf("barrier_test: Test aborted with waiting threads\n"); + goto abort_test; + } + else + { + printf("barrier_test: Thread %d created\n", i); + } + } + FFLUSH(); + + /* Wait for all thread instances to complete */ + + for (i = 0; i < CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS; i++) + { + status = pthread_join(barrier_thread[i], &result); + if (status != 0) + { + printf("barrier_test: Error in thread %d join, status=%d\n", + i, status); + } + else + { + printf("barrier_test: Thread %d completed with result=%p\n", + i, result); + } + } + + /* Destroy the barrier */ + +abort_test: + status = pthread_barrier_destroy(&barrier); + if (status != OK) + { + printf("barrier_test: pthread_barrier_destroy failed, status=%d\n", + status); + } + + status = pthread_barrierattr_destroy(&barrierattr); + if (status != OK) + { + printf("barrier_test: pthread_barrierattr_destroy failed, status=%d\n", + status); + } + FFLUSH(); +} diff --git a/apps/examples/ostest/cancel.c b/apps/examples/ostest/cancel.c new file mode 100644 index 000000000..bf2d03615 --- /dev/null +++ b/apps/examples/ostest/cancel.c @@ -0,0 +1,333 @@ +/*********************************************************************** + * examples/ostest/cancel.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +#include <stdio.h> +#include <time.h> +#include <pthread.h> +#include <errno.h> +#include "ostest.h" + +static pthread_mutex_t mutex; +static pthread_cond_t cond; + +static void *thread_waiter(void *parameter) +{ + int status; + + /* Take the mutex */ + + printf("thread_waiter: Taking mutex\n"); + status = pthread_mutex_lock(&mutex); + if (status != 0) + { + printf("thread_waiter: ERROR pthread_mutex_lock failed, status=%d\n", status); + } + + printf("thread_waiter: Starting wait for condition\n"); + + /* Are we a non-cancelable thread? Yes, set the non-cancelable state */ + + if (!parameter) + { + printf("thread_waiter: Setting non-cancelable\n"); + status = pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, NULL); + if (status != 0) + { + printf("thread_waiter: ERROR pthread_setcancelstate failed, status=%d\n", status); + } + } + + /* The wait -- we will never awaken from this. */ + + status = pthread_cond_wait(&cond, &mutex); + if (status != 0) + { + printf("thread_waiter: ERROR pthread_cond_wait failed, status=%d\n", status); + } + + /* Release the mutex */ + + printf("thread_waiter: Releasing mutex\n"); + status = pthread_mutex_unlock(&mutex); + if (status != 0) + { + printf("thread_waiter: ERROR pthread_mutex_unlock failed, status=%d\n", status); + } + + /* Set the cancelable state */ + + printf("thread_waiter: Setting cancelable\n"); + status = pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); + if (status != 0) + { + printf("thread_waiter: ERROR pthread_setcancelstate failed, status=%d\n", status); + } + + printf("thread_waiter: Exit with status 0x12345678\n"); + pthread_exit((pthread_addr_t)0x12345678); + return NULL; +} + +static void start_thread(pthread_t *waiter, int cancelable) +{ + pthread_attr_t attr; + int status; + + /* Initialize the mutex */ + + printf("start_thread: Initializing mutex\n"); + status = pthread_mutex_init(&mutex, NULL); + if (status != 0) + { + printf("start_thread: ERROR pthread_mutex_init failed, status=%d\n", status); + } + + /* Initialize the condition variable */ + + printf("start_thread: Initializing cond\n"); + status = pthread_cond_init(&cond, NULL); + if (status != 0) + { + printf("start_thread: ERROR pthread_cond_init failed, status=%d\n", status); + } + + /* Set up attributes */ + + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("start_thread: pthread_attr_init failed, status=%d\n", status); + } + + status = pthread_attr_setstacksize(&attr, STACKSIZE); + if (status != 0) + { + printf("start_thread: pthread_attr_setstacksize failed, status=%d\n", status); + } + + /* Start the waiter thread */ + + printf("start_thread: Starting thread\n"); + status = pthread_create(waiter, &attr, thread_waiter, (pthread_addr_t)cancelable); + if (status != 0) + { + printf("start_thread: ERROR pthread_create failed, status=%d\n", status); + } + + /* Make sure that the waiter thread gets a chance to run */ + + printf("start_thread: Yielding\n"); + pthread_yield(); +} + +static void restart_thread(pthread_t *waiter, int cancelable) +{ + int status; + + /* Destroy the condition variable */ + + printf("restart_thread: Destroying cond\n"); + status = pthread_cond_destroy(&cond); + if (status != 0) + { + printf("restart_thread: ERROR pthread_cond_destroy failed, status=%d\n", status); + } + + /* Destroy the mutex */ + + printf("restart_thread: Destroying mutex\n"); + status = pthread_cond_destroy(&cond); + if (status != 0) + { + printf("restart_thread: ERROR pthread_mutex_destroy failed, status=%d\n", status); + } + + /* Then restart the thread */ + + printf("restart_thread: Re-starting thread\n"); + start_thread(waiter, cancelable); +} + +void cancel_test(void) +{ + pthread_t waiter; + void *result; + int status; + + /* Test 1: Normal Cancel *********************************************/ + /* Start the waiter thread */ + + printf("cancel_test: Test 1: Normal Cancelation\n"); + printf("cancel_test: Starting thread\n"); + start_thread(&waiter, 1); + + /* Then cancel it. It should be in the pthread_cond_wait now */ + + printf("cancel_test: Canceling thread\n"); + status = pthread_cancel(waiter); + if (status != 0) + { + printf("cancel_test: ERROR pthread_cancel failed, status=%d\n", status); + } + + /* Then join to the thread to pick up the result (if we don't do + * we will have a memory leak!) + */ + + printf("cancel_test: Joining\n"); + status = pthread_join(waiter, &result); + if (status != 0) + { + printf("cancel_test: ERROR pthread_join failed, status=%d\n", status); + } + else + { + printf("cancel_test: waiter exited with result=%p\n", result); + if (result != PTHREAD_CANCELED) + { + printf("cancel_test: ERROR expected result=%p\n", PTHREAD_CANCELED); + } + else + { + printf("cancel_test: PASS thread terminated with PTHREAD_CANCELED\n"); + } + } + + /* Test 2: Cancel Detached Thread ************************************/ + + printf("cancel_test: Test 2: Cancelation of detached thread\n"); + printf("cancel_test: Re-starting thread\n"); + restart_thread(&waiter, 1); + + /* Detach the thread */ + + status = pthread_detach(waiter); + if (status != 0) + { + printf("cancel_test: ERROR pthread_detach, status=%d\n", status); + } + + /* Then cancel it. It should be in the pthread_cond_wait now */ + + printf("cancel_test: Canceling thread\n"); + status = pthread_cancel(waiter); + if (status != 0) + { + printf("cancel_test: ERROR pthread_cancel failed, status=%d\n", status); + } + + /* Join should now fail */ + + printf("cancel_test: Joining\n"); + status = pthread_join(waiter, &result); + if (status == 0) + { + printf("cancel_test: ERROR pthread_join succeeded\n"); + } + else if (status != ESRCH) + { + printf("cancel_test: ERROR pthread_join failed but with wrong status=%d\n", status); + } + else + { + printf("cancel_test: PASS pthread_join failed with status=ESRCH\n"); + } + + /* Test 3: Non-cancelable threads ************************************/ + + printf("cancel_test: Test 3: Non-cancelable threads\n"); + printf("cancel_test: Re-starting thread (non-cancelable)\n"); + restart_thread(&waiter, 0); + + /* Then cancel it. It should be in the pthread_cond_wait now. The + * behavior here is non-standard: when the thread is at a cancelation + * point, it should be cancelable, even when cancelation is disable. + * + * The cancelation should succeed, because the cancelation is pending. + */ + + printf("cancel_test: Canceling thread\n"); + status = pthread_cancel(waiter); + if (status != 0) + { + printf("cancel_test: ERROR pthread_cancel failed, status=%d\n", status); + } + + /* Signal the thread. It should wake up and restore the cancelable state. + * When the cancelable state is re-enabled, the thread should be canceled. + */ + + status = pthread_mutex_lock(&mutex); + if (status != 0) + { + printf("cancel_test: ERROR pthread_mutex_lock failed, status=%d\n", status); + } + + status = pthread_cond_signal(&cond); + if (status != 0) + { + printf("cancel_test: ERROR pthread_cond_signal failed, status=%d\n", status); + } + + status = pthread_mutex_unlock(&mutex); + if (status != 0) + { + printf("cancel_test: ERROR pthread_mutex_unlock failed, status=%d\n", status); + } + + /* Then join to the thread to pick up the result (if we don't do + * we will have a memory leak!) + */ + + printf("cancel_test: Joining\n"); + status = pthread_join(waiter, &result); + if (status != 0) + { + printf("cancel_test: ERROR pthread_join failed, status=%d\n", status); + } + else + { + printf("cancel_test: waiter exited with result=%p\n", result); + if (result != PTHREAD_CANCELED) + { + printf("cancel_test: ERROR expected result=%p\n", PTHREAD_CANCELED); + } + else + { + printf("cancel_test: PASS thread terminated with PTHREAD_CANCELED\n"); + } + } + +} diff --git a/apps/examples/ostest/cond.c b/apps/examples/ostest/cond.c new file mode 100644 index 000000000..11191b7d5 --- /dev/null +++ b/apps/examples/ostest/cond.c @@ -0,0 +1,294 @@ +/*********************************************************************** + * cond.c + * + * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +#include <stdio.h> +#include <pthread.h> +#include <unistd.h> +#include "ostest.h" + +#ifndef NULL +# define NULL (void*)0 +#endif + +static volatile enum { RUNNING, MUTEX_WAIT, COND_WAIT} waiter_state; + +static pthread_mutex_t mutex; +static pthread_cond_t cond; +static volatile int data_available = 0; +static int waiter_nloops = 0; +static int waiter_waits = 0; +static int waiter_nerrors = 0; +static int signaler_nloops = 0; +static int signaler_already = 0; +static int signaler_state = 0; +static int signaler_nerrors = 0; + +static void *thread_waiter(void *parameter) +{ + int status; + + printf("waiter_thread: Started\n"); + + for(;;) + { + /* Take the mutex */ + + waiter_state = MUTEX_WAIT; + status = pthread_mutex_lock(&mutex); + waiter_state = RUNNING; + + if (status != 0) + { + printf("waiter_thread: ERROR pthread_mutex_lock failed, status=%d\n", status); + waiter_nerrors++; + } + + /* Check if data is available -- if data is not available then + * wait for it + */ + + if (!data_available) + { + /* We are higher priority than the signaler thread so the + * only time that the signaler thread will have a chance to run is when + * we are waiting for the condition variable. In this case, pthread_cond_wait + * will automatically release the mutex for the signaler (then re-acquire + * the mutex before returning. + */ + + waiter_state = COND_WAIT; + status = pthread_cond_wait(&cond, &mutex); + waiter_state = RUNNING; + + if (status != 0) + { + printf("waiter_thread: ERROR pthread_cond_wait failed, status=%d\n", status); + waiter_nerrors++; + } + waiter_waits++; + } + + /* Now data should be available */ + + if (!data_available) + { + printf("waiter_thread: ERROR data not available after wait\n"); + waiter_nerrors++; + } + + /* Clear data available */ + + data_available = 0; + + /* Release the mutex */ + + status = pthread_mutex_unlock(&mutex); + if (status != 0) + { + printf("waiter_thread: ERROR waiter: pthread_mutex_unlock failed, status=%d\n", status); + waiter_nerrors++; + } + + waiter_nloops++; + } + return NULL; +} + +static void *thread_signaler(void *parameter) +{ + int status; + int i; + + printf("thread_signaler: Started\n"); + for (i = 0; i < 32; i++) + { + /* Take the mutex. The waiter is higher priority and should + * run until it waits for the condition. So, at this point + * signaler should be waiting for the condition. + */ + + status = pthread_mutex_lock(&mutex); + if (status != 0) + { + printf("thread_signaler: ERROR pthread_mutex_lock failed, status=%d\n", status); + signaler_nerrors++; + } + + /* Verify the state */ + + if (waiter_state != COND_WAIT) + { + printf("thread_signaler: ERROR waiter state = %d != COND_WAITING\n", waiter_state); + signaler_state++; + } + + if (data_available) + { + printf("thread_signaler: ERROR data already available, waiter_state=%d\n", waiter_state); + signaler_already++; + } + + /* Set data available and signal the waiter */ + + data_available = 1; + status = pthread_cond_signal(&cond); + if (status != 0) + { + printf("thread_signaler: ERROR pthread_cond_signal failed, status=%d\n", status); + signaler_nerrors++; + } + + /* Release the mutex */ + + status = pthread_mutex_unlock(&mutex); + if (status != 0) + { + printf("thread_signaler: ERROR pthread_mutex_unlock failed, status=%d\n", status); + signaler_nerrors++; + } + + signaler_nloops++; + } + + printf("thread_signaler: Terminating\n"); + pthread_exit(NULL); + return NULL; /* Non-reachable -- needed for some compilers */ +} + +void cond_test(void) +{ + pthread_t waiter; + pthread_t signaler; + pthread_attr_t attr; +#ifdef SDCC + pthread_addr_t result; +#endif + struct sched_param sparam; + int prio_min; + int prio_max; + int prio_mid; + int status; + + /* Initialize the mutex */ + + printf("cond_test: Initializing mutex\n"); + status = pthread_mutex_init(&mutex, NULL); + if (status != 0) + { + printf("cond_test: ERROR pthread_mutex_init failed, status=%d\n", status); + } + + /* Initialize the condition variable */ + + printf("cond_test: Initializing cond\n"); + status = pthread_cond_init(&cond, NULL); + if (status != 0) + { + printf("cond_test: ERROR pthread_condinit failed, status=%d\n", status); + } + + /* Start the waiter thread at higher priority */ + + printf("cond_test: Starting waiter\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("cond_test: pthread_attr_init failed, status=%d\n", status); + } + + prio_min = sched_get_priority_min(SCHED_FIFO); + prio_max = sched_get_priority_max(SCHED_FIFO); + prio_mid = (prio_min + prio_max) / 2; + + sparam.sched_priority = prio_mid; + status = pthread_attr_setschedparam(&attr,&sparam); + if (status != OK) + { + printf("cond_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("cond_test: Set thread 1 priority to %d\n", sparam.sched_priority); + } + + status = pthread_create(&waiter, &attr, thread_waiter, NULL); + if (status != 0) + { + printf("cond_test: pthread_create failed, status=%d\n", status); + } + + printf("cond_test: Starting signaler\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("cond_test: pthread_attr_init failed, status=%d\n", status); + } + + sparam.sched_priority = (prio_min + prio_mid) / 2; + status = pthread_attr_setschedparam(&attr,&sparam); + if (status != OK) + { + printf("cond_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("cond_test: Set thread 2 priority to %d\n", sparam.sched_priority); + } + + status = pthread_create(&signaler, &attr, thread_signaler, NULL); + if (status != 0) + { + printf("cond_test: pthread_create failed, status=%d\n", status); + } + + /* Wait for the threads to stop */ + +#ifdef SDCC + pthread_join(signaler, &result); +#else + pthread_join(signaler, NULL); +#endif + printf("cond_test: signaler terminated, now cancel the waiter\n"); + pthread_detach(waiter); + pthread_cancel(waiter); + + printf("cond_test: \tWaiter\tSignaler\n"); + printf("cond_test: Loops\t%d\t%d\n", waiter_nloops, signaler_nloops); + printf("cond_test: Errors\t%d\t%d\n", waiter_nerrors, signaler_nerrors); + printf("cond_test:\n"); + printf("cond_test: %d times, waiter did not have to wait for data\n", waiter_nloops - waiter_waits); + printf("cond_test: %d times, data was already available when the signaler run\n", signaler_already); + printf("cond_test: %d times, the waiter was in an unexpected state when the signaler ran\n", signaler_state); +} diff --git a/apps/examples/ostest/dev_null.c b/apps/examples/ostest/dev_null.c new file mode 100644 index 000000000..e8fc6cf3f --- /dev/null +++ b/apps/examples/ostest/dev_null.c @@ -0,0 +1,92 @@ +/**************************************************************************** + * examples/ostest/dev_null.c + * + * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <stdio.h> +#include <unistd.h> +#include <fcntl.h> +#include "ostest.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 + +static FAR char buffer[1024]; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int dev_null(void) +{ + int nbytes; + int fd; + + fd = open("/dev/null", O_RDWR); + if (fd < 0) + { + printf("dev_null: ERROR Failed to open /dev/null\n"); + return -1; + } + + nbytes = read(fd, buffer, 1024); + if (nbytes < 0) + { + printf("dev_null: ERROR Failed to read from /dev/null\n"); + close(fd); + return -1; + } + printf("dev_null: Read %d bytes from /dev/null\n", nbytes); + + nbytes = write(fd, buffer, 1024); + if (nbytes < 0) + { + printf("dev_null: ERROR Failed to write to /dev/null\n"); + close(fd); + return -1; + } + printf("dev_null: Wrote %d bytes to /dev/null\n", nbytes); + + close(fd); + return 0; +} + +#endif /*CONFIG_NFILE_DESCRIPTORS */ diff --git a/apps/examples/ostest/fpu.c b/apps/examples/ostest/fpu.c new file mode 100644 index 000000000..89a1034ce --- /dev/null +++ b/apps/examples/ostest/fpu.c @@ -0,0 +1,344 @@ +/*********************************************************************** + * apps/examples/ostest/fpu.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +/*********************************************************************** + * Included Files + ***********************************************************************/ + +#include <nuttx/config.h> +#include <sys/wait.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <sched.h> + +#include "ostest.h" + +/*********************************************************************** + * Pre-processor definitions + ***********************************************************************/ +/* Configuration *******************************************************/ + +#undef HAVE_FPU +#ifdef CONFIG_ARCH_FPU +# if defined(CONFIG_EXAMPLES_OSTEST_FPUSIZE) && defined(CONFIG_SCHED_WAITPID) && !defined(CONFIG_DISABLE_SIGNALS) +# define HAVE_FPU 1 +# else +# ifndef CONFIG_EXAMPLES_OSTEST_FPUSIZE +# warning "FPU test not built; CONFIG_EXAMPLES_OSTEST_FPUSIZE not defined" +# endif +# ifndef CONFIG_SCHED_WAITPID +# warning "FPU test not built; CONFIG_SCHED_WAITPID not defined" +# endif +# ifdef CONFIG_DISABLE_SIGNALS +# warning "FPU test not built; CONFIG_DISABLE_SIGNALS defined" +# endif +# endif +#endif + +#ifdef HAVE_FPU + +#ifndef CONFIG_EXAMPLES_OSTEST_FPULOOPS +# define CONFIG_EXAMPLES_OSTEST_FPULOOPS 16 +#endif + +#ifndef CONFIG_EXAMPLES_OSTEST_FPUMSDELAY +# define CONFIG_EXAMPLES_OSTEST_FPUMSDELAY 750 +#endif + +#ifndef CONFIG_EXAMPLES_OSTEST_FPUPRIORITY +# define CONFIG_EXAMPLES_OSTEST_FPUPRIORITY SCHED_PRIORITY_DEFAULT +#endif + +#ifndef CONFIG_EXAMPLES_OSTEST_FPUSTACKSIZE +# define CONFIG_EXAMPLES_OSTEST_FPUSTACKSIZE 2048 +#endif + +/* Other defintions ****************************************************/ +/* We'll keep all data using 32-bit values only to force 32-bit alignment. + * This logic has no real notion of the underlying representation. + */ + +#define FPU_WORDSIZE ((CONFIG_EXAMPLES_OSTEST_FPUSIZE+3)>>2) +#define FPU_NTHREADS 2 + +#ifndef NULL +# define NULL (void*)0 +#endif + +/*********************************************************************** + * External Dependencies + ***********************************************************************/ +/* This test is very dependent on support provided by the chip/board- + * layer logic. In particular, it expects the following functions + * to be provided: + */ + +/* Given an array of size CONFIG_EXAMPLES_OSTEST_FPUSIZE, this function + * will return the current FPU registers. + */ + +extern void arch_getfpu(FAR uint32_t *fpusave); + +/* Given two arrays of size CONFIG_EXAMPLES_OSTEST_FPUSIZE this + * function will compare them and return true if they are identical. + */ + +extern bool arch_cmpfpu(FAR const uint32_t *fpusave1, + FAR const uint32_t *fpusave2); + +/*********************************************************************** + * Private Types + ***********************************************************************/ + +struct fpu_threaddata_s +{ + uint32_t save1[FPU_WORDSIZE]; + uint32_t save2[FPU_WORDSIZE]; + + /* These are just dummy values to force the compiler to do the + * requested floating point computations without the nonsense + * computations being optimized away. + */ + + volatile float sp1; + volatile float sp2; + volatile float sp3; + volatile float sp4; + + volatile float dp1; + volatile float dp2; + volatile float dp3; + volatile float dp4; +}; + +/*********************************************************************** + * Private Data + ***********************************************************************/ + +static uint8_t g_fpuno; +/* static */ struct fpu_threaddata_s g_fputhread[FPU_NTHREADS]; + +/*********************************************************************** + * Private Functions + ***********************************************************************/ + +static void fpu_dump(FAR uint32_t *buffer, FAR const char *msg) +{ + int i, j, k; + + printf("%s (%p):\n", msg, buffer); + for (i = 0; i < FPU_WORDSIZE; i += 8) + { + printf(" %04x: ", i); + for (j = 0; j < 8; j++) + { + k = i + j; + + if (k < FPU_WORDSIZE) + { + printf("%08x ", buffer[k]); + } + else + { + printf("\n"); + break; + } + } + printf("\n"); + } +} + +static int fpu_task(int argc, char *argv[]) +{ + FAR struct fpu_threaddata_s *fpu; + register float sp1; + register float sp2; + register float sp3; + register float sp4; + register double dp1; + register double dp2; + register double dp3; + register double dp4; + + int id; + int i; + + /* Which are we? */ + + sched_lock(); + fpu = &g_fputhread[g_fpuno]; + id = (int)(++g_fpuno); + sched_unlock(); + + /* Seed the flowing point values */ + + sp1 = (float)id; + dp1 = (double)id; + + for (i = 0; i < CONFIG_EXAMPLES_OSTEST_FPULOOPS; i++) + { + printf("FPU#%d: pass %d\n", id, i+1); + fflush(stdout); + + /* Set the FPU register save arrays to a known-but-illogical values so + * that we can verify that reading of the registers actually occurs. + */ + + memset(fpu->save1, 0xff, FPU_WORDSIZE * sizeof(uint32_t)); + memset(fpu->save2, 0xff, FPU_WORDSIZE * sizeof(uint32_t)); + + /* Prevent context switches while we set up some stuff */ + + sched_lock(); + + /* Do some trivial floating point operations that should cause some + * changes to floating point registers. First, some single preceision + * nonsense. + */ + + sp4 = (float)3.14159 * sp1; /* Multiple by Pi */ + sp3 = sp4 + (float)1.61803; /* Add the golden ratio */ + sp2 = sp3 / (float)2.71828; /* Divide by Euler's constant */ + sp1 = sp2 + (float)1.0; /* Plus one */ + + fpu->sp1 = sp1; /* Make the compiler believe that somebody cares about the result */ + fpu->sp2 = sp2; + fpu->sp3 = sp3; + fpu->sp4 = sp4; + + /* Again using double precision */ + + dp4 = (double)3.14159 * dp1; /* Multiple by Pi */ + dp3 = dp4 + (double)1.61803; /* Add the golden ratio */ + dp2 = dp3 / (double)2.71828; /* Divide by Euler's constant */ + dp1 = dp2 + (double)1.0; /* Plus one */ + + fpu->dp1 = dp1; /* Make the compiler believe that somebody cares about the result */ + fpu->dp2 = dp2; + fpu->dp3 = dp3; + fpu->dp4 = dp4; + + /* Sample the floating point registers */ + + arch_getfpu(fpu->save1); + + /* Re-read and verify the FPU registers consistently without corruption */ + + arch_getfpu(fpu->save2); + if (!arch_cmpfpu(fpu->save1, fpu->save2)) + { + printf("ERROR FPU#%d: save1 and save2 do not match\n", id); + fpu_dump(fpu->save1, "Values after math operations (save1)"); + fpu_dump(fpu->save2, "Values after verify re-read (save2)"); + return EXIT_FAILURE; + } + + /* Now unlock and sleep for a while -- this should result in some context switches */ + + sched_unlock(); + usleep(CONFIG_EXAMPLES_OSTEST_FPUMSDELAY * 1000); + + /* Several context switches should have occurred. Now verify that the floating + * point registers are still correctly set. + */ + + arch_getfpu(fpu->save2); + if (!arch_cmpfpu(fpu->save1, fpu->save2)) + { + printf("ERROR FPU#%d: save1 and save2 do not match\n", id); + fpu_dump(fpu->save1, "Values before waiting (save1)"); + fpu_dump(fpu->save2, "Values after waiting (save2)"); + return EXIT_FAILURE; + } + } + + printf("FPU#%d: Succeeded\n", id); + fflush(stdout); + return EXIT_SUCCESS; +} +#endif /* HAVE_FPU */ + +/*********************************************************************** + * Private Functions + ***********************************************************************/ + +void fpu_test(void) +{ +#ifdef HAVE_FPU + pid_t task1; + pid_t task2; + int statloc; + + /* Start two two tasks */ + + g_fpuno = 0; + printf("Starting task FPU#1\n"); + task1 = TASK_CREATE("FPU#1", CONFIG_EXAMPLES_OSTEST_FPUPRIORITY, CONFIG_EXAMPLES_OSTEST_FPUSTACKSIZE, fpu_task, NULL); + if (task1 < 0) + { + printf("fpu_test: ERROR Failed to start task FPU#1\n"); + } + else + { + printf("fpu_test: Started task FPU#1 at PID=%d\n", task1); + } + fflush(stdout); + usleep(250); + + printf("Starting task FPU#2\n"); + task2 = TASK_CREATE("FPU#2", CONFIG_EXAMPLES_OSTEST_FPUPRIORITY, CONFIG_EXAMPLES_OSTEST_FPUSTACKSIZE, fpu_task, NULL); + if (task2 < 0) + { + printf("fpu_test: ERROR Failed to start task FPU#1\n"); + } + else + { + printf("fpu_test: Started task FPU#2 at PID=%d\n", task2); + } + + /* Wait for each task to complete */ + + fflush(stdout); + (void)waitpid(task1, &statloc, 0); + (void)waitpid(task2, &statloc, 0); + +#else + printf("fpu_test: ERROR: The FPU test is not properly configured\n"); +#endif + printf("fpu_test: Returning\n"); +} diff --git a/apps/examples/ostest/main.c b/apps/examples/ostest/main.c new file mode 100644 index 000000000..7d63c0ff4 --- /dev/null +++ b/apps/examples/ostest/main.c @@ -0,0 +1,530 @@ +/**************************************************************************** + * apps/examples/ostest/main.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <sched.h> +#include <nuttx/init.h> + +#include "ostest.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define PRIORITY 100 +#define NARGS 4 +#define HALF_SECOND_USEC 500000L + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const char arg1[] = "Arg1"; +static const char arg2[] = "Arg2"; +static const char arg3[] = "Arg3"; +static const char arg4[] = "Arg4"; + +#if CONFIG_NFILE_DESCRIPTORS > 0 +static const char write_data1[] = "stdio_test: write fd=1\n"; +static const char write_data2[] = "stdio_test: write fd=2\n"; +#endif + +#ifdef SDCC +/* I am not yet certain why SDCC does not like the following + * initializer. It involves some issues with 2- vs 3-byte + * pointer types. + */ + +static const char *g_argv[NARGS+1]; +#else +static const char *g_argv[NARGS+1] = { arg1, arg2, arg3, arg4, NULL }; +#endif + +#ifndef CONFIG_DISABLE_SIGNALS +static struct mallinfo g_mmbefore; +static struct mallinfo g_mmprevious; +static struct mallinfo g_mmafter; +#endif + +#ifndef CONFIG_DISABLE_ENVIRON +const char g_var1_name[] = "Variable1"; +const char g_var1_value[] = "GoodValue1"; +const char g_var2_name[] = "Variable2"; +const char g_var2_value[] = "GoodValue2"; +const char g_var3_name[] = "Variable3"; +const char g_var3_value[] = "GoodValue3"; + +const char g_bad_value1[] = "BadValue1"; +const char g_bad_value2[] = "BadValue2"; + +const char g_putenv_value[] = "Variable1=BadValue3"; + +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: show_memory_usage + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_SIGNALS +static void show_memory_usage(struct mallinfo *mmbefore, + struct mallinfo *mmafter) +{ + printf("VARIABLE BEFORE AFTER\n"); + printf("======== ======== ========\n"); + printf("arena %8x %8x\n", mmbefore->arena, mmafter->arena); + printf("ordblks %8d %8d\n", mmbefore->ordblks, mmafter->ordblks); + printf("mxordblk %8x %8x\n", mmbefore->mxordblk, mmafter->mxordblk); + printf("uordblks %8x %8x\n", mmbefore->uordblks, mmafter->uordblks); + printf("fordblks %8x %8x\n", mmbefore->fordblks, mmafter->fordblks); +} +#else +# define show_memory_usage(mm1, mm2) +#endif + +/**************************************************************************** + * Name: check_test_memory_usage + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_SIGNALS +static void check_test_memory_usage(void) +{ + /* Wait a little bit to let any threads terminate */ + + usleep(HALF_SECOND_USEC); + + /* Get the current memory usage */ + +#ifdef CONFIG_CAN_PASS_STRUCTS + g_mmafter = mallinfo(); +#else + (void)mallinfo(&g_mmafter); +#endif + + /* Show the change from the previous time */ + + printf("\nEnd of test memory usage:\n"); + show_memory_usage(&g_mmprevious, &g_mmafter); + + /* Set up for the next test */ + +#ifdef CONFIG_CAN_PASS_STRUCTS + g_mmprevious = g_mmafter; +#else + memcpy(&g_mmprevious, &g_mmafter, sizeof(struct mallinfo)); +#endif + + /* If so enabled, show the use of priority inheritance resources */ + + dump_nfreeholders("user_main:"); +} +#else +# define check_test_memory_usage() +#endif + +/**************************************************************************** + * Name: show_variable + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_ENVIRON +static void show_variable(const char *var_name, const char *exptd_value, bool var_valid) +{ + char *actual_value = getenv(var_name); + if (actual_value) + { + if (var_valid) + { + if (strcmp(actual_value, exptd_value) == 0) + { + printf("show_variable: Variable=%s has value=%s\n", var_name, exptd_value); + } + else + { + printf("show_variable: ERROR Variable=%s has the wrong value\n", var_name); + printf("show_variable: found=%s expected=%s\n", actual_value, exptd_value); + } + } + else + { + printf("show_variable: ERROR Variable=%s has a value when it should not\n", var_name); + printf("show_variable: value=%s\n", actual_value); + } + } + else if (var_valid) + { + printf("show_variable: ERROR Variable=%s has no value\n", var_name); + printf("show_variable: Should have had value=%s\n", exptd_value); + } + else + { + printf("show_variable: Variable=%s has no value\n", var_name); + } +} + +static void show_environment(bool var1_valid, bool var2_valid, bool var3_valid) +{ + show_variable(g_var1_name, g_var1_value, var1_valid); + show_variable(g_var2_name, g_var2_value, var2_valid); + show_variable(g_var3_name, g_var3_value, var3_valid); +} +#else +# define show_environment() +#endif + +/**************************************************************************** + * Name: user_main + ****************************************************************************/ + +static int user_main(int argc, char *argv[]) +{ + int i; + + /* Sample the memory usage now */ + +#ifndef CONFIG_DISABLE_SIGNALS + usleep(HALF_SECOND_USEC); + +#ifdef CONFIG_CAN_PASS_STRUCTS + g_mmbefore = mallinfo(); + g_mmprevious = g_mmbefore; +#else + (void)mallinfo(&g_mmbefore); + memcpy(&g_mmprevious, &g_mmbefore, sizeof(struct mallinfo)); +#endif +#endif + + printf("\nuser_main: Begin argument test\n"); + printf("user_main: Started with argc=%d\n", argc); + + /* Verify passed arguments */ + + if (argc != NARGS + 1) + { + printf("user_main: Error expected argc=%d got argc=%d\n", + NARGS+1, argc); + } + + for (i = 0; i <= NARGS; i++) + { + printf("user_main: argv[%d]=\"%s\"\n", i, argv[i]); + } + + for (i = 1; i <= NARGS; i++) + { + if (strcmp(argv[i], g_argv[i-1]) != 0) + { + printf("user_main: ERROR argv[%d]: Expected \"%s\" found \"%s\"\n", + i, g_argv[i-1], argv[i]); + } + } + check_test_memory_usage(); + + /* Check environment variables */ +#ifndef CONFIG_DISABLE_ENVIRON + show_environment(true, true, true); + + unsetenv(g_var1_name); + show_environment(false, true, true); + check_test_memory_usage(); + + clearenv(); + show_environment(false, false, false); + check_test_memory_usage(); +#endif + + /* Top of test loop */ + +#if CONFIG_EXAMPLES_OSTEST_LOOPS > 1 + for (i = 0; i < CONFIG_EXAMPLES_OSTEST_LOOPS; i++) +#elif CONFIG_EXAMPLES_OSTEST_LOOPS == 0 + for (;;) +#endif + { +#if CONFIG_NFILE_DESCRIPTORS > 0 + /* Checkout /dev/null */ + + printf("\nuser_main: /dev/null test\n"); + dev_null(); + check_test_memory_usage(); +#endif + +#ifdef CONFIG_ARCH_FPU + /* Check that the FPU is properly supported during context switching */ + + printf("\nuser_main: FPU test\n"); + fpu_test(); + check_test_memory_usage(); +#endif + +#ifndef CONFIG_DISABLE_PTHREAD + /* Verify pthreads and pthread mutex */ + + printf("\nuser_main: mutex test\n"); + mutex_test(); + check_test_memory_usage(); +#endif + +#if !defined(CONFIG_DISABLE_PTHREAD) && defined(CONFIG_MUTEX_TYPES) + /* Verify recursive mutexes */ + + printf("\nuser_main: recursive mutex test\n"); + recursive_mutex_test(); + check_test_memory_usage(); +#endif + +#ifndef CONFIG_DISABLE_PTHREAD + /* Verify pthread cancellation */ + + printf("\nuser_main: cancel test\n"); + cancel_test(); + check_test_memory_usage(); +#endif + +#ifndef CONFIG_DISABLE_PTHREAD + /* Verify pthreads and semaphores */ + + printf("\nuser_main: semaphore test\n"); + sem_test(); + check_test_memory_usage(); +#endif + +#ifndef CONFIG_DISABLE_PTHREAD + /* Verify pthreads and condition variables */ + + printf("\nuser_main: condition variable test\n"); +#ifdef CONFIG_PRIORITY_INHERITANCE + printf("\n Skipping, Test logic incompatible with priority inheritance\n"); +#else + cond_test(); + check_test_memory_usage(); +#endif +#endif + +#if !defined(CONFIG_DISABLE_SIGNALS) && !defined(CONFIG_DISABLE_PTHREAD) && !defined(CONFIG_DISABLE_CLOCK) + /* Verify pthreads and condition variable timed waits */ + + printf("\nuser_main: timed wait test\n"); + timedwait_test(); + check_test_memory_usage(); +#endif + +#if !defined(CONFIG_DISABLE_MQUEUE) && !defined(CONFIG_DISABLE_PTHREAD) + /* Verify pthreads and message queues */ + + printf("\nuser_main: message queue test\n"); + mqueue_test(); + check_test_memory_usage(); +#endif + +#if !defined(CONFIG_DISABLE_MQUEUE) && !defined(CONFIG_DISABLE_PTHREAD) && !defined(CONFIG_DISABLE_CLOCK) + /* Verify pthreads and message queues */ + + printf("\nuser_main: timed message queue test\n"); + timedmqueue_test(); + check_test_memory_usage(); +#endif + +#ifndef CONFIG_DISABLE_SIGNALS + /* Verify signal handlers */ + + printf("\nuser_main: signal handler test\n"); + sighand_test(); + check_test_memory_usage(); +#endif + +#if !defined(CONFIG_DISABLE_POSIX_TIMERS) && !defined(CONFIG_DISABLE_SIGNALS) + /* Verify posix timers */ + + printf("\nuser_main: POSIX timer test\n"); + timer_test(); + check_test_memory_usage(); +#endif + +#if !defined(CONFIG_DISABLE_PTHREAD) && CONFIG_RR_INTERVAL > 0 + /* Verify round robin scheduling */ + + printf("\nuser_main: round-robin scheduler test\n"); + rr_test(); + check_test_memory_usage(); +#endif + +#ifndef CONFIG_DISABLE_PTHREAD + /* Verify pthread barriers */ + + printf("\nuser_main: barrier test\n"); + barrier_test(); + check_test_memory_usage(); +#endif + +#if defined(CONFIG_PRIORITY_INHERITANCE) && !defined(CONFIG_DISABLE_SIGNALS) && !defined(CONFIG_DISABLE_PTHREAD) + /* Verify priority inheritance */ + + printf("\nuser_main: priority inheritance test\n"); + priority_inheritance(); + check_test_memory_usage(); +#endif /* CONFIG_PRIORITY_INHERITANCE && !CONFIG_DISABLE_SIGNALS && !CONFIG_DISABLE_PTHREAD */ + + /* Compare memory usage at time user_start started until + * user_main exits. These should not be identical, but should + * be similar enough that we can detect any serious OS memory + * leaks. + */ + +#ifndef CONFIG_DISABLE_SIGNALS + usleep(HALF_SECOND_USEC); + +#ifdef CONFIG_CAN_PASS_STRUCTS + g_mmafter = mallinfo(); +#else + (void)mallinfo(&g_mmafter); +#endif + + printf("\nFinal memory usage:\n"); + show_memory_usage(&g_mmbefore, &g_mmafter); +#endif + } + printf("user_main: Exitting\n"); + return 0; +} + +/**************************************************************************** + * Name: stdio_test + ****************************************************************************/ + +static void stdio_test(void) +{ + /* Verify that we can communicate */ + +#if CONFIG_NFILE_DESCRIPTORS > 0 + write(1, write_data1, sizeof(write_data1)-1); +#endif + printf("stdio_test: Standard I/O Check: printf\n"); + +#if CONFIG_NFILE_DESCRIPTORS > 1 + write(2, write_data2, sizeof(write_data2)-1); +#endif +#if CONFIG_NFILE_STREAMS > 0 + fprintf(stderr, "stdio_test: Standard I/O Check: fprintf to stderr\n"); +#endif +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * user_start/ostest_main + ****************************************************************************/ + +#ifdef CONFIG_EXAMPLES_OSTEST_BUILTIN +# define MAIN_NAME ostest_main +# define MAIN_STRING "ostest_main: " +#else +# define MAIN_NAME user_start +# define MAIN_STRING "user_start: " +#endif + +int MAIN_NAME(int argc, char *argv[]) +{ + int result; + + /* Verify that stdio works first */ + + stdio_test(); + +#ifdef SDCC + /* I am not yet certain why SDCC does not like the following initilizers. + * It involves some issues with 2- vs 3-byte pointer types. + */ + + g_argv[0] = arg1; + g_argv[1] = arg2; + g_argv[2] = arg3; + g_argv[3] = arg4; + g_argv[4] = NULL; +#endif + + /* Set up some environment variables */ + +#ifndef CONFIG_DISABLE_ENVIRON + printf(MAIN_STRING "putenv(%s)\n", g_putenv_value); + putenv(g_putenv_value); /* Varaible1=BadValue3 */ + printf(MAIN_STRING "setenv(%s, %s, TRUE)\n", g_var1_name, g_var1_value); + setenv(g_var1_name, g_var1_value, TRUE); /* Variable1=GoodValue1 */ + + printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var2_name, g_bad_value1); + setenv(g_var2_name, g_bad_value1, FALSE); /* Variable2=BadValue1 */ + printf(MAIN_STRING "setenv(%s, %s, TRUE)\n", g_var2_name, g_var2_value); + setenv(g_var2_name, g_var2_value, TRUE); /* Variable2=GoodValue2 */ + + printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name); + setenv(g_var3_name, g_var3_value, FALSE); /* Variable3=GoodValue3 */ + printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name); + setenv(g_var3_name, g_bad_value2, FALSE); /* Variable3=GoodValue3 */ + show_environment(true, true, true); +#endif + + /* Verify that we can spawn a new task */ + +#ifndef CONFIG_CUSTOM_STACK + result = task_create("ostest", PRIORITY, STACKSIZE, user_main, g_argv); +#else + result = task_create("ostest", PRIORITY, user_main, g_argv); +#endif + if (result == ERROR) + { + printf(MAIN_STRING "ERROR Failed to start user_main\n"); + } + else + { + printf(MAIN_STRING "Started user_main at PID=%d\n", result); + } + + printf(MAIN_STRING "Exitting\n"); + return 0; +} diff --git a/apps/examples/ostest/mqueue.c b/apps/examples/ostest/mqueue.c new file mode 100644 index 000000000..39ef76a53 --- /dev/null +++ b/apps/examples/ostest/mqueue.c @@ -0,0 +1,394 @@ +/************************************************************************** + * apps/examples/ostest/mqueue.c + * + * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + **************************************************************************/ + +/************************************************************************** + * Included Files + **************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <ctype.h> +#include <fcntl.h> +#include <pthread.h> +#include <mqueue.h> +#include <sched.h> +#include <errno.h> + +#include "ostest.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +#define TEST_MESSAGE "This is a test and only a test" +#if defined(SDCC) || defined(__ZILOG__) + /* Cannot use strlen in array size */ + +# define TEST_MSGLEN (31) +#else + /* Message lenght is the size of the message plus the null terminator */ + +# define TEST_MSGLEN (strlen(TEST_MESSAGE)+1) +#endif + +#define TEST_SEND_NMSGS (10) +#ifndef CONFIG_DISABLE_SIGNALS +# define TEST_RECEIVE_NMSGS (11) +#else +# define TEST_RECEIVE_NMSGS (10) +#endif + +#define HALF_SECOND_USEC_USEC 500000L + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/************************************************************************** + * Global Variables + **************************************************************************/ + +/************************************************************************** + * Private Variables + **************************************************************************/ + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Public Functions + **************************************************************************/ + +static void *sender_thread(void *arg) +{ + mqd_t mqfd; + char msg_buffer[TEST_MSGLEN]; + struct mq_attr attr; + int status = 0; + int nerrors = 0; + int i; + + printf("sender_thread: Starting\n"); + + /* Fill in attributes for message queue */ + + attr.mq_maxmsg = 20; + attr.mq_msgsize = TEST_MSGLEN; + attr.mq_flags = 0; + + /* Set the flags for the open of the queue. + * Make it a blocking open on the queue, meaning it will block if + * this process tries to send to the queue and the queue is full. + * + * O_CREAT - the queue will get created if it does not already exist. + * O_WRONLY - we are only planning to write to the queue. + * + * Open the queue, and create it if the receiving process hasn't + * already created it. + */ + + mqfd = mq_open("testmq", O_WRONLY|O_CREAT, 0666, &attr); + if (mqfd < 0) + { + printf("sender_thread: ERROR mq_open failed\n"); + pthread_exit((pthread_addr_t)1); + } + + /* Fill in a test message buffer to send */ + + memcpy(msg_buffer, TEST_MESSAGE, TEST_MSGLEN); + + /* Perform the send TEST_SEND_NMSGS times */ + + for (i = 0; i < TEST_SEND_NMSGS; i++) + { + status = mq_send(mqfd, msg_buffer, TEST_MSGLEN, 42); + if (status < 0) + { + printf("sender_thread: ERROR mq_send failure=%d on msg %d\n", status, i); + nerrors++; + } + else + { + printf("sender_thread: mq_send succeeded on msg %d\n", i); + } + } + + /* Close the queue and return success */ + + if (mq_close(mqfd) < 0) + { + printf("sender_thread: ERROR mq_close failed\n"); + } + + printf("sender_thread: returning nerrors=%d\n", nerrors); + return (pthread_addr_t)nerrors; +} + +static void *receiver_thread(void *arg) +{ + mqd_t mqfd; + char msg_buffer[TEST_MSGLEN]; + struct mq_attr attr; + int nbytes; + int nerrors = 0; + int i; + + printf("receiver_thread: Starting\n"); + + /* Fill in attributes for message queue */ + + attr.mq_maxmsg = 20; + attr.mq_msgsize = TEST_MSGLEN; + attr.mq_flags = 0; + + /* Set the flags for the open of the queue. + * Make it a blocking open on the queue, meaning it will block if + * this task tries to read from the queue when the queue is empty + * + * O_CREAT - the queue will get created if it does not already exist. + * O_RDONLY - we are only planning to read from the queue. + * + * Open the queue, and create it if the sending process hasn't + * already created it. + */ + + mqfd = mq_open("testmq", O_RDONLY|O_CREAT, 0666, &attr); + if (mqfd < 0) + { + printf("receiver_thread: ERROR mq_open failed\n"); + pthread_exit((pthread_addr_t)1); + } + + /* Perform the receive TEST_RECEIVE_NMSGS times */ + + for (i = 0; i < TEST_RECEIVE_NMSGS; i++) + { + memset(msg_buffer, 0xaa, TEST_MSGLEN); + nbytes = mq_receive(mqfd, msg_buffer, TEST_MSGLEN, 0); + if (nbytes < 0) + { + /* mq_receive failed. If the error is because of EINTR then + * it is not a failure. + */ + + if (errno != EINTR) + { + printf("receiver_thread: ERROR mq_receive failure on msg %d, errno=%d\n", i, errno); + nerrors++; + } + else + { + printf("receiver_thread: mq_receive interrupted!\n"); + } + } + else if (nbytes != TEST_MSGLEN) + { + printf("receiver_thread: mq_receive return bad size %d on msg %d\n", nbytes, i); + nerrors++; + } + else if (memcmp(TEST_MESSAGE, msg_buffer, nbytes) != 0) + { + int j; + + printf("receiver_thread: mq_receive returned corrupt message on msg %d\n", i); + printf("receiver_thread: i Expected Received\n"); + + for (j = 0; j < TEST_MSGLEN-1; j++) + { + if (isprint(msg_buffer[j])) + { + printf("receiver_thread: %2d %02x (%c) %02x (%c)\n", + j, TEST_MESSAGE[j], TEST_MESSAGE[j], msg_buffer[j], msg_buffer[j]); + } + else + { + printf("receiver_thread: %2d %02x (%c) %02x\n", + j, TEST_MESSAGE[j], TEST_MESSAGE[j], msg_buffer[j]); + } + } + printf("receiver_thread: %2d 00 %02x\n", + j, msg_buffer[j]); + } + else + { + printf("receiver_thread: mq_receive succeeded on msg %d\n", i); + } + } + + /* Close the queue and return success */ + + if (mq_close(mqfd) < 0) + { + printf("receiver_thread: ERROR mq_close failed\n"); + nerrors++; + } + + /* Destroy the queue */ + + if (mq_unlink("testmq") < 0) + { + printf("receiver_thread: ERROR mq_close failed\n"); + nerrors++; + } + + printf("receiver_thread: returning nerrors=%d\n", nerrors); + pthread_exit((pthread_addr_t)nerrors); + return (pthread_addr_t)nerrors; +} + +void mqueue_test(void) +{ + pthread_t sender; + pthread_t receiver; + void *result; + pthread_attr_t attr; + struct sched_param sparam; + int prio_min; + int prio_max; + int prio_mid; + int status; + + /* Start the sending thread at higher priority */ + + printf("mqueue_test: Starting receiver\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("mqueue_test: pthread_attr_init failed, status=%d\n", status); + } + + status = pthread_attr_setstacksize(&attr, STACKSIZE); + if (status != 0) + { + printf("mqueue_test: pthread_attr_setstacksize failed, status=%d\n", status); + } + + prio_min = sched_get_priority_min(SCHED_FIFO); + prio_max = sched_get_priority_max(SCHED_FIFO); + prio_mid = (prio_min + prio_max) / 2; + + sparam.sched_priority = prio_mid; + status = pthread_attr_setschedparam(&attr,&sparam); + if (status != OK) + { + printf("mqueue_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("mqueue_test: Set receiver priority to %d\n", sparam.sched_priority); + } + + status = pthread_create(&receiver, &attr, receiver_thread, NULL); + if (status != 0) + { + printf("mqueue_test: pthread_create failed, status=%d\n", status); + } + + /* Start the sending thread at lower priority */ + + printf("mqueue_test: Starting sender\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("mqueue_test: pthread_attr_init failed, status=%d\n", status); + } + + status = pthread_attr_setstacksize(&attr, STACKSIZE); + if (status != 0) + { + printf("mqueue_test: pthread_attr_setstacksize failed, status=%d\n", status); + } + + sparam.sched_priority = (prio_min + prio_mid) / 2; + status = pthread_attr_setschedparam(&attr,&sparam); + if (status != OK) + { + printf("mqueue_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("mqueue_test: Set sender thread priority to %d\n", sparam.sched_priority); + } + + status = pthread_create(&sender, &attr, sender_thread, NULL); + if (status != 0) + { + printf("mqueue_test: pthread_create failed, status=%d\n", status); + } + + printf("mqueue_test: Waiting for sender to complete\n"); + pthread_join(sender, &result); + if (result != (void*)0) + { + printf("mqueue_test: ERROR sender thread exited with %d errors\n", (int)result); + } + +#ifndef CONFIG_DISABLE_SIGNALS + /* Wake up the receiver thread with a signal */ + + printf("mqueue_test: Killing receiver\n"); + pthread_kill(receiver, 9); + + /* Wait a bit to see if the thread exits on its own */ + + usleep(HALF_SECOND_USEC_USEC); +#endif + + /* Then cancel the thread and see if it did */ + + printf("mqueue_test: Canceling receiver\n"); + status = pthread_cancel(receiver); + if (status == ESRCH) + { + printf("mqueue_test: receiver has already terminated\n"); + } + + pthread_join(receiver, &result); + if (result != (void*)0) + { + printf("mqueue_test: ERROR receiver thread exited with %d errors\n", (int)result); + } +} + + diff --git a/apps/examples/ostest/mutex.c b/apps/examples/ostest/mutex.c new file mode 100644 index 000000000..752f833f2 --- /dev/null +++ b/apps/examples/ostest/mutex.c @@ -0,0 +1,142 @@ +/*********************************************************************** + * mutex.c + * + * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +#include <stdio.h> +#include <pthread.h> +#include "ostest.h" + +#ifndef NULL +# define NULL (void*)0 +#endif + +#define NLOOPS 32 + +static pthread_mutex_t mut; +static volatile int my_mutex = 0; +static unsigned long nloops[2] = {0, 0}; +static unsigned long nerrors[2] = {0, 0}; + +static void *thread_func(void *parameter) +{ + int id = (int)parameter; + int ndx = id - 1; + int i; + + for (nloops[ndx] = 0; nloops[ndx] < NLOOPS; nloops[ndx]++) + { + int status = pthread_mutex_lock(&mut); + if (status != 0) + { + printf("ERROR thread %d: pthread_mutex_lock failed, status=%d\n", + id, status); + } + + if (my_mutex == 1) + { + printf("ERROR thread=%d: " + "my_mutex should be zero, instead my_mutex=%d\n", + id, my_mutex); + nerrors[ndx]++; + } + + my_mutex = 1; + for (i = 0; i < 10; i++) + { + pthread_yield(); + } + my_mutex = 0; + + status = pthread_mutex_unlock(&mut); + if (status != 0) + { + printf("ERROR thread %d: pthread_mutex_unlock failed, status=%d\n", + id, status); + } + } + pthread_exit(NULL); + return NULL; /* Non-reachable -- needed for some compilers */ +} + +void mutex_test(void) +{ + pthread_t thread1, thread2; +#ifdef SDCC + pthread_addr_t result1, result2; + pthread_attr_t attr; +#endif + int status; + + /* Initialize the mutex */ + + printf("Initializing mutex\n"); + pthread_mutex_init(&mut, NULL); + + /* Start two thread instances */ + + printf("Starting thread 1\n"); +#ifdef SDCC + (void)pthread_attr_init(&attr); + status = pthread_create(&thread1, &attr, thread_func, (pthread_addr_t)1); +#else + status = pthread_create(&thread1, NULL, thread_func, (pthread_addr_t)1); +#endif + if (status != 0) + { + printf("Error in thread#1 creation\n"); + } + + printf("Starting thread 2\n"); +#ifdef SDCC + status = pthread_create(&thread2, &attr, thread_func, (pthread_addr_t)2); +#else + status = pthread_create(&thread2, NULL, thread_func, (pthread_addr_t)2); +#endif + if (status != 0) + { + printf("Error in thread#2 creation\n"); + } + +#ifdef SDCC + pthread_join(thread1, &result1); + pthread_join(thread2, &result2); +#else + pthread_join(thread1, NULL); + pthread_join(thread2, NULL); +#endif + + printf("\t\tThread1\tThread2\n"); + printf("\tLoops\t%ld\t%ld\n", nloops[0], nloops[1]); + printf("\tErrors\t%ld\t%ld\n", nerrors[0], nerrors[1]); +} diff --git a/apps/examples/ostest/ostest.h b/apps/examples/ostest/ostest.h new file mode 100644 index 000000000..a4af37f05 --- /dev/null +++ b/apps/examples/ostest/ostest.h @@ -0,0 +1,178 @@ +/**************************************************************************** + * apps/examples/ostest/ostest.h + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_EXAMPLES_OSTEST_OSTEST_H +#define __APPS_EXAMPLES_OSTEST_OSTEST_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* The task_create task size can be specified in the defconfig file */ + +#ifdef CONFIG_EXAMPLES_OSTEST_STACKSIZE +# define STACKSIZE CONFIG_EXAMPLES_OSTEST_STACKSIZE +#else +# define STACKSIZE 8192 +#endif + +/* The number of times to execute the test can be specified in the defconfig + * file. + */ + +#ifndef CONFIG_EXAMPLES_OSTEST_LOOPS +# define CONFIG_EXAMPLES_OSTEST_LOOPS 1 +#endif + +/* This is the number of threads that are created in the barrier test. + * A smaller number should be selected on systems without sufficient memory + * to start so many threads. + */ + +#ifndef CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS +# define CONFIG_EXAMPLES_OSTEST_NBARRIER_THREADS 8 +#endif + +/* Priority inheritance */ + +#if defined(CONFIG_DEBUG) && defined(CONFIG_PRIORITY_INHERITANCE) && defined(CONFIG_SEM_PHDEBUG) +# define dump_nfreeholders(s) printf(s " nfreeholders: %d\n", sem_nfreeholders()) +#else +# define dump_nfreeholders(s) +#endif + +/* If CONFIG_STDIO_LINEBUFFER is defined, the STDIO buffer will be flushed + * on each new line. Otherwise, STDIO needs to be explicitly flushed to + * see the output in context. + */ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NFILE_STREAMS > 0 && \ + CONFIG_STDIO_BUFFER_SIZE > 0 && !defined(CONFIG_STDIO_LINEBUFFER) +# define FFLUSH() fflush(stdout) +#else +# define FFLUSH() +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/* dev_null.c ***************************************************************/ + +extern int dev_null(void); + +/* fpu.c ********************************************************************/ + +extern void fpu_test(void); + +/* mutex.c ******************************************************************/ + +extern void mutex_test(void); + +/* rmutex.c ******************************************************************/ + +extern void recursive_mutex_test(void); + +/* sem.c ********************************************************************/ + +extern void sem_test(void); + +/* cond.c *******************************************************************/ + +extern void cond_test(void); + +/* mqueue.c *****************************************************************/ + +extern void mqueue_test(void); + +/* timedmqueue.c ************************************************************/ + +extern void timedmqueue_test(void); + +/* cancel.c *****************************************************************/ + +extern void cancel_test(void); + +/* timedwait.c **************************************************************/ + +extern void timedwait_test(void); + +/* sighand.c ****************************************************************/ + +extern void sighand_test(void); + +/* posixtimers.c ************************************************************/ + +extern void timer_test(void); + +/* roundrobin.c *************************************************************/ + +extern void rr_test(void); + +/* barrier.c ****************************************************************/ + +extern void barrier_test(void); + +/* prioinherit.c ************************************************************/ + +extern void priority_inheritance(void); + +/* APIs exported (conditionally) by the OS specifically for testing of + * priority inheritance + */ + +#if defined(CONFIG_DEBUG) && defined(CONFIG_PRIORITY_INHERITANCE) && defined(CONFIG_SEM_PHDEBUG) +extern void sem_enumholders(FAR sem_t *sem); +extern int sem_nfreeholders(void); +#else +# define sem_enumholders(sem) +# define sem_nfreeholders() +#endif + +#endif /* __APPS_EXAMPLES_OSTEST_OSTEST_H */ diff --git a/apps/examples/ostest/posixtimer.c b/apps/examples/ostest/posixtimer.c new file mode 100644 index 000000000..3560c712b --- /dev/null +++ b/apps/examples/ostest/posixtimer.c @@ -0,0 +1,262 @@ +/*********************************************************************** + * examples/ostest/posixtimer.c + * + * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +/************************************************************************** + * Included Files + **************************************************************************/ + +#include <stdio.h> +#include <unistd.h> +#include <semaphore.h> +#include <signal.h> +#include <sched.h> +#include <errno.h> +#include "ostest.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +#ifndef NULL +# define NULL (void*)0 +#endif + +#define MY_TIMER_SIGNAL 17 +#define SIGVALUE_INT 42 + +/************************************************************************** + * Private Data + **************************************************************************/ + +static sem_t sem; +static int g_nsigreceived = 0; + +/************************************************************************** + * Private Functions + **************************************************************************/ + +static void timer_expiration(int signo, siginfo_t *info, void *ucontext) +{ + sigset_t oldset; + sigset_t allsigs; + int status; + + printf("timer_expiration: Received signal %d\n" , signo); + + g_nsigreceived++; + + /* Check signo */ + + if (signo != MY_TIMER_SIGNAL) + { + printf("timer_expiration: ERROR expected signo=%d\n" , MY_TIMER_SIGNAL); + } + + /* Check siginfo */ + + if (info->si_value.sival_int != SIGVALUE_INT) + { + printf("timer_expiration: ERROR sival_int=%d expected %d\n", + info->si_value.sival_int, SIGVALUE_INT); + } + else + { + printf("timer_expiration: sival_int=%d\n" , info->si_value.sival_int); + } + + if (info->si_signo != MY_TIMER_SIGNAL) + { + printf("timer_expiration: ERROR expected si_signo=%d, got=%d\n", + MY_TIMER_SIGNAL, info->si_signo); + } + + if (info->si_code == SI_TIMER) + { + printf("timer_expiration: si_code=%d (SI_TIMER)\n" , info->si_code); + } + else + { + printf("timer_expiration: ERROR si_code=%d, expected SI_TIMER=%d\n", + info->si_code, SI_TIMER); + } + + /* Check ucontext_t */ + + printf("timer_expiration: ucontext=%p\n" , ucontext); + + /* Check sigprocmask */ + + (void)sigfillset(&allsigs); + status = sigprocmask(SIG_SETMASK, NULL, &oldset); + if (status != OK) + { + printf("timer_expiration: ERROR sigprocmask failed, status=%d\n", + status); + } + + if (oldset != allsigs) + { + printf("timer_expiration: ERROR sigprocmask=%x expected=%x\n", + oldset, allsigs); + } + +} + +/************************************************************************** + * Public Functions + **************************************************************************/ + +void timer_test(void) +{ + sigset_t sigset; + struct sigaction act; + struct sigaction oact; + struct sigevent notify; + struct itimerspec timer; + timer_t timerid; + int status; + int i; + + printf("timer_test: Initializing semaphore to 0\n" ); + sem_init(&sem, 0, 0); + + /* Start waiter thread */ + + printf("timer_test: Unmasking signal %d\n" , MY_TIMER_SIGNAL); + + (void)sigemptyset(&sigset); + (void)sigaddset(&sigset, MY_TIMER_SIGNAL); + status = sigprocmask(SIG_UNBLOCK, &sigset, NULL); + if (status != OK) + { + printf("timer_test: ERROR sigprocmask failed, status=%d\n", + status); + } + + printf("timer_test: Registering signal handler\n" ); + act.sa_sigaction = timer_expiration; + act.sa_flags = SA_SIGINFO; + + (void)sigfillset(&act.sa_mask); + (void)sigdelset(&act.sa_mask, MY_TIMER_SIGNAL); + + status = sigaction(MY_TIMER_SIGNAL, &act, &oact); + if (status != OK) + { + printf("timer_test: ERROR sigaction failed, status=%d\n" , status); + } + +#ifndef SDCC + printf("timer_test: oact.sigaction=%p oact.sa_flags=%x oact.sa_mask=%x\n", + oact.sa_sigaction, oact.sa_flags, oact.sa_mask); +#endif + + /* Create the POSIX timer */ + + printf("timer_test: Creating timer\n" ); + + notify.sigev_notify = SIGEV_SIGNAL; + notify.sigev_signo = MY_TIMER_SIGNAL; + notify.sigev_value.sival_int = SIGVALUE_INT; + + status = timer_create(CLOCK_REALTIME, ¬ify, &timerid); + if (status != OK) + { + printf("timer_test: timer_create failed, errno=%d\n", errno); + goto errorout; + } + + /* Start the POSIX timer */ + + printf("timer_test: Starting timer\n" ); + + timer.it_value.tv_sec = 2; + timer.it_value.tv_nsec = 0; + timer.it_interval.tv_sec = 2; + timer.it_interval.tv_nsec = 0; + + status = timer_settime(timerid, 0, &timer, NULL); + if (status != OK) + { + printf("timer_test: timer_settime failed, errno=%d\n", errno); + goto errorout; + } + + /* Take the semaphore */ + + for (i = 0; i < 5; i++) + { + printf("timer_test: Waiting on semaphore\n" ); + FFLUSH(); + status = sem_wait(&sem); + if (status != 0) + { + int error = errno; + if (error == EINTR) + { + printf("timer_test: sem_wait() successfully interrupted by signal\n" ); + } + else + { + printf("timer_test: ERROR sem_wait failed, errno=%d\n" , error); + } + } + else + { + printf("timer_test: ERROR awakened with no error!\n" ); + } + printf("timer_test: g_nsigreceived=%d\n", g_nsigreceived); + } + +errorout: + sem_destroy(&sem); + + /* Then delete the timer */ + + printf("timer_test: Deleting timer\n" ); + status = timer_delete(timerid); + if (status != OK) + { + printf("timer_test: timer_create failed, errno=%d\n", errno); + } + + /* Detach the signal handler */ + + act.sa_sigaction = SIG_DFL; + status = sigaction(MY_TIMER_SIGNAL, &act, &oact); + + printf("timer_test: done\n" ); + FFLUSH(); +} diff --git a/apps/examples/ostest/prioinherit.c b/apps/examples/ostest/prioinherit.c new file mode 100644 index 000000000..993c9e14a --- /dev/null +++ b/apps/examples/ostest/prioinherit.c @@ -0,0 +1,541 @@ +/**************************************************************************** + * examples/ostest/prioinherit.c + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <stdio.h> +#include <unistd.h> +#include <semaphore.h> +#include <pthread.h> +#include <errno.h> + +#ifdef CONFIG_ARCH_SIM +# include <nuttx/arch.h> +#endif + +#include "ostest.h" + +#if defined(CONFIG_PRIORITY_INHERITANCE) && !defined(CONFIG_DISABLE_SIGNALS) && !defined(CONFIG_DISABLE_PTHREAD) + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#ifndef CONFIG_SEM_PREALLOCHOLDERS +# define CONFIG_SEM_PREALLOCHOLDERS 0 +#endif +#define NLOWPRI_THREADS (CONFIG_SEM_PREALLOCHOLDERS+1) + +#ifndef CONFIG_SEM_NNESTPRIO +# define CONFIG_SEM_NNESTPRIO 0 +#endif +#define NHIGHPRI_THREADS (CONFIG_SEM_NNESTPRIO+1) + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +enum thstate_e +{ + NOTSTARTED = 0, + RUNNING, + WAITING, + DONE +}; + +static sem_t g_sem; +static volatile enum thstate_e g_middlestate; +static volatile enum thstate_e g_highstate[NHIGHPRI_THREADS]; +static volatile enum thstate_e g_lowstate[NLOWPRI_THREADS]; +static int g_highpri; +static int g_medpri; +static int g_lowpri; + +/**************************************************************************** + * Name: nhighpri_waiting + ****************************************************************************/ + +static int nhighpri_waiting(void) +{ + int n = 0; + int i; + + for (i = 0; i < NHIGHPRI_THREADS; i++) + { + if (g_highstate[i] == WAITING) + { + n++; + } + } + return n; +} + +/**************************************************************************** + * Name: nhighpri_running + ****************************************************************************/ + +static int nhighpri_running(void) +{ + int n = 0; + int i; + + for (i = 0; i < NHIGHPRI_THREADS; i++) + { + if (g_highstate[i] != DONE) + { + n++; + } + } + return n; +} + +/**************************************************************************** + * Name: highpri_thread + ****************************************************************************/ + +static void *highpri_thread(void *parameter) +{ + int threadno = (int)parameter; + int ret; + + g_highstate[threadno-1] = RUNNING; + + printf("highpri_thread-%d: Started\n", threadno); + FFLUSH(); + sleep(1); + + printf("highpri_thread-%d: Calling sem_wait()\n", threadno); + g_highstate[threadno-1] = WAITING; + ret = sem_wait(&g_sem); + g_highstate[threadno-1] = DONE; + + if (ret != 0) + { + printf("highpri_thread-%d: sem_take failed: %d\n", threadno, ret); + } + else if (g_middlestate == RUNNING) + { + printf("highpri_thread-%d: SUCCESS midpri_thread is still running!\n", threadno); + } + else + { + printf("highpri_thread-%d: ERROR -- midpri_thread has already exited!\n", threadno); + } + + sem_post(&g_sem); + printf("highpri_thread-%d: Okay... I'm done!\n", threadno); + FFLUSH(); + return NULL; +} + +/**************************************************************************** + * Name: hog_cpu + ****************************************************************************/ + +static inline void hog_cpu(void) +{ +#ifdef CONFIG_ARCH_SIM + /* The simulator doesn't have any mechanism to do asynchronous pre-emption + * (basically because it doesn't have any interupts/asynchronous events). + * The simulator does "fake" a timer interrupt in up_idle() -- the idle + * thread that only executes when nothing else is running. In the simulator, + * we cannot suspend the middle priority task, or we wouldn't have the + * test that we want. So, we have no option but to pump the fake clock + * here by calling up_idle(). Sigh! + */ + + up_idle(); +#else + /* On real platforms with a real timer interrupt, we really can hog the + * CPU. When the sleep() goes off in priority_inheritance(), it will + * wake up and start the high priority thread. + */ + + volatile int i; + for (i = 0; i < INT_MAX; i++); +#endif +} + +/**************************************************************************** + * Name: medpri_thread + ****************************************************************************/ + +static void *medpri_thread(void *parameter) +{ + printf("medpri_thread: Started ... I won't let go of the CPU!\n"); + g_middlestate = RUNNING; + FFLUSH(); + + /* The following loop will completely block lowpri_thread from running. + * UNLESS priority inheritance is working. In that case, its priority + * will be boosted. + */ + + while (nhighpri_running() > 0) + { + hog_cpu(); + } + + printf("medpri_thread: Okay... I'm done!\n"); + FFLUSH(); + g_middlestate = DONE; + return NULL; +} + +/**************************************************************************** + * Name: lowpri_thread + ****************************************************************************/ + +static void *lowpri_thread(void *parameter) +{ + void *retval = (void*)-1; + struct sched_param sparam; + int threadno = (int)parameter; + int expected; + int count; + int policy; + int ret; + int nwaiting; + int i; + + g_lowstate[threadno-1] = RUNNING; + printf("lowpri_thread-%d: Started\n", threadno); + + ret = pthread_getschedparam(pthread_self(), &policy, &sparam); + if (ret != 0) + { + printf("lowpri_thread-%d: ERROR pthread_getschedparam failed: %d\n", threadno, ret); + } + else + { + printf("lowpri_thread-%d: initial priority: %d\n", threadno, sparam.sched_priority); + if (sparam.sched_priority != g_lowpri) + { + printf(" ERROR should have been %d\n", g_lowpri); + } + } + + g_lowstate[threadno-1] = WAITING; + ret = sem_wait(&g_sem); + if (ret != 0) + { + printf("lowpri_thread-%d: sem_take failed: %d\n", threadno, ret); + } + else + { + /* Hang on to the thread until the middle priority thread runs */ + + while (g_middlestate == NOTSTARTED && nhighpri_waiting() < NHIGHPRI_THREADS) + { + printf("lowpri_thread-%d: Waiting for the midle pri task to run\n", threadno); + printf(" g_middlestate: %d\n", (int)g_middlestate); + for (i = 0; i < NHIGHPRI_THREADS; i++) + { + printf(" g_highstate[%d]: %d\n", i, (int)g_highstate[i]); + } + printf(" I still have a count on the semaphore\n"); + sem_enumholders(&g_sem); + FFLUSH(); + sleep(1); + } + + /* Account for all of the semaphore counts. At any given time if there are 'n' + * running hight prioity tasks, then the semaphore count should be '-n' + */ + + sched_lock(); /* Needs to be atomic */ + ret = sem_getvalue(&g_sem, &count); + nwaiting = nhighpri_waiting(); + sched_unlock(); + + if (ret < 0) + { + printf("lowpri_thread-%d: ERROR sem_getvalue failed: %d\n", threadno, errno); + } + printf("lowpri_thread-%d: Sem count: %d, No. highpri thread: %d\n", threadno, count, nwaiting); + + /* The middle priority task is running, let go of the semaphore */ + + if (g_middlestate == RUNNING && nwaiting == -count) + { + /* Good.. the middle priority task is still running and the counts are okay. */ + + retval = NULL; + } + else + { + /* If the sem count is positive, then there all of the higher priority threads + * should have already completed. + */ + + printf("lowpri_thread-%d: %s the middle priority task has already exitted!\n", + threadno, count >= 0 ? "SUCCESS" : "ERROR" ); + printf(" g_middlestate: %d sem count=%d\n", (int)g_middlestate, count); + for (i = 0; i < NHIGHPRI_THREADS; i++) + { + printf(" g_highstate[%d]: %d\n", i, (int)g_highstate[i]); + } + } + } + + ret = pthread_getschedparam(pthread_self(), &policy, &sparam); + sem_enumholders(&g_sem); + sem_post(&g_sem); + if (ret != 0) + { + printf("lowpri_thread-%d: ERROR pthread_getschedparam failed: %d\n", threadno, ret); + } + else + { + if (nwaiting > 0) + { + expected = g_highpri; + } + else + { + expected = g_lowpri; + } + + printf("lowpri_thread-%d: %s priority before sem_post: %d\n", + threadno, + sparam.sched_priority != expected ? "ERROR" : "SUCCESS", + sparam.sched_priority); + + if (sparam.sched_priority != expected) + { + printf(" ERROR should have been %d\n", expected); + } + } + + ret = pthread_getschedparam(pthread_self(), &policy, &sparam); + if (ret != 0) + { + printf("lowpri_thread-%d: ERROR pthread_getschedparam failed: %d\n", threadno, ret); + } + else + { + printf("lowpri_thread-%d: %s final priority: %d\n", + threadno, + sparam.sched_priority != g_lowpri ? "ERROR" : "SUCCESS", + sparam.sched_priority); + + if (sparam.sched_priority != g_lowpri) + { + printf(" ERROR should have been %d\n", g_lowpri); + } + } + sem_enumholders(&g_sem); + + printf("lowpri_thread-%d: Okay... I'm done!\n", threadno); + FFLUSH(); + g_lowstate[threadno-1] = DONE; + return retval; +} +#endif /* CONFIG_PRIORITY_INHERITANCE && !CONFIG_DISABLE_SIGNALS && !CONFIG_DISABLE_PTHREAD */ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: priority_inheritance + ****************************************************************************/ + +void priority_inheritance(void) +{ +#if defined(CONFIG_PRIORITY_INHERITANCE) && !defined(CONFIG_DISABLE_SIGNALS) && !defined(CONFIG_DISABLE_PTHREAD) + pthread_t lowpri[NLOWPRI_THREADS]; + pthread_t medpri; + pthread_t highpri[NHIGHPRI_THREADS]; + pthread_addr_t result; + pthread_attr_t attr; + struct sched_param sparam; + int my_pri; + int status; + int i; + + printf("priority_inheritance: Started\n"); + + g_middlestate = NOTSTARTED; + for (i = 0; i < NHIGHPRI_THREADS; i++) g_highstate[i] = NOTSTARTED; + for (i = 0; i < NLOWPRI_THREADS; i++) g_lowstate[i] = NOTSTARTED; + + status = sched_getparam (getpid(), &sparam); + if (status != 0) + { + printf("priority_inheritance: sched_getparam failed\n"); + sparam.sched_priority = PTHREAD_DEFAULT_PRIORITY; + } + my_pri = sparam.sched_priority; + + g_highpri = sched_get_priority_max(SCHED_FIFO); + g_lowpri = sched_get_priority_min(SCHED_FIFO); + g_medpri = my_pri - 1; + + sem_init(&g_sem, 0, NLOWPRI_THREADS); + dump_nfreeholders("priority_inheritance:"); + + /* Start the low priority threads */ + + for (i = 0; i < NLOWPRI_THREADS; i++) + { + int threadno = i+1; + printf("priority_inheritance: Starting lowpri_thread-%d (of %d) at %d\n", + threadno, NLOWPRI_THREADS, g_lowpri); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("priority_inheritance: pthread_attr_init failed, status=%d\n", status); + } + sparam.sched_priority = g_lowpri; + status = pthread_attr_setschedparam(&attr,& sparam); + if (status != OK) + { + printf("priority_inheritance: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("priority_inheritance: Set lowpri_thread-%d priority to %d\n", + threadno, sparam.sched_priority); + } + + status = pthread_create(&lowpri[i], &attr, lowpri_thread, (void*)threadno); + if (status != 0) + { + printf("priority_inheritance: pthread_create failed, status=%d\n", status); + } + } + printf("priority_inheritance: Waiting...\n"); + sleep(2); + dump_nfreeholders("priority_inheritance:"); + + /* Start the medium priority thread */ + + printf("priority_inheritance: Starting medpri_thread at %d\n", g_medpri); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("priority_inheritance: pthread_attr_init failed, status=%d\n", status); + } + + sparam.sched_priority = g_medpri; + status = pthread_attr_setschedparam(&attr,& sparam); + if (status != OK) + { + printf("priority_inheritance: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("priority_inheritance: Set medpri_thread priority to %d\n", sparam.sched_priority); + } + FFLUSH(); + + status = pthread_create(&medpri, &attr, medpri_thread, NULL); + if (status != 0) + { + printf("priority_inheritance: pthread_create failed, status=%d\n", status); + } + printf("priority_inheritance: Waiting...\n"); + sleep(1); + dump_nfreeholders("priority_inheritance:"); + + /* Start the high priority threads */ + + for (i = 0; i < NHIGHPRI_THREADS; i++) + { + int threadno = i+1; + printf("priority_inheritance: Starting highpri_thread-%d (of %d) at %d\n", + threadno, NHIGHPRI_THREADS, g_highpri); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("priority_inheritance: pthread_attr_init failed, status=%d\n", status); + } + + sparam.sched_priority = g_highpri - i; + status = pthread_attr_setschedparam(&attr,& sparam); + if (status != OK) + { + printf("priority_inheritance: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("priority_inheritance: Set highpri_thread-%d priority to %d\n", + threadno, sparam.sched_priority); + } + FFLUSH(); + + status = pthread_create(&highpri[i], &attr, highpri_thread, (void*)threadno); + if (status != 0) + { + printf("priority_inheritance: pthread_create failed, status=%d\n", status); + } + } + dump_nfreeholders("priority_inheritance:"); + FFLUSH(); + + /* Wait for all thread instances to complete */ + + for (i = 0; i < NHIGHPRI_THREADS; i++) + { + printf("priority_inheritance: Waiting for highpri_thread-%d to complete\n", i+1); + FFLUSH(); + (void)pthread_join(highpri[i], &result); + dump_nfreeholders("priority_inheritance:"); + } + printf("priority_inheritance: Waiting for medpri_thread to complete\n"); + FFLUSH(); + (void)pthread_join(medpri, &result); + dump_nfreeholders("priority_inheritance:"); + for (i = 0; i < NLOWPRI_THREADS; i++) + { + printf("priority_inheritance: Waiting for lowpri_thread-%d to complete\n", i+1); + FFLUSH(); + (void)pthread_join(lowpri[i], &result); + dump_nfreeholders("priority_inheritance:"); + } + + printf("priority_inheritance: Finished\n"); + sem_destroy(&g_sem); + dump_nfreeholders("priority_inheritance:"); + FFLUSH(); +#endif /* CONFIG_PRIORITY_INHERITANCE && !CONFIG_DISABLE_SIGNALS && !CONFIG_DISABLE_PTHREAD */ +} diff --git a/apps/examples/ostest/rmutex.c b/apps/examples/ostest/rmutex.c new file mode 100644 index 000000000..44eb4bb3b --- /dev/null +++ b/apps/examples/ostest/rmutex.c @@ -0,0 +1,166 @@ +/*********************************************************************** + * rmutex.c + * + * Copyright (C) 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +#include <stdio.h> +#include <pthread.h> +#include "ostest.h" + +#ifndef NULL +# define NULL (void*)0 +#endif + +#define NTHREADS 3 +#define NLOOPS 3 +#define NRECURSIONS 3 + +static pthread_mutex_t mut; + +static void thread_inner(int id, int level) +{ + int status; + if (level < NRECURSIONS) + { + /* Take the mutex */ + + printf("thread_inner[%d, %d]: Locking\n", id, level); + status = pthread_mutex_lock(&mut); + if (status != 0) + { + printf("thread_inner[%d, %d]: ERROR pthread_mutex_lock failed: %d\n", + id, level, status); + } + printf("thread_inner[%d, %d]: Locked\n", id, level); + + /* Give the other threads a chance */ + + pthread_yield(); + thread_inner(id, level+1); + pthread_yield(); + + /* Unlock the mutex */ + + printf("thread_inner[%d, %d]: Unlocking\n", id, level); + status = pthread_mutex_unlock(&mut); + if (status != 0) + { + printf("thread_inner[%d, %d]: ERROR pthread_mutex_unlock failed: %d\n", + id, level, status); + } + printf("thread_inner[%d, %d]: Unlocked\n", id, level); + pthread_yield(); + } +} + +static void *thread_outer(void *parameter) +{ + int i; + printf("thread_outer[%d]: Started\n", (int)parameter); + for (i = 0; i < NLOOPS; i++) + { + printf("thread_outer[%d]: Loop %d\n", (int)parameter, i); + thread_inner((int)parameter, 0); + } + printf("thread_outer[%d]: Exitting\n", (int)parameter); + pthread_exit(NULL); + return NULL; /* Non-reachable -- needed for some compilers */ +} + +void recursive_mutex_test(void) +{ + pthread_t thread[NTHREADS]; +#ifdef SDCC + pthread_addr_t result[NTHREADS]; + pthread_attr_t attr; +#endif + pthread_mutexattr_t mattr; + int type; + int status; + int i; + + /* Initialize the mutex attributes */ + + pthread_mutexattr_init(&mattr); + status = pthread_mutexattr_settype(&mattr, PTHREAD_MUTEX_RECURSIVE); + if (status != 0) + { + printf("recursive_mutex_test: ERROR pthread_mutexattr_settype failed, status=%d\n", status); + } + + status = pthread_mutexattr_gettype(&mattr, &type); + if (status != 0) + { + printf("recursive_mutex_test: ERROR pthread_mutexattr_gettype failed, status=%d\n", status); + } + if (type != PTHREAD_MUTEX_RECURSIVE) + { + printf("recursive_mutex_test: ERROR pthread_mutexattr_gettype return type=%d\n", type); + } + + /* Initialize the mutex */ + + printf("recursive_mutex_test: Initializing mutex\n"); + pthread_mutex_init(&mut, &mattr); + + /* Start the threads -- all at the same, default priority */ + + for (i = 0; i < NTHREADS; i++) + { + printf("recursive_mutex_test: Starting thread %d\n", i+1); +#ifdef SDCC + (void)pthread_attr_init(&attr); + status = pthread_create(&thread[i], &attr, thread_outer, (pthread_addr_t)i+1); +#else + status = pthread_create(&thread[i], NULL, thread_outer, (pthread_addr_t)i+1); +#endif + if (status != 0) + { + printf("recursive_mutex_test: ERRROR thread#%d creation: %d\n", i+1, status); + } + } + + /* Wait for all; of the threads to complete */ + + for (i = 0; i < NTHREADS; i++) + { + printf("recursive_mutex_test: Waiting for thread %d\n", i+1); +#ifdef SDCC + pthread_join(thread[i], &result1); +#else + pthread_join(thread[i], NULL); +#endif + } + + printf("recursive_mutex_test: Complete\n"); +} diff --git a/apps/examples/ostest/roundrobin.c b/apps/examples/ostest/roundrobin.c new file mode 100644 index 000000000..061d51f3d --- /dev/null +++ b/apps/examples/ostest/roundrobin.c @@ -0,0 +1,232 @@ +/******************************************************************************** + * examples/ostest/roundrobin.c + * + * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ********************************************************************************/ + +/******************************************************************************** + * Included Files + ********************************************************************************/ + +#include <nuttx/config.h> +#include <stdio.h> +#include "ostest.h" + +#if CONFIG_RR_INTERVAL > 0 + +/******************************************************************************** + * 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. + */ + +/* #define CONFIG_NINTEGERS 32768 Takes forever on 60Mhz ARM7 */ + +#define CONFIG_NINTEGERS 2048 + +/******************************************************************************** + * Private Data + ********************************************************************************/ + +static int prime1[CONFIG_NINTEGERS]; +static int prime2[CONFIG_NINTEGERS]; + +/******************************************************************************** + * Private Functions + ********************************************************************************/ + +/******************************************************************************** + * Name: dosieve + * + * 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. + * + ********************************************************************************/ + +static void dosieve(int *prime) +{ + int a,d; + int i; + int j; + + a = 2; + d = a; + + for (i = 0; i < CONFIG_NINTEGERS; i++) + { + 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++; + } + +#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++; + } + } +#endif +} + +/******************************************************************************** + * Name: sieve1 + ********************************************************************************/ + +static void *sieve1(void *parameter) +{ + int i; + + printf("sieve1 started\n"); + + for (i = 0; i < 1000; i++) + { + dosieve(prime1); + } + + 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"); + + pthread_exit(NULL); + return NULL; /* To keep some compilers happy */ +} + +/******************************************************************************** + * Public Functions + ********************************************************************************/ + +/******************************************************************************** + * Name: rr_test + ********************************************************************************/ + +void rr_test(void) +{ + pthread_t sieve1_thread; + pthread_t sieve2_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) + { + printf("rr_test: pthread_attr_init failed, status=%d\n", status); + } + + sparam.sched_priority = sched_get_priority_min(SCHED_FIFO); + status = pthread_attr_setschedparam(&attr, &sparam); + if (status != OK) + { + printf("rr_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("rr_test: Set thread priority to %d\n", sparam.sched_priority); + } + + status = pthread_attr_setschedpolicy(&attr, SCHED_RR); + if (status != OK) + { + printf("rr_test: pthread_attr_setschedpolicy failed, status=%d\n", status); + } + else + { + printf("rr_test: Set thread policty to SCHED_RR\n"); + } + + status = pthread_create(&sieve1_thread, &attr, sieve1, NULL); + if (status != 0) + { + printf("rr_test: Error in thread 1 creation, status=%d\n", status); + } + + printf("rr_test: Starting sieve1 thread \n"); + + status = pthread_create(&sieve2_thread, &attr, sieve2, NULL); + 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: 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); + printf("rr_test: Done\n"); +} + +#endif /* CONFIG_RR_INTERVAL */ diff --git a/apps/examples/ostest/sem.c b/apps/examples/ostest/sem.c new file mode 100644 index 000000000..850cf8040 --- /dev/null +++ b/apps/examples/ostest/sem.c @@ -0,0 +1,246 @@ +/*********************************************************************** + * sem.c + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +#include <stdio.h> +#include <pthread.h> +#include <semaphore.h> +#include <sched.h> +#include "ostest.h" + +#ifndef NULL +# define NULL (void*)0 +#endif + +static sem_t sem; + +static void *waiter_func(void *parameter) +{ + int id = (int)parameter; + int status; + int value; + + printf("waiter_func: Thread %d Started\n", id); + + /* Take the semaphore */ + + status = sem_getvalue(&sem, &value); + if (status < 0) + { + printf("waiter_func: ERROR thread %d could not get semaphore value\n", id); + } + else + { + printf("waiter_func: Thread %d initial semaphore value = %d\n", id, value); + } + + printf("waiter_func: Thread %d waiting on semaphore\n", id); + status = sem_wait(&sem); + if (status != 0) + { + printf("waiter_func: ERROR thread %d sem_wait failed\n", id); + } + printf("waiter_func: Thread %d awakened\n", id); + + status = sem_getvalue(&sem, &value); + if (status < 0) + { + printf("waiter_func: ERROR thread %d could not get semaphore value\n", id); + } + else + { + printf("waiter_func: Thread %d new semaphore value = %d\n", id, value); + } + + printf("waiter_func: Thread %d done\n", id); + return NULL; +} + +static void *poster_func(void *parameter) +{ + int id = (int)parameter; + int status; + int value; + + printf("poster_func: Thread %d started\n", id); + + /* Take the semaphore */ + + do + { + status = sem_getvalue(&sem, &value); + if (status < 0) + { + printf("poster_func: ERROR thread %d could not get semaphore value\n", id); + } + else + { + printf("poster_func: Thread %d semaphore value = %d\n", id, value); + } + + if (value < 0) + { + printf("poster_func: Thread %d posting semaphore\n", id); + status = sem_post(&sem); + if (status != 0) + { + printf("poster_func: ERROR thread %d sem_wait failed\n", id); + } + + pthread_yield(); + + status = sem_getvalue(&sem, &value); + if (status < 0) + { + printf("poster_func: ERROR thread %d could not get semaphore value\n", id); + } + else + { + printf("poster_func: Thread %d new semaphore value = %d\n", id, value); + } + } + } + while (value < 0); + + printf("poster_func: Thread %d done\n", id); + return NULL; + +} + +void sem_test(void) +{ + pthread_t waiter_thread1; + pthread_t waiter_thread2; + pthread_t poster_thread; +#ifdef SDCC + pthread_addr_t result; +#endif + struct sched_param sparam; + int prio_min; + int prio_max; + int prio_mid; + pthread_attr_t attr; + int status; + + printf("sem_test: Initializing semaphore to 0\n"); + sem_init(&sem, 0, 0); + + /* Start two waiter thread instances */ + + printf("sem_test: Starting waiter thread 1\n"); + status = pthread_attr_init(&attr); + if (status != OK) + { + printf("sem_test: pthread_attr_init failed, status=%d\n", status); + } + + prio_min = sched_get_priority_min(SCHED_FIFO); + prio_max = sched_get_priority_max(SCHED_FIFO); + prio_mid = (prio_min + prio_max) / 2; + + sparam.sched_priority = (prio_mid + prio_max) / 2; + status = pthread_attr_setschedparam(&attr,&sparam); + if (status != OK) + { + printf("sem_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("sem_test: Set thread 1 priority to %d\n", sparam.sched_priority); + } + + status = pthread_create(&waiter_thread1, &attr, waiter_func, (pthread_addr_t)1); + if (status != 0) + { + printf("sem_test: Error in thread 1 creation, status=%d\n", status); + } + + printf("sem_test: Starting waiter thread 2\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("sem_test: pthread_attr_init failed, status=%d\n", status); + } + + sparam.sched_priority = prio_mid; + status = pthread_attr_setschedparam(&attr,&sparam); + if (status != OK) + { + printf("sem_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("sem_test: Set thread 2 priority to %d\n", sparam.sched_priority); + } + + status = pthread_create(&waiter_thread2, &attr, waiter_func, (pthread_addr_t)2); + if (status != 0) + { + printf("sem_test: Error in thread 2 creation, status=%d\n", status); + } + + printf("sem_test: Starting poster thread 3\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("sem_test: pthread_attr_init failed, status=%d\n", status); + } + + sparam.sched_priority = (prio_min + prio_mid) / 2; + status = pthread_attr_setschedparam(&attr,&sparam); + if (status != OK) + { + printf("sem_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("sem_test: Set thread 3 priority to %d\n", sparam.sched_priority); + } + + status = pthread_create(&poster_thread, &attr, poster_func, (pthread_addr_t)3); + if (status != 0) + { + printf("sem_test: Error in thread 3 creation, status=%d\n", status); + } + +#ifdef SDCC + pthread_join(waiter_thread1, &result); + pthread_join(waiter_thread2, &result); + pthread_join(poster_thread, &result); +#else + pthread_join(waiter_thread1, NULL); + pthread_join(waiter_thread2, NULL); + pthread_join(poster_thread, NULL); +#endif +} diff --git a/apps/examples/ostest/sighand.c b/apps/examples/ostest/sighand.c new file mode 100644 index 000000000..681531639 --- /dev/null +++ b/apps/examples/ostest/sighand.c @@ -0,0 +1,267 @@ +/*********************************************************************** + * apps/examples/ostest/sighand.c + * + * Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +#include <sys/types.h> +#include <stdbool.h> +#include <stdio.h> +#include <unistd.h> +#include <semaphore.h> +#include <signal.h> +#include <sched.h> +#include <errno.h> +#include "ostest.h" + +#ifndef NULL +# define NULL (void*)0 +#endif + +#define WAKEUP_SIGNAL 17 +#define SIGVALUE_INT 42 + +static sem_t sem; +static bool sigreceived = false; +static bool threadexited = false; + +static void wakeup_action(int signo, siginfo_t *info, void *ucontext) +{ + sigset_t oldset; + sigset_t allsigs; + int status; + + printf("wakeup_action: Received signal %d\n" , signo); + + sigreceived = true; + + /* Check signo */ + + if (signo != WAKEUP_SIGNAL) + { + printf("wakeup_action: ERROR expected signo=%d\n" , WAKEUP_SIGNAL); + } + + /* Check siginfo */ + + if (info->si_value.sival_int != SIGVALUE_INT) + { + printf("wakeup_action: ERROR sival_int=%d expected %d\n", + info->si_value.sival_int, SIGVALUE_INT); + } + else + { + printf("wakeup_action: sival_int=%d\n" , info->si_value.sival_int); + } + + if (info->si_signo != WAKEUP_SIGNAL) + { + printf("wakeup_action: ERROR expected si_signo=%d, got=%d\n", + WAKEUP_SIGNAL, info->si_signo); + } + + printf("wakeup_action: si_code=%d\n" , info->si_code); + + /* Check ucontext_t */ + + printf("wakeup_action: ucontext=%p\n" , ucontext); + + /* Check sigprocmask */ + + (void)sigfillset(&allsigs); + status = sigprocmask(SIG_SETMASK, NULL, &oldset); + if (status != OK) + { + printf("wakeup_action: ERROR sigprocmask failed, status=%d\n", + status); + } + + if (oldset != allsigs) + { + printf("wakeup_action: ERROR sigprocmask=%x expected=%x\n", + oldset, allsigs); + } +} + +static int waiter_main(int argc, char *argv[]) +{ + sigset_t sigset; + struct sigaction act; + struct sigaction oact; + int status; + + printf("waiter_main: Waiter started\n" ); + + printf("waiter_main: Unmasking signal %d\n" , WAKEUP_SIGNAL); + (void)sigemptyset(&sigset); + (void)sigaddset(&sigset, WAKEUP_SIGNAL); + status = sigprocmask(SIG_UNBLOCK, &sigset, NULL); + if (status != OK) + { + printf("waiter_main: ERROR sigprocmask failed, status=%d\n", + status); + } + + printf("waiter_main: Registering signal handler\n" ); + act.sa_sigaction = wakeup_action; + act.sa_flags = SA_SIGINFO; + + (void)sigfillset(&act.sa_mask); + (void)sigdelset(&act.sa_mask, WAKEUP_SIGNAL); + + status = sigaction(WAKEUP_SIGNAL, &act, &oact); + if (status != OK) + { + printf("waiter_main: ERROR sigaction failed, status=%d\n" , status); + } + +#ifndef SDCC + printf("waiter_main: oact.sigaction=%p oact.sa_flags=%x oact.sa_mask=%x\n", + oact.sa_sigaction, oact.sa_flags, oact.sa_mask); +#endif + + /* Take the semaphore */ + + printf("waiter_main: Waiting on semaphore\n" ); + FFLUSH(); + + status = sem_wait(&sem); + if (status != 0) + { + int error = errno; + if (error == EINTR) + { + printf("waiter_main: sem_wait() successfully interrupted by signal\n" ); + } + else + { + printf("waiter_main: ERROR sem_wait failed, errno=%d\n" , error); + } + } + else + { + printf("waiter_main: ERROR awakened with no error!\n" ); + } + + /* Detach the signal handler */ + + act.sa_sigaction = SIG_DFL; + status = sigaction(WAKEUP_SIGNAL, &act, &oact); + + printf("waiter_main: done\n" ); + FFLUSH(); + + threadexited = true; + return 0; +} + +void sighand_test(void) +{ + struct sched_param param; + union sigval sigvalue; + pid_t waiterpid; + int policy; + int status; + + printf("sighand_test: Initializing semaphore to 0\n" ); + sem_init(&sem, 0, 0); + + /* Start waiter thread */ + + printf("sighand_test: Starting waiter task\n" ); + status = sched_getparam (0, ¶m); + if (status != OK) + { + printf("sighand_test: ERROR sched_getparam() failed\n" ); + param.sched_priority = PTHREAD_DEFAULT_PRIORITY; + } + + policy = sched_getscheduler(0); + if (policy == ERROR) + { + printf("sighand_test: ERROR sched_getscheduler() failed\n" ); + policy = SCHED_FIFO; + } + + waiterpid = task_create("waiter", param.sched_priority, + PTHREAD_STACK_DEFAULT, waiter_main, NULL); + if (waiterpid == ERROR) + { + printf("sighand_test: ERROR failed to start waiter_main\n" ); + } + else + { + printf("sighand_test: Started waiter_main pid=%d\n", waiterpid); + } + + /* Wait a bit */ + + FFLUSH(); + sleep(2); + + /* Then signal the waiter thread. */ + + printf("sighand_test: Signaling pid=%d with signo=%d sigvalue=%d\n", + waiterpid, WAKEUP_SIGNAL, SIGVALUE_INT); + + sigvalue.sival_int = SIGVALUE_INT; +#ifdef CONFIG_CAN_PASS_STRUCTS + status = sigqueue(waiterpid, WAKEUP_SIGNAL, sigvalue); +#else + status = sigqueue(waiterpid, WAKEUP_SIGNAL, sigvalue.sival_ptr); +#endif + if (status != OK) + { + printf("sighand_test: ERROR sigqueue failed\n" ); + task_delete(waiterpid); + } + + /* Wait a bit */ + + FFLUSH(); + sleep(2); + + /* Then check the result */ + + if (!threadexited) + { + printf("sighand_test: ERROR waiter task did not exit\n" ); + } + + if (!sigreceived) + { + printf("sighand_test: ERROR signal handler did not run\n" ); + } + + printf("sighand_test: done\n" ); + FFLUSH(); +} diff --git a/apps/examples/ostest/timedmqueue.c b/apps/examples/ostest/timedmqueue.c new file mode 100644 index 000000000..807d8537b --- /dev/null +++ b/apps/examples/ostest/timedmqueue.c @@ -0,0 +1,387 @@ +/************************************************************************** + * apps/examples/ostest/mqueue.c + * + * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + **************************************************************************/ + +/************************************************************************** + * Included Files + **************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <ctype.h> +#include <fcntl.h> +#include <pthread.h> +#include <mqueue.h> +#include <sched.h> +#include <errno.h> + +#include "ostest.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +#define TEST_MESSAGE "This is a test and only a test" +#if defined(SDCC) || defined(__ZILOG__) + /* Cannot use strlen in array size */ + +# define TEST_MSGLEN (31) +#else + /* Message lenght is the size of the message plus the null terminator */ + +# define TEST_MSGLEN (strlen(TEST_MESSAGE)+1) +#endif + +#define TEST_SEND_NMSGS (10) +#define TEST_RECEIVE_NMSGS (10) + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/************************************************************************** + * Global Variables + **************************************************************************/ + +/************************************************************************** + * Private Variables + **************************************************************************/ + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Public Functions + **************************************************************************/ + +static void *sender_thread(void *arg) +{ + mqd_t mqfd; + char msg_buffer[TEST_MSGLEN]; + struct mq_attr attr; + int status = 0; + int nerrors = 0; + int i; + + printf("sender_thread: Starting\n"); + + /* Fill in attributes for message queue */ + + attr.mq_maxmsg = TEST_SEND_NMSGS-1; + attr.mq_msgsize = TEST_MSGLEN; + attr.mq_flags = 0; + + /* Set the flags for the open of the queue. + * Make it a blocking open on the queue, meaning it will block if + * this process tries to send to the queue and the queue is full. + * + * O_CREAT - the queue will get created if it does not already exist. + * O_WRONLY - we are only planning to write to the queue. + * + * Open the queue, and create it if the receiving process hasn't + * already created it. + */ + + mqfd = mq_open("testmq", O_WRONLY|O_CREAT, 0666, &attr); + if (mqfd < 0) + { + printf("sender_thread: ERROR mq_open failed\n"); + pthread_exit((pthread_addr_t)1); + } + + /* Fill in a test message buffer to send */ + + memcpy(msg_buffer, TEST_MESSAGE, TEST_MSGLEN); + + /* Perform the send TEST_SEND_NMSGS times */ + + for (i = 0; i < TEST_SEND_NMSGS; i++) + { + struct timespec ts; + status = clock_gettime(CLOCK_REALTIME, &ts); + if (status != 0) + { + printf("sender_thread: ERROR clock_gettime failed\n"); + } + ts.tv_sec += 5; + + /* The first TEST_SEND_NMSGS-1 send should succeed. The last + * one should fail with errno == ETIMEDOUT + */ + + status = mq_timedsend(mqfd, msg_buffer, TEST_MSGLEN, 42, &ts); + if (status < 0) + { + if (i == TEST_SEND_NMSGS-1 && errno == ETIMEDOUT) + { + printf("sender_thread: mq_timedsend %d timed out as expected\n", i); + } + else + { + printf("sender_thread: ERROR mq_timedsend failure=%d on msg %d\n", errno, i); + nerrors++; + } + } + else + { + if (i == TEST_SEND_NMSGS-1) + { + printf("sender_thread: ERROR mq_timedsend of msg %d succeeded\n", i); + nerrors++; + } + else + { + printf("sender_thread: mq_timedsend succeeded on msg %d\n", i); + } + } + } + + /* Close the queue and return success */ + + if (mq_close(mqfd) < 0) + { + printf("sender_thread: ERROR mq_close failed\n"); + } + + printf("sender_thread: returning nerrors=%d\n", nerrors); + FFLUSH(); + return (pthread_addr_t)nerrors; +} + +static void *receiver_thread(void *arg) +{ + mqd_t mqfd; + char msg_buffer[TEST_MSGLEN]; + struct mq_attr attr; + int nbytes; + int nerrors = 0; + int i; + + printf("receiver_thread: Starting\n"); + + /* Fill in attributes for message queue */ + + attr.mq_maxmsg = TEST_SEND_NMSGS-1; + attr.mq_msgsize = TEST_MSGLEN; + attr.mq_flags = 0; + + /* Set the flags for the open of the queue. + * Make it a blocking open on the queue, meaning it will block if + * this process tries to* send to the queue and the queue is full. + * + * O_CREAT - the queue will get created if it does not already exist. + * O_RDONLY - we are only planning to write to the queue. + * + * Open the queue, and create it if the sending process hasn't + * already created it. + */ + + mqfd = mq_open("testmq", O_RDONLY|O_CREAT, 0666, &attr); + if (mqfd < 0) + { + printf("receiver_thread: ERROR mq_open failed\n"); + pthread_exit((pthread_addr_t)1); + } + + /* Perform the receive TEST_RECEIVE_NMSGS times */ + + for (i = 0; i < TEST_RECEIVE_NMSGS; i++) + { + struct timespec ts; + int status = clock_gettime(CLOCK_REALTIME, &ts); + if (status != 0) + { + printf("sender_thread: ERROR clock_gettime failed\n"); + } + ts.tv_sec += 5; + + /* The first TEST_SEND_NMSGS-1 send should succeed. The last + * one should fail with errno == ETIMEDOUT + */ + + memset(msg_buffer, 0xaa, TEST_MSGLEN); + nbytes = mq_timedreceive(mqfd, msg_buffer, TEST_MSGLEN, 0, &ts); + if (nbytes < 0) + { + if (i == TEST_SEND_NMSGS-1 && errno == ETIMEDOUT) + { + printf("receiver_thread: Receive %d timed out as expected\n", i); + } + else + { + printf("receiver_thread: ERROR mq_timedreceive failure=%d on msg %d\n", errno, i); + nerrors++; + } + } + else if (nbytes != TEST_MSGLEN) + { + printf("receiver_thread: mq_timedreceive return bad size %d on msg %d\n", nbytes, i); + nerrors++; + } + else if (memcmp(TEST_MESSAGE, msg_buffer, nbytes) != 0) + { + int j; + + printf("receiver_thread: mq_timedreceive returned corrupt message on msg %d\n", i); + printf("receiver_thread: i Expected Received\n"); + + for (j = 0; j < TEST_MSGLEN-1; j++) + { + if (isprint(msg_buffer[j])) + { + printf("receiver_thread: %2d %02x (%c) %02x (%c)\n", + j, TEST_MESSAGE[j], TEST_MESSAGE[j], msg_buffer[j], msg_buffer[j]); + } + else + { + printf("receiver_thread: %2d %02x (%c) %02x\n", + j, TEST_MESSAGE[j], TEST_MESSAGE[j], msg_buffer[j]); + } + } + printf("receiver_thread: %2d 00 %02x\n", + j, msg_buffer[j]); + } + else if (i == TEST_SEND_NMSGS-1) + { + printf("receiver_thread: ERROR mq_timedreceive of msg %d succeeded\n", i); + nerrors++; + } + else + { + printf("receiver_thread: mq_timedreceive succeeded on msg %d\n", i); + } + } + + /* Close the queue and return success */ + + if (mq_close(mqfd) < 0) + { + printf("receiver_thread: ERROR mq_close failed\n"); + nerrors++; + } + + /* Destroy the queue */ + + if (mq_unlink("testmq") < 0) + { + printf("receiver_thread: ERROR mq_close failed\n"); + nerrors++; + } + + printf("receiver_thread: returning nerrors=%d\n", nerrors); + FFLUSH(); + pthread_exit((pthread_addr_t)nerrors); + return (pthread_addr_t)nerrors; +} + +void timedmqueue_test(void) +{ + pthread_t sender; + pthread_t receiver; + void *result; + pthread_attr_t attr; + int status; + + /* Start the sending thread at the default priority */ + + printf("timedmqueue_test: Starting sender\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("timedmqueue_test: pthread_attr_init failed, status=%d\n", status); + } + + status = pthread_attr_setstacksize(&attr, STACKSIZE); + if (status != 0) + { + printf("timedmqueue_test: pthread_attr_setstacksize failed, status=%d\n", status); + } + + status = pthread_create(&sender, &attr, sender_thread, NULL); + if (status != 0) + { + printf("timedmqueue_test: pthread_create failed, status=%d\n", status); + } + + /* Wait for the sending thread to complete */ + + printf("timedmqueue_test: Waiting for sender to complete\n"); + pthread_join(sender, &result); + if (result != (void*)0) + { + printf("timedmqueue_test: ERROR sender thread exited with %d errors\n", (int)result); + } + + /* Start the receiving thread at the default priority */ + + printf("timedmqueue_test: Starting receiver\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("timedmqueue_test: pthread_attr_init failed, status=%d\n", status); + } + + status = pthread_attr_setstacksize(&attr, STACKSIZE); + if (status != 0) + { + printf("timedmqueue_test: pthread_attr_setstacksize failed, status=%d\n", status); + } + + status = pthread_create(&receiver, &attr, receiver_thread, NULL); + if (status != 0) + { + printf("timedmqueue_test: pthread_create failed, status=%d\n", status); + } + + /* Wait for the receiving thread to complete */ + + printf("timedmqueue_test: Waiting for receiver to complete\n"); + pthread_join(receiver, &result); + if (result != (void*)0) + { + printf("timedmqueue_test: ERROR receiver thread exited with %d errors\n", (int)result); + } + + printf("timedmqueue_test: Test complete\n"); +} + + diff --git a/apps/examples/ostest/timedwait.c b/apps/examples/ostest/timedwait.c new file mode 100644 index 000000000..fc381ddda --- /dev/null +++ b/apps/examples/ostest/timedwait.c @@ -0,0 +1,195 @@ +/*********************************************************************** + * examples/ostest/timedwait.c + * + * Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ***********************************************************************/ + +/************************************************************************** + * Included Files + **************************************************************************/ + +#include <stdio.h> +#include <time.h> +#include <pthread.h> +#include <unistd.h> +#include <errno.h> + +#include "ostest.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +/************************************************************************** + * Private Data + **************************************************************************/ + +static pthread_mutex_t mutex; +static pthread_cond_t cond; + +/************************************************************************** + * Private Functions + **************************************************************************/ + +static void *thread_waiter(void *parameter) +{ + struct timespec ts; + int status; + + /* Take the mutex */ + + printf("thread_waiter: Taking mutex\n"); + status = pthread_mutex_lock(&mutex); + if (status != 0) + { + printf("thread_waiter: ERROR pthread_mutex_lock failed, status=%d\n", status); + } + + printf("thread_waiter: Starting 5 second wait for condition\n"); + + status = clock_gettime(CLOCK_REALTIME, &ts); + if (status != 0) + { + printf("thread_waiter: ERROR clock_gettime failed\n"); + } + ts.tv_sec += 5; + + /* The wait -- no-one is ever going to awaken us */ + + status = pthread_cond_timedwait(&cond, &mutex, &ts); + if (status != 0) + { + if (status == ETIMEDOUT) + { + printf("thread_waiter: pthread_cond_timedwait timed out\n"); + } + else + { + printf("thread_waiter: ERROR pthread_cond_timedwait failed, status=%d\n", status); + } + } + else + { + printf("thread_waiter: ERROR pthread_cond_timedwait returned without timeout, status=%d\n", status); + } + + /* Release the mutex */ + + printf("thread_waiter: Releasing mutex\n"); + status = pthread_mutex_unlock(&mutex); + if (status != 0) + { + printf("thread_waiter: ERROR pthread_mutex_unlock failed, status=%d\n", status); + } + + printf("thread_waiter: Exit with status 0x12345678\n"); + pthread_exit((pthread_addr_t)0x12345678); + return NULL; +} + +/************************************************************************** + * Public Definitions + **************************************************************************/ + +void timedwait_test(void) +{ + pthread_t waiter; + pthread_attr_t attr; + struct sched_param sparam; + void *result; + int prio_max; + int status; + + /* Initialize the mutex */ + + printf("thread_waiter: Initializing mutex\n"); + status = pthread_mutex_init(&mutex, NULL); + if (status != 0) + { + printf("timedwait_test: ERROR pthread_mutex_init failed, status=%d\n", status); + } + + /* Initialize the condition variable */ + + printf("timedwait_test: Initializing cond\n"); + status = pthread_cond_init(&cond, NULL); + if (status != 0) + { + printf("timedwait_test: ERROR pthread_condinit failed, status=%d\n", status); + } + + /* Start the waiter thread at higher priority */ + + printf("timedwait_test: Starting waiter\n"); + status = pthread_attr_init(&attr); + if (status != 0) + { + printf("timedwait_test: pthread_attr_init failed, status=%d\n", status); + } + + prio_max = sched_get_priority_max(SCHED_FIFO); + status = sched_getparam (getpid(), &sparam); + if (status != 0) + { + printf("timedwait_test: sched_getparam failed\n"); + sparam.sched_priority = PTHREAD_DEFAULT_PRIORITY; + } + + sparam.sched_priority = (prio_max + sparam.sched_priority) / 2; + status = pthread_attr_setschedparam(&attr,&sparam); + if (status != OK) + { + printf("timedwait_test: pthread_attr_setschedparam failed, status=%d\n", status); + } + else + { + printf("timedwait_test: Set thread 2 priority to %d\n", sparam.sched_priority); + } + + status = pthread_create(&waiter, &attr, thread_waiter, NULL); + if (status != 0) + { + printf("timedwait_test: pthread_create failed, status=%d\n", status); + } + + printf("timedwait_test: Joining\n"); + FFLUSH(); + status = pthread_join(waiter, &result); + if (status != 0) + { + printf("timedwait_test: ERROR pthread_join failed, status=%d\n", status); + } + else + { + printf("timedwait_test: waiter exited with result=%p\n", result); + } +} diff --git a/apps/examples/pipe/Kconfig b/apps/examples/pipe/Kconfig new file mode 100644 index 000000000..26bc92fcc --- /dev/null +++ b/apps/examples/pipe/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_PIPE + bool "Pipe example" + default n + ---help--- + Enable the pipe example + +if EXAMPLES_PIPE +endif diff --git a/apps/examples/pipe/Makefile b/apps/examples/pipe/Makefile new file mode 100644 index 000000000..3bcc9b5f7 --- /dev/null +++ b/apps/examples/pipe/Makefile @@ -0,0 +1,93 @@ +############################################################################ +# apps/examples/pipe/Makefile +# +# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# Pipe Example + +ASRCS = +CSRCS = pipe_main.c transfer_test.c interlock_test.c redirect_test.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/pipe/interlock_test.c b/apps/examples/pipe/interlock_test.c new file mode 100644 index 000000000..e049a65f6 --- /dev/null +++ b/apps/examples/pipe/interlock_test.c @@ -0,0 +1,224 @@ +/**************************************************************************** + * examples/pipe/interlock_test.c + * + * Copyright (C) 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <sys/stat.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <errno.h> + +#include "pipe.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: null_writer + ****************************************************************************/ + +static void *null_writer(pthread_addr_t pvarg) +{ + int fd; + + /* Wait a bit */ + + printf("null_writer: started -- sleeping\n"); + sleep(5); + + /* Then open the FIFO for write access */ + + printf("null_writer: Opening FIFO for write access\n"); + fd = open(FIFO_PATH2, O_WRONLY); + if (fd < 0) + { + fprintf(stderr, "null_writer: Failed to open FIFO %s for writing, errno=%d\n", + FIFO_PATH2, errno); + return (void*)1; + } + + /* Wait a bit more */ + + printf("null_writer: Opened %s for writing -- sleeping\n", FIFO_PATH2); + sleep(5); + + /* Then close the FIFO */ + + printf("null_writer: Closing %s\n", FIFO_PATH2); + if (close(fd) != 0) + { + fprintf(stderr, "null_writer: close failed: %d\n", errno); + } + sleep(5); + + printf("null_writer: Returning success\n"); + return (void*)0; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: interlock_test + ****************************************************************************/ + +int interlock_test(void) +{ + pthread_t writerid; + void *value; + char data[16]; + ssize_t nbytes; + int fd; + int ret; + + /* Create a FIFO */ + + ret = mkfifo(FIFO_PATH2, 0666); + if (ret < 0) + { + fprintf(stderr, "interlock_test: mkfifo failed with errno=%d\n", errno); + return 1; + } + + /* Start the null_writer_thread */ + + printf("interlock_test: Starting null_writer thread\n"); + ret = pthread_create(&writerid, NULL, null_writer, (pthread_addr_t)NULL); + if (ret != 0) + { + fprintf(stderr, "interlock_test: Failed to create null_writer thread, error=%d\n", ret); + ret = 2; + goto errout_with_fifo; + } + + /* Open one end of the FIFO for reading. This open call should block until the + * null_writer thread opens the other end of the FIFO for writing. + */ + + printf("interlock_test: Opening FIFO for read access\n"); + fd = open(FIFO_PATH2, O_RDONLY); + if (fd < 0) + { + fprintf(stderr, "interlock_test: Failed to open FIFO %s for reading, errno=%d\n", + FIFO_PATH2, errno); + ret = 3; + goto errout_with_thread; + } + + /* Attempt to read one byte from the FIFO. This should return end-of-file because + * the null_writer closes the FIFO without writing anything. + */ + + printf("interlock_test: Reading from %s\n", FIFO_PATH2); + nbytes = read(fd, data, 16); + if (nbytes < 0 ) + { + fprintf(stderr, "interlock_test: read failed, errno=%d\n", errno); + ret = 4; + goto errout_with_file; + } + else if (ret != 0) + { + fprintf(stderr, "interlock_test: Read %d bytes of data -- aborting: %d\n", nbytes); + ret = 5; + goto errout_with_file; + } + + /* Close the file */ + + printf("interlock_test: Closing %s\n", FIFO_PATH2); + if (close(fd) != 0) + { + fprintf(stderr, "interlock_test: close failed: %d\n", errno); + } + + /* Wait for null_writer thread to complete */ + + printf("interlock_test: Waiting for null_writer thread\n"); + ret = pthread_join(writerid, &value); + if (ret != 0) + { + fprintf(stderr, "interlock_test: pthread_join failed, error=%d\n", ret); + ret = 6; + goto errout_with_fifo; + } + else + { + printf("interlock_test: writer returned %d\n", (int)value); + if (value != (void*)0) + { + ret = 7; + goto errout_with_fifo; + } + } + + /* unlink(FIFO_PATH2); */ + printf("interlock_test: Returning success\n"); + return 0; + +errout_with_file: + if (close(fd) != 0) + { + fprintf(stderr, "interlock_test: close failed: %d\n", errno); + } +errout_with_thread: + pthread_detach(writerid); + pthread_cancel(writerid); +errout_with_fifo: + /* unlink(FIFO_PATH2); */ + printf("interlock_test: Returning %d\n", ret); + return ret; +} diff --git a/apps/examples/pipe/pipe.h b/apps/examples/pipe/pipe.h new file mode 100644 index 000000000..2c18fd868 --- /dev/null +++ b/apps/examples/pipe/pipe.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * examples/pipe/pipe.h + * + * Copyright (C) 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __EXAMPLES_PIPE_PIPE_H +#define __EXAMPLES_PIPE_PIPE_H + +/**************************************************************************** + * Compilation Switches + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define FIFO_PATH1 "/tmp/testfifo-1" +#define FIFO_PATH2 "/tmp/testfifo-2" + +#ifndef CONFIG_EXAMPLES_PIPE_STACKSIZE +# define CONFIG_EXAMPLES_PIPE_STACKSIZE 1024 +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +extern int transfer_test(int fdin, int fdout); +extern int interlock_test(void); +extern int redirection_test(void); + +#endif /* __EXAMPLES_PIPE_PIPE_H */ diff --git a/apps/examples/pipe/pipe_main.c b/apps/examples/pipe/pipe_main.c new file mode 100644 index 000000000..1f0f73032 --- /dev/null +++ b/apps/examples/pipe/pipe_main.c @@ -0,0 +1,189 @@ +/**************************************************************************** + * examples/pipe/pipe_main.c + * + * Copyright (C) 2008-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/stat.h> +#include <stdio.h> +#include <unistd.h> +#include <sched.h> +#include <fcntl.h> +#include <errno.h> + +#include "pipe.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start + ****************************************************************************/ + +int user_start(int argc, char *argv[]) +{ + int filedes[2]; + int ret; + + /* Test FIFO logic */ + + printf("\nuser_start: Performing FIFO test\n"); + ret = mkfifo(FIFO_PATH1, 0666); + if (ret < 0) + { + fprintf(stderr, "user_start: mkfifo failed with errno=%d\n", errno); + return 1; + } + + /* Open one end of the FIFO for reading and the other end for writing. NOTE: + * the following might not work on most FIFO implementations because the attempt + * to open just one end of the FIFO for writing might block. The NuttX FIFOs block + * only on open for read-only (see interlock_test()). + */ + + filedes[1] = open(FIFO_PATH1, O_WRONLY); + if (filedes[1] < 0) + { + fprintf(stderr, "user_start: Failed to open FIFO %s for writing, errno=%d\n", + FIFO_PATH1, errno); + return 2; + } + + filedes[0] = open(FIFO_PATH1, O_RDONLY); + if (filedes[0] < 0) + { + fprintf(stderr, "user_start: Failed to open FIFO %s for reading, errno=%d\n", + FIFO_PATH1, errno); + if (close(filedes[1]) != 0) + { + fprintf(stderr, "user_start: close failed: %d\n", errno); + } + return 3; + } + + /* Then perform the test using those file descriptors */ + + ret = transfer_test(filedes[0], filedes[1]); + if (close(filedes[0]) != 0) + { + fprintf(stderr, "user_start: close failed: %d\n", errno); + } + if (close(filedes[1]) != 0) + { + fprintf(stderr, "user_start: close failed: %d\n", errno); + } + /* unlink(FIFO_PATH1); fails */ + + if (ret != 0) + { + fprintf(stderr, "user_start: FIFO test FAILED (%d)\n", ret); + return 4; + } + printf("user_start: FIFO test PASSED\n"); + + /* Test PIPE logic */ + + printf("\nuser_start: Performing pipe test\n"); + ret = pipe(filedes); + if (ret < 0) + { + fprintf(stderr, "user_start: pipe failed with errno=%d\n", errno); + return 5; + } + + /* Then perform the test using those file descriptors */ + + ret = transfer_test(filedes[0], filedes[1]); + if (close(filedes[0]) != 0) + { + fprintf(stderr, "user_start: close failed: %d\n", errno); + } + if (close(filedes[1]) != 0) + { + fprintf(stderr, "user_start: close failed: %d\n", errno); + } + + if (ret != 0) + { + fprintf(stderr, "user_start: PIPE test FAILED (%d)\n", ret); + return 6; + } + printf("user_start: PIPE test PASSED\n"); + + /* Perform the FIFO interlock test */ + + printf("\nuser_start: Performing pipe interlock test\n"); + ret = interlock_test(); + if (ret != 0) + { + fprintf(stderr, "user_start: FIFO interlock test FAILED (%d)\n", ret); + return 7; + } + printf("user_start: PIPE interlock test PASSED\n"); + + /* Perform the pipe redirection test */ + + printf("\nuser_start: Performing redirection test\n"); + ret = redirection_test(); + if (ret != 0) + { + fprintf(stderr, "user_start: FIFO redirection test FAILED (%d)\n", ret); + return 7; + } + printf("user_start: PIPE redirection test PASSED\n"); + + fflush(stdout); + return 0; +} diff --git a/apps/examples/pipe/redirect_test.c b/apps/examples/pipe/redirect_test.c new file mode 100644 index 000000000..45e86c356 --- /dev/null +++ b/apps/examples/pipe/redirect_test.c @@ -0,0 +1,326 @@ +/**************************************************************************** + * examples/pipe/redirect_test.c + * + * Copyright (C) 2008-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <sched.h> +#include <semaphore.h> +#include <errno.h> + +#include "pipe.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define READ_SIZE 37 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static sem_t g_rddone; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: redirect_reader + ****************************************************************************/ + +static int redirect_reader(int argc, char *argv[]) +{ + char buffer[READ_SIZE]; + int fdin; + int fdout; + int ret; + int nbytes = 0; + + printf("redirect_reader: started with fdin=%s\n", argv[1]); + + /* Convert the fdin to binary */ + + fdin = atoi(argv[1]); + fdout = atoi(argv[2]); + + /* Close fdout -- we don't need it */ + + ret = close(fdout); + if (ret != 0) + { + fprintf(stderr, "redirect_reader: failed to close fdout=%d\n", fdout); + return 1; + } + + /* Re-direct the fdin to stdin */ + + ret = dup2(fdin, 0); + if (ret != 0) + { + fprintf(stderr, "redirect_reader: dup2 failed: %d\n", errno); + close(fdin); + return 2; + } + + /* Close the original file descriptor */ + + ret = close(fdin); + if (ret != 0) + { + fprintf(stderr, "redirect_reader: failed to close fdin=%d\n", fdin); + return 3; + } + + /* Then read from stdin until we hit the end of file */ + + fflush(stdout); + for (;;) + { + /* Read from stdin */ + + ret = read(0, buffer, READ_SIZE); + if (ret < 0 ) + { + fprintf(stderr, "redirect_reader: read failed, errno=%d\n", errno); + return 4; + } + else if (ret == 0) + { + break; + } + nbytes += ret; + + /* Echo to stdout */ + + ret = write(1, buffer, ret); + if (ret < 0) + { + fprintf(stderr, "redirect_reader: read failed, errno=%d\n", errno); + return 5; + } + } + + printf("redirect_reader: %d bytes read\n", nbytes); + ret = close(0); + if (ret != 0) + { + fprintf(stderr, "redirect_reader: failed to close fd=0\n"); + return 6; + } + + sem_post(&g_rddone); + printf("redirect_reader: Returning success\n"); + return 0; +} + +/**************************************************************************** + * Name: redirect_writer + ****************************************************************************/ + +static int redirect_writer(int argc, char *argv[]) +{ + int fdin; + int fdout; + int nbytes = 0; + int ret; + + fprintf(stderr, "redirect_writer: started with fdout=%s\n", argv[2]); + + /* Convert the fdout to binary */ + + fdin = atoi(argv[1]); + fdout = atoi(argv[2]); + + /* Close fdin -- we don't need it */ + + ret = close(fdin); + if (ret != 0) + { + fprintf(stderr, "redirect_reader: failed to close fdin=%d\n", fdin); + return 1; + } + + /* Re-direct the fdout to stdout */ + + ret = dup2(fdout, 1); + if (ret != 0) + { + fprintf(stderr, "redirect_writer: dup2 failed: %d\n", errno); + return 2; + } + + /* Close the original file descriptor */ + + ret = close(fdout); + if (ret != 0) + { + fprintf(stderr, "redirect_reader: failed to close fdout=%d\n", fdout); + return 3; + } + + /* Then write a bunch of stuff to stdout */ + + fflush(stderr); + nbytes += printf("\nFour score and seven years ago our fathers brought forth on this continent a new nation,\n"); + nbytes += printf("conceived in Liberty, and dedicated to the proposition that all men are created equal.\n"); + nbytes += printf("\nNow we are engaged in a great civil war, testing whether that nation, or any nation, so\n"); + nbytes += printf("conceived and so dedicated, can long endure. We are met on a great battle-field of that war.\n"); + nbytes += printf("We have come to dedicate a portion of that field, as a final resting place for those who here\n"); + nbytes += printf("gave their lives that that nation might live. It is altogether fitting and proper that we\n"); + nbytes += printf("should do this.\n"); + nbytes += printf("\nBut, in a larger sense, we can not dedicate - we can not consecrate - we can not hallow - this ground.\n"); + nbytes += printf("The brave men, living and dead, who struggled here, have consecrated it, far above our poor power\n"); + nbytes += printf("to add or detract. The world will little note, nor long remember what we say here, but it can\n"); + nbytes += printf("never forget what they did here. It is for us the living, rather, to be dedicated here to the\n"); + nbytes += printf("unfinished work which they who fought here have thus far so nobly advanced. It is rather for us to\n"); + nbytes += printf("be here dedicated to the great task remaining before us - that from these honored dead we take\n"); + nbytes += printf("increased devotion to that cause for which they gave the last full measure of devotion - that we\n"); + nbytes += printf("here highly resolve that these dead shall not have died in vain - that this nation, under God,\n"); + nbytes += printf("shall have a new birth of freedom - and that government of the people, by the people, for the\n"); + nbytes += printf("people, shall not perish from the earth.\n\n"); + fflush(stdout); + + fprintf(stderr, "redirect_writer: %d bytes written\n", nbytes); + + ret = close(1); + if (ret != 0) + { + fprintf(stderr, "redirect_writer: failed to close fd=1\n"); + return 4; + } + + fprintf(stderr, "redirect_writer: Returning success\n"); + return 0; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: redirection_test + ****************************************************************************/ + +int redirection_test(void) +{ + const char *argv[3]; + char buffer1[8]; + char buffer2[8]; + int readerid; + int writerid; + int filedes[2]; + int ret; + + sem_init(&g_rddone, 0, 0); + + /* Create the pipe */ + + ret = pipe(filedes); + if (ret < 0) + { + fprintf(stderr, "redirection_test: pipe failed with errno=%d\n", errno); + return 5; + } + + sprintf(buffer1, "%d", filedes[0]); + argv[0] = buffer1; + sprintf(buffer2, "%d", filedes[1]); + argv[1] = buffer2; + argv[2] = NULL; + + /* Start redirect_reader thread */ + + printf("redirection_test: Starting redirect_reader task with fd=%d\n", filedes[0]); + readerid = task_create("redirect_reader", 50, CONFIG_EXAMPLES_PIPE_STACKSIZE, redirect_reader, argv); + if (readerid < 0) + { + fprintf(stderr, "redirection_test: Failed to create redirect_writer task: %d\n", errno); + return 1; + } + + /* Start redirect_writer task */ + + printf("redirection_test: Starting redirect_writer task with fd=%d\n", filedes[1]); + writerid = task_create("redirect_writer", 50, CONFIG_EXAMPLES_PIPE_STACKSIZE, redirect_writer, argv); + if (writerid < 0) + { + fprintf(stderr, "redirection_test: Failed to create redirect_writer task: %d\n", errno); + ret = task_delete(readerid); + if (ret != 0) + { + fprintf(stderr, "redirection_test: Failed to delete redirect_reader task %d\n", errno); + } + return 2; + } + + /* We should be able to close the pipe file descriptors now. */ + + if (close(filedes[0]) != 0) + { + fprintf(stderr, "user_start: close failed: %d\n", errno); + } + if (close(filedes[1]) != 0) + { + fprintf(stderr, "user_start: close failed: %d\n", errno); + } + + if (ret != 0) + { + fprintf(stderr, "user_start: PIPE test FAILED (%d)\n", ret); + return 6; + } + + /* Wait for redirect_writer thread to complete */ + + printf("redirection_test: Waiting...\n"); + fflush(stdout); + sem_wait(&g_rddone); + + printf("redirection_test: returning %d\n", ret); + return ret; +} + diff --git a/apps/examples/pipe/transfer_test.c b/apps/examples/pipe/transfer_test.c new file mode 100644 index 000000000..cb8cad04a --- /dev/null +++ b/apps/examples/pipe/transfer_test.c @@ -0,0 +1,242 @@ +/**************************************************************************** + * examples/pipe/transfer_test.c + * + * Copyright (C) 2008-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <unistd.h> +#include <errno.h> + +#include "pipe.h" + +/**************************************************************************** + * Pre-proecessor Definitions + ****************************************************************************/ + +#define MAX_BYTE 13 + +#define WRITE_SIZE MAX_BYTE +#define NWRITES 1400 +#define NWRITE_BYTES (NWRITES * WRITE_SIZE) + +#define READ_SIZE (2*MAX_BYTE) +#define NREADS (NWRITES / 2) +#define NREAD_BYTES NWRITE_BYTES + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: transfer_reader + ****************************************************************************/ + +static void *transfer_reader(pthread_addr_t pvarg) +{ + char buffer[READ_SIZE]; + int fd = (int)pvarg; + int ret; + int nbytes; + int value; + int ndx; + + printf("transfer_reader: started\n"); + for (nbytes = 0, value = 0; nbytes < NREAD_BYTES;) + { + ret = read(fd, buffer, READ_SIZE); + if (ret < 0 ) + { + fprintf(stderr, "transfer_reader: read failed, errno=%d\n", errno); + return (void*)1; + } + else if (ret == 0) + { + if (nbytes < NREAD_BYTES) + { + fprintf(stderr, "transfer_reader: Too few bytes read -- aborting: %d\n", nbytes); + return (void*)2; + } + break; + } + for (ndx = 0; ndx < ret; ndx++) + { + if (value >= WRITE_SIZE) + { + value = 0; + } + if (buffer[ndx] != value) + { + fprintf(stderr, "transfer_reader: Byte %d, expected %d, found %d\n", + nbytes + ndx, value, buffer[ndx]); + return (void*)3; + } + value++; + } + nbytes += ret; + if (nbytes > NREAD_BYTES) + { + fprintf(stderr, "transfer_reader: Too many bytes read -- aborting: %d\n", nbytes); + return (void*)4; + } + } + printf("transfer_reader: %d bytes read\n", nbytes); + return (void*)0; +} + +/**************************************************************************** + * Name: transfer_writer + ****************************************************************************/ + +static void *transfer_writer(pthread_addr_t pvarg) +{ + char buffer[WRITE_SIZE]; + int fd = (int)pvarg; + int ret; + int i; + + printf("transfer_writer: started\n"); + for (i = 0; i < WRITE_SIZE; i++) + { + buffer[i] = i; + } + + for (i = 0; i < NWRITES; i++) + { + ret = write(fd, buffer, WRITE_SIZE); + if (ret < 0 ) + { + fprintf(stderr, "transfer_writer: write failed, errno=%d\n", errno); + return (void*)1; + } + else if (ret != WRITE_SIZE) + { + fprintf(stderr, "transfer_writer: Unexpected write size=%d\n", ret); + return (void*)2; + } + } + printf("transfer_writer: %d bytes written\n", NWRITE_BYTES); + return (void*)0; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: transfer_test + ****************************************************************************/ + +int transfer_test(int fdin, int fdout) +{ + pthread_t readerid; + pthread_t writerid; + void *value; + int tmp; + int ret; + + /* Start transfer_reader thread */ + + printf("transfer_test: Starting transfer_reader thread\n"); + ret = pthread_create(&readerid, NULL, transfer_reader, (pthread_addr_t)fdin); + if (ret != 0) + { + fprintf(stderr, "transfer_test: Failed to create transfer_reader thread, error=%d\n", ret); + return 1; + } + + /* Start transfer_writer thread */ + + printf("transfer_test: Starting transfer_writer thread\n"); + ret = pthread_create(&writerid, NULL, transfer_writer, (pthread_addr_t)fdout); + if (ret != 0) + { + fprintf(stderr, "transfer_test: Failed to create transfer_writer thread, error=%d\n", ret); + pthread_detach(readerid); + ret = pthread_cancel(readerid); + if (ret != 0) + { + fprintf(stderr, "transfer_test: Failed to cancel transfer_reader thread, error=%d\n", ret); + } + return 2; + } + + /* Wait for transfer_writer thread to complete */ + + printf("transfer_test: Waiting for transfer_writer thread\n"); + ret = pthread_join(writerid, &value); + if (ret != 0) + { + fprintf(stderr, "transfer_test: pthread_join failed, error=%d\n", ret); + } + else + { + ret = (int)value; + printf("transfer_test: transfer_writer returned %d\n", ret); + } + + /* Wait for transfer_reader thread to complete */ + + printf("transfer_test: Waiting for transfer_reader thread\n"); + tmp = pthread_join(readerid, &value); + if (tmp != 0) + { + fprintf(stderr, "transfer_test: pthread_join failed, error=%d\n", ret); + } + else + { + tmp = (int)value; + printf("transfer_test: transfer_reader returned %d\n", tmp); + } + + if (ret == 0) + { + ret = tmp; + } + printf("transfer_test: returning %d\n", ret); + return ret; +} + diff --git a/apps/examples/poll/Kconfig b/apps/examples/poll/Kconfig new file mode 100644 index 000000000..c52827496 --- /dev/null +++ b/apps/examples/poll/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_POLL + bool "Poll example" + default n + ---help--- + Enable the poll example + +if EXAMPLES_POLL +endif diff --git a/apps/examples/poll/Makefile b/apps/examples/poll/Makefile new file mode 100644 index 000000000..1c85d6f36 --- /dev/null +++ b/apps/examples/poll/Makefile @@ -0,0 +1,94 @@ +############################################################################ +# apps/examples/poll/Makefile +# +# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# Device Driver poll()/select() Example + +ASRCS = +CSRCS = poll_main.c poll_listener.c select_listener.c net_listener.c net_reader.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +# Register application +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend host + +-include Make.dep diff --git a/apps/examples/poll/Makefile.host b/apps/examples/poll/Makefile.host new file mode 100644 index 000000000..9d9daee27 --- /dev/null +++ b/apps/examples/poll/Makefile.host @@ -0,0 +1,54 @@ +############################################################################ +# apps/examples/poll/Makefile.host +# +# Copyright (C) 2008, 2009, 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +# TOPDIR must be defined on the make command line + +include $(TOPDIR)/.config +include $(TOPDIR)/Make.defs + +SRC = host.c +BIN = host + +DEFINES = -DTARGETIP=\"$(TARGETIP)\" + +all: $(BIN) + +$(BIN): $(SRC) + $(HOSTCC) $(HOSTCFLAGS) $(DEFINES) $^ -o $@ + +clean: + @rm -f $(BIN) *~ .*.swp *.o + $(call CLEAN) + diff --git a/apps/examples/poll/host.c b/apps/examples/poll/host.c new file mode 100644 index 000000000..302cceb0f --- /dev/null +++ b/apps/examples/poll/host.c @@ -0,0 +1,171 @@ +/**************************************************************************** + * examples/poll/host.c + * + * Copyright (C) 2008-2009, 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <sys/socket.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <pthread.h> +#include <string.h> +#include <errno.h> + +#include <netinet/in.h> +#include <arpa/inet.h> + +#define pthread_addr_t void * +#include "poll_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifndef TARGETIP +# error TARGETIP not defined +#endif + +#define IOBUFFER_SIZE 80 + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * main + ****************************************************************************/ + +int main(int argc, char **argv, char **envp) +{ + struct sockaddr_in myaddr; + char outbuf[IOBUFFER_SIZE]; + char inbuf[IOBUFFER_SIZE]; + int sockfd; + int len; + int nbytessent; + int nbytesrecvd; + int i; + + /* Create a new TCP socket */ + + sockfd = socket(PF_INET, SOCK_STREAM, 0); + if (sockfd < 0) + { + message("client socket failure %d\n", errno); + goto errout_with_outbufs; + } + + /* Connect the socket to the server */ + + myaddr.sin_family = AF_INET; + myaddr.sin_port = htons(LISTENER_PORT); + myaddr.sin_addr.s_addr = inet_addr(TARGETIP); + + message("client: Connecting to %s...\n", TARGETIP); + if (connect( sockfd, (struct sockaddr*)&myaddr, sizeof(struct sockaddr_in)) < 0) + { + message("client: connect failure: %d\n", errno); + goto errout_with_socket; + } + message("client: Connected\n"); + + /* Then send and receive messages */ + + for (i = 0; ; i++) + { + sprintf(outbuf, "Remote message %d", i); + len = strlen(outbuf); + + message("client: Sending '%s' (%d bytes)\n", outbuf, len); + nbytessent = send(sockfd, outbuf, len, 0); + message("client: Sent %d bytes\n", nbytessent); + + if (nbytessent < 0) + { + message("client: send failed: %d\n", errno); + goto errout_with_socket; + } + else if (nbytessent != len) + { + message("client: Bad send length: %d Expected: %d\n", nbytessent, len); + goto errout_with_socket; + } + + message("client: Receiving...\n"); + nbytesrecvd = recv(sockfd, inbuf, IOBUFFER_SIZE, 0); + + if (nbytesrecvd < 0) + { + message("client: recv failed: %d\n", errno); + goto errout_with_socket; + } + else if (nbytesrecvd == 0) + { + message("client: The server broke the connections\n"); + goto errout_with_socket; + } + + inbuf[nbytesrecvd] = '\0'; + message("client: Received '%s' (%d bytes)\n", inbuf, nbytesrecvd); + + if (nbytesrecvd != len) + { + message("client: Bad recv length: %d Expected: %d\n", nbytesrecvd, len); + goto errout_with_socket; + } + else if (memcmp(inbuf, outbuf, len) != 0) + { + message("client: Received outbuf does not match sent outbuf\n"); + goto errout_with_socket; + } + + message("client: Sleeping\n"); + sleep(8); + } + + close(sockfd); + return 0; + +errout_with_socket: + close(sockfd); +errout_with_outbufs: + exit(1); +} diff --git a/apps/examples/poll/net_listener.c b/apps/examples/poll/net_listener.c new file mode 100644 index 000000000..81ad7cdcc --- /dev/null +++ b/apps/examples/poll/net_listener.c @@ -0,0 +1,428 @@ +/**************************************************************************** + * examples/poll/net_listener.c + * + * Copyright (C) 2008-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/stat.h> +#include <sys/select.h> +#include <sys/socket.h> +#include <sys/ioctl.h> + +#include <stdint.h> +#include <stdbool.h> +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <pthread.h> +#include <errno.h> +#include <debug.h> + +#include <net/if.h> +#include <apps/netutils/uiplib.h> + +#include "poll_internal.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define IOBUFFER_SIZE 80 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct net_listener_s +{ + struct sockaddr_in addr; + fd_set master; + fd_set working; + char buffer[IOBUFFER_SIZE]; + int listensd; + int mxsd; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: net_closeclient + ****************************************************************************/ + +static bool net_closeclient(struct net_listener_s *nls, int sd) +{ + message("net_listener: Closing host side connection sd=%d\n", sd); + close(sd); + FD_CLR(sd, &nls->master); + + /* If we just closed the max SD, then search downward for the next biggest SD. */ + + while (FD_ISSET(nls->mxsd, &nls->master) == false) + { + nls->mxsd -= 1; + } + return true; +} + +/**************************************************************************** + * Name: net_incomingdata + ****************************************************************************/ + +static inline bool net_incomingdata(struct net_listener_s *nls, int sd) +{ + char *ptr; + int nbytes; + int ret; + + /* Read data from the socket */ + +#ifdef FIONBIO + for (;;) +#endif + { + message("net_listener: Read data from sd=%d\n", sd); + ret = recv(sd, nls->buffer, IOBUFFER_SIZE, 0); + if (ret < 0) + { + if (errno != EINTR) + { + message("net_listener: recv failed sd=%d: %d\n", sd, errno); + if (errno != EAGAIN) + { + net_closeclient(nls, sd); + return false; + } + } + } + else if (ret == 0) + { + message("net_listener: Client connection lost sd=%d\n", sd); + net_closeclient(nls, sd); + return false; + } + else + { + nls->buffer[ret]='\0'; + message("poll_listener: Read '%s' (%d bytes)\n", nls->buffer, ret); + + /* Echo the data back to the client */ + + for (nbytes = ret, ptr = nls->buffer; nbytes > 0; ) + { + ret = send(sd, ptr, nbytes, 0); + if (ret < 0) + { + if (errno != EINTR) + { + message("net_listener: Send failed sd=%d: \n", sd, errno); + net_closeclient(nls, sd); + return false; + } + } + else + { + nbytes -= ret; + ptr += ret; + } + } + } + } + return 0; +} + +/**************************************************************************** + * Name: net_connection + ****************************************************************************/ + +static inline bool net_connection(struct net_listener_s *nls) +{ + int sd; + + /* Loop until all connections have been processed */ + +#ifdef FIONBIO + for (;;) +#endif + { + message("net_listener: Accepting new connection on sd=%d\n", nls->listensd); + + sd = accept(nls->listensd, NULL, NULL); + if (sd < 0) + { + message("net_listener: accept failed: %d\n", errno); + + if (errno != EINTR) + { + return false; + } + } + else + { + /* Add the new connection to the master set */ + + message("net_listener: Connection accepted for sd=%d\n", sd); + + FD_SET(sd, &nls->master); + if (sd > nls->mxsd) + { + nls->mxsd = sd; + } + return true; + } + } + return false; +} + +/**************************************************************************** + * Name: net_mksocket + ****************************************************************************/ + +static inline bool net_mksocket(struct net_listener_s *nls) +{ + int value; + int ret; + + /* Create a listening socket */ + + message("net_listener: Initializing listener socket\n"); + nls->listensd = socket(AF_INET, SOCK_STREAM, 0); + if (nls->listensd < 0) + { + message("net_listener: socket failed: %d\n", errno); + return false; + } + + /* Configure the socket */ + + value = 1; + ret = setsockopt(nls->listensd, SOL_SOCKET, SO_REUSEADDR, (char*)&value, sizeof(int)); + if (ret < 0) + { + message("net_listener: setsockopt failed: %d\n", errno); + close(nls->listensd); + return false; + } + + /* Set the socket to non-blocking */ + +#ifdef FIONBIO + ret = ioctl(nls->listensd, FIONBIO, (char *)&value); + if (ret < 0) + { + message("net_listener: ioctl failed: %d\n", errno); + close(nls->listensd); + return false; + } +#endif + + /* Bind the socket */ + + memset(&nls->addr, 0, sizeof(struct sockaddr_in)); + nls->addr.sin_family = AF_INET; + nls->addr.sin_addr.s_addr = htonl(INADDR_ANY); + nls->addr.sin_port = htons(LISTENER_PORT); + ret = bind(nls->listensd, (struct sockaddr *)&nls->addr, sizeof(struct sockaddr_in)); + if (ret < 0) + { + message("net_listener: bind failed: %d\n", errno); + close(nls->listensd); + return false; + } + + /* Mark the socket as a listener */ + + ret = listen(nls->listensd, 32); + if (ret < 0) + { + message("net_listener: bind failed: %d\n", errno); + close(nls->listensd); + return false; + } + + return true; +} + +/**************************************************************************** + * Name: net_configure + ****************************************************************************/ + +static void net_configure(void) +{ + struct in_addr addr; +#if defined(CONFIG_EXAMPLE_POLL_NOMAC) + uint8_t mac[IFHWADDRLEN]; +#endif + + /* Configure uIP */ + /* Many embedded network interfaces must have a software assigned MAC */ + +#ifdef CONFIG_EXAMPLE_POLL_NOMAC + mac[0] = 0x00; + mac[1] = 0xe0; + mac[2] = 0xde; + mac[3] = 0xad; + mac[4] = 0xbe; + mac[5] = 0xef; + uip_setmacaddr("eth0", mac); +#endif + + /* Set up our host address */ + + addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_IPADDR); + uip_sethostaddr("eth0", &addr); + + /* Set up the default router address */ + + addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_DRIPADDR); + uip_setdraddr("eth0", &addr); + + /* Setup the subnet mask */ + + addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_NETMASK); + uip_setnetmask("eth0", &addr); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: net_listener + ****************************************************************************/ + +void *net_listener(pthread_addr_t pvarg) +{ + struct net_listener_s nls; + struct timeval timeout; + int nsds; + int ret; + int i; + + /* Configure uIP */ + + net_configure(); + + /* Set up a listening socket */ + + memset(&nls, 0, sizeof(struct net_listener_s)); + if (!net_mksocket(&nls)) + { + return (void*)1; + } + + /* Initialize the 'master' file descriptor set */ + + FD_ZERO(&nls.master); + nls.mxsd = nls.listensd; + FD_SET(nls.listensd, &nls.master); + + /* Set up a 3 second timeout */ + + timeout.tv_sec = NET_LISTENER_DELAY; + timeout.tv_usec = 0; + + /* Loop waiting for incoming connections or for incoming data + * on any of the connect sockets. + */ + + for (;;) + { + /* Wait on select */ + + message("net_listener: Calling select(), listener sd=%d\n", nls.listensd); + memcpy(&nls.working, &nls.master, sizeof(fd_set)); + ret = select(nls.mxsd + 1, (FAR fd_set*)&nls.working, (FAR fd_set*)NULL, (FAR fd_set*)NULL, &timeout); + if (ret < 0) + { + message("net_listener: select failed: %d\n", errno); + break; + } + + /* Check for timeout */ + + if (ret == 0) + { + message("net_listener: Timeout\n"); + continue; + } + + /* Find which descriptors caused the wakeup */ + + nsds = ret; + for (i = 0; i <= nls.mxsd && nsds > 0; i++) + { + /* Is this descriptor ready? */ + + if (FD_ISSET(i, &nls.working)) + { + /* Yes, is it our listener? */ + + message("net_listener: Activity on sd=%d\n", i); + + nsds--; + if (i == nls.listensd) + { + (void)net_connection(&nls); + } + else + { + net_incomingdata(&nls, i); + } + } + } + } + + /* Cleanup */ + +#if 0 /* Don't get here */ + for (i = 0; i <= nls.mxsd; +i++) + { + if (FD_ISSET(i, &nls.master)) + { + close(i); + } + } +#endif + return NULL; /* Keeps some compilers from complaining */ +} diff --git a/apps/examples/poll/net_reader.c b/apps/examples/poll/net_reader.c new file mode 100644 index 000000000..8a13618c3 --- /dev/null +++ b/apps/examples/poll/net_reader.c @@ -0,0 +1,317 @@ +/**************************************************************************** + * examples/poll/net_reader.c + * + * Copyright (C) 2008-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/stat.h> +#include <sys/select.h> +#include <sys/socket.h> + +#include <stdint.h> +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <pthread.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <net/if.h> +#include <apps/netutils/uiplib.h> + +#include "poll_internal.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define IOBUFFER_SIZE 80 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: net_configure + ****************************************************************************/ + +static void net_configure(void) +{ + struct in_addr addr; +#if defined(CONFIG_EXAMPLE_POLL_NOMAC) + uint8_t mac[IFHWADDRLEN]; +#endif + + /* Configure uIP */ + /* Many embedded network interfaces must have a software assigned MAC */ + +#ifdef CONFIG_EXAMPLE_POLL_NOMAC + mac[0] = 0x00; + mac[1] = 0xe0; + mac[2] = 0xde; + mac[3] = 0xad; + mac[4] = 0xbe; + mac[5] = 0xef; + uip_setmacaddr("eth0", mac); +#endif + + /* Set up our host address */ + + addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_IPADDR); + uip_sethostaddr("eth0", &addr); + + /* Set up the default router address */ + + addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_DRIPADDR); + uip_setdraddr("eth0", &addr); + + /* Setup the subnet mask */ + + addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_NETMASK); + uip_setnetmask("eth0", &addr); +} + +/**************************************************************************** + * Name: net_receive + ****************************************************************************/ + +static void net_receive(int sd) +{ + struct timeval timeout; + char buffer[IOBUFFER_SIZE]; + char *ptr; + fd_set readset; + int nbytes; + int ret; + + /* Set up the timeout */ + + timeout.tv_sec = NET_LISTENER_DELAY; + timeout.tv_usec = 0; + + /* Loop while we have the connection */ + + for (;;) + { + /* Wait for incoming message */ + + do + { + FD_ZERO(&readset); + FD_SET(sd, &readset); + ret = select(sd + 1, (FAR fd_set*)&readset, (FAR fd_set*)NULL, (FAR fd_set*)NULL, &timeout); + } + while (ret < 0 && errno == EINTR); + + /* Something has happened */ + + if (ret < 0) + { + message("net_reader: select failed: %d\n", errno); + return; + } + else if (ret == 0) + { + message("net_reader: Timeout\n"); + } + else + { + message("net_reader: Read data from sd=%d\n", sd); + memset(buffer, '?', IOBUFFER_SIZE); /* Just to make sure we really receive something */ + ret = recv(sd, buffer, IOBUFFER_SIZE, 0); + if (ret < 0) + { + if (errno != EINTR) + { + message("net_reader: recv failed sd=%d: %d\n", sd, errno); + if (errno != EAGAIN) + { + return; + } + } + } + else if (ret == 0) + { + message("net_reader: Client connection lost sd=%d\n", sd); + return; + } + else + { + buffer[ret]='\0'; + message("net_reader: Read '%s' (%d bytes)\n", buffer, ret); + + /* Echo the data back to the client */ + + for (nbytes = ret, ptr = buffer; nbytes > 0; ) + { + ret = send(sd, ptr, nbytes, 0); + if (ret < 0) + { + if (errno != EINTR) + { + message("net_reader: Send failed sd=%d: %d\n", sd, errno); + return; + } + } + else + { + nbytes -= ret; + ptr += ret; + } + } + } + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: net_reader + ****************************************************************************/ + +void *net_reader(pthread_addr_t pvarg) +{ + struct sockaddr_in addr; +#ifdef POLL_HAVE_SOLINGER + struct linger ling; +#endif + int listensd; + int acceptsd; + socklen_t addrlen; + int optval; + + /* Configure uIP */ + + net_configure(); + + /* Create a new TCP socket */ + + listensd = socket(PF_INET, SOCK_STREAM, 0); + if (listensd < 0) + { + message("net_reader: socket failure: %d\n", errno); + goto errout; + } + + /* Set socket to reuse address */ + + optval = 1; + if (setsockopt(listensd, SOL_SOCKET, SO_REUSEADDR, (void*)&optval, sizeof(int)) < 0) + { + message("net_reader: setsockopt SO_REUSEADDR failure: %d\n", errno); + goto errout_with_listensd; + } + + /* Bind the socket to a local address */ + + addr.sin_family = AF_INET; + addr.sin_port = HTONS(LISTENER_PORT); + addr.sin_addr.s_addr = INADDR_ANY; + + if (bind(listensd, (struct sockaddr*)&addr, sizeof(struct sockaddr_in)) < 0) + { + message("net_reader: bind failure: %d\n", errno); + goto errout_with_listensd; + } + + /* Listen for connections on the bound TCP socket */ + + if (listen(listensd, 5) < 0) + { + message("net_reader: listen failure %d\n", errno); + goto errout_with_listensd; + } + + /* Connection loop */ + + for (;;) + { + /* Accept only one connection */ + + message("net_reader: Accepting new connections on port %d\n", LISTENER_PORT); + addrlen = sizeof(struct sockaddr_in); + acceptsd = accept(listensd, (struct sockaddr*)&addr, &addrlen); + if (acceptsd < 0) + { + message("net_reader: accept failure: %d\n", errno); + continue; + } + message("net_reader: Connection accepted on sd=%d\n", acceptsd); + + /* Configure to "linger" until all data is sent when the socket is closed */ + +#ifdef POLL_HAVE_SOLINGER + ling.l_onoff = 1; + ling.l_linger = 30; /* timeout is seconds */ + if (setsockopt(acceptsd, SOL_SOCKET, SO_LINGER, &ling, sizeof(struct linger)) < 0) + { + message("net_reader: setsockopt SO_LINGER failure: %d\n", errno); + goto errout_with_acceptsd; + } +#endif + + /* Handle incoming messsages on the connection. */ + + net_receive(acceptsd); + + message("net_reader: Closing sd=%d\n", acceptsd); + close(acceptsd); + } + +#ifdef POLL_HAVE_SOLINGER +errout_with_acceptsd: + close(acceptsd); +#endif +errout_with_listensd: + close(listensd); +errout: + return NULL; +} diff --git a/apps/examples/poll/poll_internal.h b/apps/examples/poll/poll_internal.h new file mode 100644 index 000000000..cbf42ac56 --- /dev/null +++ b/apps/examples/poll/poll_internal.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * examples/poll/poll_internal.h + * + * Copyright (C) 2008, 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __EXAMPLES_PIPE_PIPE_H +#define __EXAMPLES_PIPE_PIPE_H + +/**************************************************************************** + * Compilation Switches + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <pthread.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#ifdef CONFIG_DISABLE_POLL +# error "The polling API is disabled" +#endif + +/* Here are all of the configuration settings that must be met to have TCP/IP + * poll/select support. This kind of looks like overkill. + * + * CONFIG_NET - Network support must be enabled + * CONFIG_NSOCKET_DESCRIPTORS - Socket descriptors must be allocated + * CONFIG_NET_TCP - Only support on TCP (because read-ahead + * ibuffering s not yet support for UDP) + * CONFIG_NET_NTCP_READAHEAD_BUFFERS - TCP/IP read-ahead buffering must be enabled + */ + +#if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0 && \ + defined(CONFIG_NET_TCP) && CONFIG_NET_NTCP_READAHEAD_BUFFERS > 0 +# define HAVE_NETPOLL 1 +#else +# undef HAVE_NETPOLL +#endif + +/* If debug is enabled, then use lib_rawprintf so that OS debug output and + * the test output are synchronized. + * + * These macros will differ depending upon if the toolchain supports + * macros with a variable number of arguments or not. + */ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +#define FIFO_PATH1 "/dev/fifo0" +#define FIFO_PATH2 "/dev/fifo1" + +#define POLL_LISTENER_DELAY 2000 /* 2 seconds */ +#define SELECT_LISTENER_DELAY 4 /* 4 seconds */ +#define NET_LISTENER_DELAY 3 /* 3 seconds */ +#define WRITER_DELAY 6 /* 6 seconds */ + +#define LISTENER_PORT 5471 + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +extern void *poll_listener(pthread_addr_t pvarg); +extern void *select_listener(pthread_addr_t pvarg); + +#ifdef HAVE_NETPOLL +extern void *net_listener(pthread_addr_t pvarg); +extern void *net_reader(pthread_addr_t pvarg); +#endif +#endif /* __EXAMPLES_PIPE_PIPE_H */ diff --git a/apps/examples/poll/poll_listener.c b/apps/examples/poll/poll_listener.c new file mode 100644 index 000000000..816647e34 --- /dev/null +++ b/apps/examples/poll/poll_listener.c @@ -0,0 +1,262 @@ +/**************************************************************************** + * examples/poll/poll_listener.c + * + * Copyright (C) 2008 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/stat.h> +#include <stdbool.h> +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <pthread.h> +#include <poll.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include "poll_internal.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#if defined(CONFIG_DEV_CONSOLE) && !defined(CONFIG_DEV_LOWCONSOLE) +# define HAVE_CONSOLE +# define NPOLLFDS 2 +# define CONSNDX 0 +# define FIFONDX 1 +#else +# undef HAVE_CONSOLE +# define NPOLLFDS 1 +# define FIFONDX 0 +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: poll_listener + ****************************************************************************/ + +void *poll_listener(pthread_addr_t pvarg) +{ + struct pollfd fds[NPOLLFDS]; + char buffer[64]; + ssize_t nbytes; + bool timeout; + bool pollin; + int nevents; + int fd; + int ret; + int i; + + /* Open the FIFO for non-blocking read */ + + message("poll_listener: Opening %s for non-blocking read\n", FIFO_PATH1); + fd = open(FIFO_PATH1, O_RDONLY|O_NONBLOCK); + if (fd < 0) + { + message("poll_listener: ERROR Failed to open FIFO %s: %d\n", + FIFO_PATH1, errno); + (void)close(fd); + return (void*)-1; + } + + /* Loop forever */ + + for (;;) + { + message("poll_listener: Calling poll()\n"); + + memset(fds, 0, sizeof(struct pollfd)*NPOLLFDS); +#ifdef HAVE_CONSOLE + fds[CONSNDX].fd = 0; + fds[CONSNDX].events = POLLIN; + fds[CONSNDX].revents = 0; +#endif + fds[FIFONDX].fd = fd; + fds[FIFONDX].events = POLLIN; + fds[FIFONDX].revents = 0; + + timeout = false; + pollin = false; + + ret = poll(fds, NPOLLFDS, POLL_LISTENER_DELAY); + + message("\npoll_listener: poll returned: %d\n", ret); + if (ret < 0) + { + message("poll_listener: ERROR poll failed: %d\n", errno); + } + else if (ret == 0) + { + message("poll_listener: Timeout\n"); + timeout = true; + } + else if (ret > NPOLLFDS) + { + message("poll_listener: ERROR poll reported: %d\n"); + } + else + { + pollin = true; + } + + nevents = 0; + for (i = 0; i < NPOLLFDS; i++) + { + message("poll_listener: FIFO revents[%d]=%02x\n", i, fds[i].revents); + if (timeout) + { + if (fds[i].revents != 0) + { + message("poll_listener: ERROR? expected revents=00, received revents[%d]=%02x\n", + fds[i].revents, i); + } + } + else if (pollin) + { + if (fds[i].revents == POLLIN) + { + nevents++; + } + else if (fds[i].revents != 0) + { + message("poll_listener: ERROR unexpected revents[i]=%02x\n", + i, fds[i].revents); + } + } + } + + if (pollin && nevents != ret) + { + message("poll_listener: ERROR found %d events, poll reported %d\n", nevents, ret); + } + + /* In any event, read until the pipe/serial is empty */ + + for (i = 0; i < NPOLLFDS; i++) + { + do + { +#ifdef HAVE_CONSOLE + /* Hack to work around the fact that the console driver on the + * simulator is always non-blocking. + */ + + if (i == CONSNDX) + { + if ((fds[CONSNDX].revents & POLLIN) != 0) + { + buffer[0] = getchar(); + nbytes = 1; + } + else + { + nbytes = 0; + } + } + else +#endif + { + /* The pipe works differently, it returns whatever data + * it has available without blocking. + */ + + nbytes = read(fds[i].fd, buffer, 63); + } + + if (nbytes <= 0) + { + if (nbytes == 0 || errno == EAGAIN) + { + if ((fds[i].revents & POLLIN) != 0) + { + message("poll_listener: ERROR no read data[%d]\n", i); + } + } + else if (errno != EINTR) + { + message("poll_listener: read[%d] failed: %d\n", i, errno); + } + nbytes = 0; + } + else + { + if (timeout) + { + message("poll_listener: ERROR? Poll timeout, but data read[%d]\n", i); + message(" (might just be a race condition)\n"); + } + + buffer[nbytes] = '\0'; + message("poll_listener: Read[%d] '%s' (%d bytes)\n", i, buffer, nbytes); + } + + /* Suppress error report if no read data on the next time through */ + + fds[i].revents = 0; + } + while (nbytes > 0); + } + + /* Make sure that everything is displayed */ + + msgflush(); + } + + /* Won't get here */ + + (void)close(fd); + return NULL; +} diff --git a/apps/examples/poll/poll_main.c b/apps/examples/poll/poll_main.c new file mode 100644 index 000000000..3db0150e7 --- /dev/null +++ b/apps/examples/poll/poll_main.c @@ -0,0 +1,221 @@ +/**************************************************************************** + * examples/poll/poll_main.c + * + * Copyright (C) 2008, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/stat.h> + +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <pthread.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <debug.h> + +#include "poll_internal.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start + ****************************************************************************/ + +int user_start(int argc, char *argv[]) +{ + char buffer[64]; + ssize_t nbytes; + pthread_t tid1; + pthread_t tid2; +#ifdef HAVE_NETPOLL + pthread_t tid3; +#endif + int count; + int fd1 = -1; + int fd2 = -1; + int ret; + int exitcode = 0; + + /* Open FIFOs */ + + message("\nuser_start: Creating FIFO %s\n", FIFO_PATH1); + ret = mkfifo(FIFO_PATH1, 0666); + if (ret < 0) + { + message("user_start: mkfifo failed: %d\n", errno); + exitcode = 1; + goto errout; + } + + message("\nuser_start: Creating FIFO %s\n", FIFO_PATH2); + ret = mkfifo(FIFO_PATH2, 0666); + if (ret < 0) + { + message("user_start: mkfifo failed: %d\n", errno); + exitcode = 2; + goto errout; + } + + /* Open the FIFOs for blocking, write */ + + fd1 = open(FIFO_PATH1, O_WRONLY); + if (fd1 < 0) + { + message("user_start: Failed to open FIFO %s for writing, errno=%d\n", + FIFO_PATH1, errno); + exitcode = 3; + goto errout; + } + + fd2 = open(FIFO_PATH2, O_WRONLY); + if (fd2 < 0) + { + message("user_start: Failed to open FIFO %s for writing, errno=%d\n", + FIFO_PATH2, errno); + exitcode = 4; + goto errout; + } + + /* Start the listeners */ + + message("user_start: Starting poll_listener thread\n"); + + ret = pthread_create(&tid1, NULL, poll_listener, NULL); + if (ret != 0) + { + message("user_start: Failed to create poll_listener thread: %d\n", ret); + exitcode = 5; + goto errout; + } + + message("user_start: Starting select_listener thread\n"); + + ret = pthread_create(&tid2, NULL, select_listener, NULL); + if (ret != 0) + { + message("user_start: Failed to create select_listener thread: %d\n", ret); + exitcode = 6; + goto errout; + } + +#ifdef HAVE_NETPOLL +#ifdef CONFIG_NET_TCPBACKLOG + message("user_start: Starting net_listener thread\n"); + + ret = pthread_create(&tid3, NULL, net_listener, NULL); +#else + message("user_start: Starting net_reader thread\n"); + + ret = pthread_create(&tid3, NULL, net_reader, NULL); +#endif + if (ret != 0) + { + message("user_start: Failed to create net_listener thread: %d\n", ret); + } +#endif + + /* Loop forever */ + + for (count = 0; ; count++) + { + /* Send a message to the listener... this should wake the listener + * from the poll. + */ + + sprintf(buffer, "Message %d", count); + nbytes = write(fd1, buffer, strlen(buffer)); + if (nbytes < 0) + { + message("user_start: Write to fd1 failed: %d\n", errno); + exitcode = 7; + goto errout; + } + + nbytes = write(fd2, buffer, strlen(buffer)); + if (nbytes < 0) + { + message("user_start: Write fd2 failed: %d\n", errno); + exitcode = 8; + goto errout; + } + + message("\nuser_start: Sent '%s' (%d bytes)\n", buffer, nbytes); + msgflush(); + + /* Wait awhile. This delay should be long enough that the + * listener will timeout. + */ + + sleep(WRITER_DELAY); + } + +errout: + if (fd1 >= 0) + { + close(fd1); + } + + if (fd2 >= 0) + { + close(fd2); + } + + fflush(stdout); + return exitcode; +} diff --git a/apps/examples/poll/select_listener.c b/apps/examples/poll/select_listener.c new file mode 100644 index 000000000..80039ada3 --- /dev/null +++ b/apps/examples/poll/select_listener.c @@ -0,0 +1,193 @@ +/**************************************************************************** + * examples/poll/select_listener.c + * + * Copyright (C) 2008, 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/stat.h> +#include <sys/select.h> +#include <stdbool.h> +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <pthread.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include "poll_internal.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: select_listener + ****************************************************************************/ + +void *select_listener(pthread_addr_t pvarg) +{ + fd_set rfds; + struct timeval tv; + char buffer[64]; + ssize_t nbytes; + bool timeout; + bool ready; + int fd; + int ret; + + /* Open the FIFO for non-blocking read */ + + message("select_listener: Opening %s for non-blocking read\n", FIFO_PATH2); + fd = open(FIFO_PATH2, O_RDONLY|O_NONBLOCK); + if (fd < 0) + { + message("select_listener: ERROR Failed to open FIFO %s: %d\n", + FIFO_PATH2, errno); + (void)close(fd); + return (void*)-1; + } + + /* Loop forever */ + + for (;;) + { + message("select_listener: Calling select()\n"); + + FD_ZERO(&rfds); + FD_SET(fd, &rfds); + + tv.tv_sec = SELECT_LISTENER_DELAY; + tv.tv_usec = 0; + + timeout = false; + ready = false; + + ret = select(fd+1, (FAR fd_set*)&rfds, (FAR fd_set*)NULL, (FAR fd_set*)NULL, &tv); + message("\nselect_listener: select returned: %d\n", ret); + + if (ret < 0) + { + message("select_listener: ERROR select failed: %d\n"); + } + else if (ret == 0) + { + message("select_listener: Timeout\n"); + timeout = true; + } + else + { + if (ret != 1) + { + message("select_listener: ERROR poll reported: %d\n"); + } + else + { + ready = true; + } + + if (!FD_ISSET(fd, rfds)) + { + message("select_listener: ERROR fd=%d not in fd_set\n"); + } + } + + /* In any event, read until the pipe is empty */ + + do + { + nbytes = read(fd, buffer, 63); + if (nbytes <= 0) + { + if (nbytes == 0 || errno == EAGAIN) + { + if (ready) + { + message("select_listener: ERROR no read data\n"); + } + } + else if (errno != EINTR) + { + message("select_listener: read failed: %d\n", errno); + } + nbytes = 0; + } + else + { + if (timeout) + { + message("select_listener: ERROR? Poll timeout, but data read\n"); + message(" (might just be a race condition)\n"); + } + + buffer[nbytes] = '\0'; + message("select_listener: Read '%s' (%d bytes)\n", buffer, nbytes); + } + + timeout = false; + ready = false; + } + while (nbytes > 0); + + /* Make sure that everything is displayed */ + + msgflush(); + } + + /* Won't get here */ + + (void)close(fd); + return NULL; +} diff --git a/apps/examples/pwm/Kconfig b/apps/examples/pwm/Kconfig new file mode 100644 index 000000000..78edd2178 --- /dev/null +++ b/apps/examples/pwm/Kconfig @@ -0,0 +1,48 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_PWM + bool "Pulse width modulation (PWM) example" + default n + depends on PWM && NSH_BUILTIN_APPS + ---help--- + Enable the Pulse width modulation (PWM) example + +if EXAMPLES_PWM + +config EXAMPLES_PWM_DEVPATH + string "PWM device path" + default "/dev/pwm0" + ---help--- + The path to the PWM device. Default: /dev/pwm0 + +config EXAMPLES_PWM_FREQUENCY + int "Default PWM freququency" + default 100 + ---help--- + The default PWM frequency. Default: 100 Hz + +config EXAMPLES_PWM_DUTYPCT + int "Default PWM duty percentage" + default 50 + ---help--- + The default PWM duty as a percentage. Default: 50% + +config EXAMPLES_PWM_DURATION + int "Default PWM duration" + default 5 if !EXAMPLES_PWM_PULSECOUNT + ---help--- + The default PWM pulse train duration in seconds. Used only if the current + pulse count is zero (pulse countis only supported if CONFIG_PWM_PULSECOUNT + is defined). Default: 5 seconds + +config EXAMPLES_PWM_PULSECOUNT + int "Default pulse count" + default 0 + ---help--- + The initial PWM pulse count. This option is only available if CONFIG_PWM_PULSECOUNT + is nonzero. Default: 0 (i.e., use the duration, not the count). + +endif diff --git a/apps/examples/pwm/Makefile b/apps/examples/pwm/Makefile new file mode 100644 index 000000000..efbdb048e --- /dev/null +++ b/apps/examples/pwm/Makefile @@ -0,0 +1,103 @@ +############################################################################ +# apps/examples/pwm/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# PWM Example. + +ASRCS = +CSRCS = pwm_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# PWM built-in application info + +APPNAME = pwm +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: context clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/pwm/pwm.h b/apps/examples/pwm/pwm.h new file mode 100644 index 000000000..5c049a8f8 --- /dev/null +++ b/apps/examples/pwm/pwm.h @@ -0,0 +1,134 @@ +/**************************************************************************** + * examples/examples/pwm/pwm.h + * + * 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 + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_EXAMPLES_PWM_PWM_H +#define __APPS_EXAMPLES_PWM_PWM_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* CONFIG_NSH_BUILTIN_APPS - Build the PWM test as an NSH built-in function. + * Default: Not built! The example can only be used as an NSH built-in + * application + * CONFIG_EXAMPLES_PWM_DEVPATH - The path to the PWM device. Default: /dev/pwm0 + * CONFIG_EXAMPLES_PWM_FREQUENCY - The initial PWM frequency. Default: 100 Hz + * CONFIG_EXAMPLES_PWM_DUTYPCT - The initial PWM duty as a percentage. Default: 50% + * CONFIG_EXAMPLES_PWM_DURATION - The initial PWM pulse train duration in seconds. + * Used only if the current pulse count is zero (pulse count is only supported + * if CONFIG_PWM_PULSECOUNT is defined). Default: 5 seconds + * CONFIG_EXAMPLES_PWM_PULSECOUNT - The initial PWM pulse count. This option is + * only available if CONFIG_PWM_PULSECOUNT is defined. Default: 0 (i.e., use + * the duration, not the count). + */ + +#ifndef CONFIG_PWM +# error "PWM device support is not enabled (CONFIG_PWM)" +#endif + +#ifndef CONFIG_NSH_BUILTIN_APPS +# warning "The PWM example only works as an NSH built-in application (CONFIG_NSH_BUILTIN_APPS)" +#endif + +#ifndef CONFIG_EXAMPLES_PWM_DEVPATH +# define CONFIG_EXAMPLES_PWM_DEVPATH "/dev/pwm0" +#endif + +#ifndef CONFIG_EXAMPLES_PWM_FREQUENCY +# define CONFIG_EXAMPLES_PWM_FREQUENCY 100 +#endif + +#ifndef CONFIG_EXAMPLES_PWM_DUTYPCT +# define CONFIG_EXAMPLES_PWM_DUTYPCT 50 +#endif + +#ifndef CONFIG_EXAMPLES_PWM_DURATION +# define CONFIG_EXAMPLES_PWM_DURATION 5 +#endif + +#ifndef CONFIG_EXAMPLES_PWM_COUNT +# define CONFIG_EXAMPLES_PWM_COUNT 0 +#endif + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: pwm_devinit() + * + * Description: + * Perform architecuture-specific initialization of the PWM hardware. This + * interface must be provided by all configurations using apps/examples/pwm + * + ****************************************************************************/ + +int pwm_devinit(void); + +#endif /* __APPS_EXAMPLES_PWM_PWM_H */ diff --git a/apps/examples/pwm/pwm_main.c b/apps/examples/pwm/pwm_main.c new file mode 100644 index 000000000..64c5dfb2c --- /dev/null +++ b/apps/examples/pwm/pwm_main.c @@ -0,0 +1,394 @@ +/**************************************************************************** + * examples/pwm/pwm_main.c + * + * 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 + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/ioctl.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/pwm.h> + +#include "pwm.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct pwm_state_s +{ + bool initialized; + FAR char *devpath; + uint8_t duty; + uint32_t freq; +#ifdef CONFIG_PWM_PULSECOUNT + uint32_t count; +#endif + int duration; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct pwm_state_s g_pwmstate; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pwm_devpath + ****************************************************************************/ + +static void pwm_devpath(FAR struct pwm_state_s *pwm, FAR const char *devpath) +{ + /* Get rid of any old device path */ + + if (pwm->devpath) + { + free(pwm->devpath); + } + + /* Then set-up the new device path by copying the string */ + + pwm->devpath = strdup(devpath); +} + +/**************************************************************************** + * Name: pwm_help + ****************************************************************************/ + +static void pwm_help(FAR struct pwm_state_s *pwm) +{ + message("Usage: pwm [OPTIONS]\n"); + message("\nArguments are \"sticky\". For example, once the PWM frequency is\n"); + message("specified, that frequency will be re-used until it is changed.\n"); + message("\n\"sticky\" OPTIONS include:\n"); + message(" [-p devpath] selects the PWM device. " + "Default: %s Current: %s\n", + CONFIG_EXAMPLES_PWM_DEVPATH, pwm->devpath ? pwm->devpath : "NONE"); + message(" [-f addr] selects the pulse frequency. " + "Default: %d Hz Current: %d Hz\n", + CONFIG_EXAMPLES_PWM_FREQUENCY, pwm->freq); + message(" [-d duty] selcts the pulse duty as a percentage. " + "Default: %d %% Current: %d %%\n", + CONFIG_EXAMPLES_PWM_DUTYPCT, pwm->duty); +#ifdef CONFIG_PWM_PULSECOUNT + message(" [-n count] selects the pulse count. " + "Default: %d Current: %d\n", + CONFIG_EXAMPLES_PWM_COUNT, pwm->count); +#endif + message(" [-t duration] is the duration of the pulse train in seconds. " + "Default: %d Current: %d\n", + CONFIG_EXAMPLES_PWM_DURATION, pwm->duration); + message(" [-h] shows this message and exits\n"); +} + +/**************************************************************************** + * Name: arg_string + ****************************************************************************/ + +static int arg_string(FAR char **arg, FAR char **value) +{ + FAR char *ptr = *arg; + + if (ptr[2] == '\0') + { + *value = arg[1]; + return 2; + } + else + { + *value = &ptr[2]; + return 1; + } +} + +/**************************************************************************** + * Name: arg_decimal + ****************************************************************************/ + +static int arg_decimal(FAR char **arg, FAR long *value) +{ + FAR char *string; + int ret; + + ret = arg_string(arg, &string); + *value = strtol(string, NULL, 10); + return ret; +} + +/**************************************************************************** + * Name: parse_args + ****************************************************************************/ + +static void parse_args(FAR struct pwm_state_s *pwm, int argc, FAR char **argv) +{ + FAR char *ptr; + FAR char *str; + long value; + int index; + int nargs; + + for (index = 1; index < argc; ) + { + ptr = argv[index]; + if (ptr[0] != '-') + { + message("Invalid options format: %s\n", ptr); + exit(0); + } + + switch (ptr[1]) + { + case 'f': + nargs = arg_decimal(&argv[index], &value); + if (value < 1) + { + message("Frequency out of range: %ld\n", value); + exit(1); + } + + pwm->freq = (uint32_t)value; + index += nargs; + break; + + case 'd': + nargs = arg_decimal(&argv[index], &value); + if (value < 1 || value > 99) + { + message("Duty out of range: %ld\n", value); + exit(1); + } + + pwm->duty = (uint8_t)value; + index += nargs; + break; + +#ifdef CONFIG_PWM_PULSECOUNT + case 'n': + nargs = arg_decimal(&argv[index], &value); + if (value < 0) + { + message("Count must be non-negative: %ld\n", value); + exit(1); + } + + pwm->count = (uint32_t)value; + index += nargs; + break; + +#endif + case 'p': + nargs = arg_string(&argv[index], &str); + pwm_devpath(pwm, str); + index += nargs; + break; + + case 't': + nargs = arg_decimal(&argv[index], &value); + if (value < 1 || value > INT_MAX) + { + message("Duration out of range: %ld\n", value); + exit(1); + } + + pwm->duration = (int)value; + index += nargs; + break; + + case 'h': + pwm_help(pwm); + exit(0); + + default: + message("Unsupported option: %s\n", ptr); + pwm_help(pwm); + exit(1); + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start/pwm_main + ****************************************************************************/ + +int pwm_main(int argc, char *argv[]) +{ + struct pwm_info_s info; + int fd; + int ret; + + /* Initialize the state data */ + + if (!g_pwmstate.initialized) + { + g_pwmstate.duty = CONFIG_EXAMPLES_PWM_DUTYPCT; + g_pwmstate.freq = CONFIG_EXAMPLES_PWM_FREQUENCY; + g_pwmstate.duration = CONFIG_EXAMPLES_PWM_DURATION; +#ifdef CONFIG_PWM_PULSECOUNT + g_pwmstate.count = CONFIG_EXAMPLES_PWM_COUNT; +#endif + g_pwmstate.initialized = true; + } + + /* Parse the command line */ + + parse_args(&g_pwmstate, argc, argv); + + /* Has a device been assigned? */ + + if (!g_pwmstate.devpath) + { + /* No.. use the default device */ + + pwm_devpath(&g_pwmstate, CONFIG_EXAMPLES_PWM_DEVPATH); + } + + /* Initialization of the PWM hardware is performed by logic external to + * this test. + */ + + ret = pwm_devinit(); + if (ret != OK) + { + message("pwm_main: pwm_devinit failed: %d\n", ret); + goto errout; + } + + /* Open the PWM device for reading */ + + fd = open(g_pwmstate.devpath, O_RDONLY); + if (fd < 0) + { + message("pwm_main: open %s failed: %d\n", g_pwmstate.devpath, errno); + goto errout; + } + + /* Configure the characteristics of the pulse train */ + + info.frequency = g_pwmstate.freq; + info.duty = ((uint32_t)g_pwmstate.duty << 16) / 100; +#ifdef CONFIG_PWM_PULSECOUNT + info.count = g_pwmstate.count; + + message("pwm_main: starting output with frequency: %d duty: %08x count: %d\n", + info.frequency, info.duty, info.count); + +#else + message("pwm_main: starting output with frequency: %d duty: %08x\n", + info.frequency, info.duty); + +#endif + + ret = ioctl(fd, PWMIOC_SETCHARACTERISTICS, (unsigned long)((uintptr_t)&info)); + if (ret < 0) + { + message("pwm_main: ioctl(PWMIOC_SETCHARACTERISTICS) failed: %d\n", errno); + goto errout_with_dev; + } + + /* Then start the pulse train. Since the driver was opened in blocking + * mode, this call will block if the count value is greater than zero. + */ + + ret = ioctl(fd, PWMIOC_START, 0); + if (ret < 0) + { + message("pwm_main: ioctl(PWMIOC_START) failed: %d\n", errno); + goto errout_with_dev; + } + + /* It a non-zero count was not specified, then wait for the selected + * duration, then stop the PWM output. + */ + +#ifdef CONFIG_PWM_PULSECOUNT + if (info.count == 0) +#endif + { + /* Wait for the specified duration */ + + sleep(g_pwmstate.duration); + + /* Then stop the pulse train */ + + message("pwm_main: stopping output\n", info.frequency, info.duty); + + ret = ioctl(fd, PWMIOC_STOP, 0); + if (ret < 0) + { + message("pwm_main: ioctl(PWMIOC_STOP) failed: %d\n", errno); + goto errout_with_dev; + } + } + + close(fd); + msgflush(); + return OK; + +errout_with_dev: + close(fd); +errout: + msgflush(); + return ERROR; +} diff --git a/apps/examples/qencoder/Kconfig b/apps/examples/qencoder/Kconfig new file mode 100644 index 000000000..e0026d08c --- /dev/null +++ b/apps/examples/qencoder/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_QENCODER + bool "Quadrature encoder example" + default n + ---help--- + Enable the quadrature encoder example + +if EXAMPLES_QENCODER +endif diff --git a/apps/examples/qencoder/Makefile b/apps/examples/qencoder/Makefile new file mode 100644 index 000000000..3f3fc9def --- /dev/null +++ b/apps/examples/qencoder/Makefile @@ -0,0 +1,105 @@ +############################################################################ +# apps/examples/qe/Makefile +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# NuttX NX Graphics Example. + +ASRCS = +CSRCS = qe_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Quadrature Encoder built-in application info + +APPNAME = qe +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: context clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: +ifeq ($(CONFIG_NSH_BUILTIN_APPS),y) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +endif + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/qencoder/qe.h b/apps/examples/qencoder/qe.h new file mode 100644 index 000000000..4c03689ab --- /dev/null +++ b/apps/examples/qencoder/qe.h @@ -0,0 +1,134 @@ +/**************************************************************************** + * examples/examples/qe/qe.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_EXAMPLES_QENCODER_QE_H +#define __APPS_EXAMPLES_QENCODER_QE_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* CONFIG_NSH_BUILTIN_APPS - Build the QE test as an NSH built-in function. + * Default: Built as a standalone problem + * 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 + */ + +#ifndef CONFIG_QENCODER +# error "QE device support is not enabled (CONFIG_QENCODER)" +#endif + +#ifndef CONFIG_EXAMPLES_QENCODER_DEVPATH +# define CONFIG_EXAMPLES_QENCODER_DEVPATH "/dev/qe0" +#endif + +#ifndef CONFIG_EXAMPLES_QENCODER_DELAY +# define CONFIG_EXAMPLES_QENCODER_DELAY 100 +#endif + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +struct qe_example_s +{ + bool initialized; /* True: QE devices have been initialized */ + bool reset; /* True: set the count back to zero */ + FAR char *devpath; /* Path to the QE device */ + unsigned int nloops; /* Collect this number of samples */ + unsigned int delay; /* Delay this number of seconds between samples */ +}; +#endif + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +extern struct qe_example_s g_qeexample; +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: qe_devinit() + * + * Description: + * Perform architecuture-specific initialization of the QE hardware. This + * interface must be provided by all configurations using apps/examples/qe + * + ****************************************************************************/ + +int qe_devinit(void); + +#endif /* __APPS_EXAMPLES_QENCODER_QE_H */ diff --git a/apps/examples/qencoder/qe_main.c b/apps/examples/qencoder/qe_main.c new file mode 100644 index 000000000..c58a2b0ad --- /dev/null +++ b/apps/examples/qencoder/qe_main.c @@ -0,0 +1,370 @@ +/**************************************************************************** + * examples/qe/qe_main.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/ioctl.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <fcntl.h> +#include <limits.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/sensors/qencoder.h> + +#include "qe.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +# define MAIN_NAME qe_main +# define MAIN_STRING "qe_main: " +#else +# define MAIN_NAME user_start +# define MAIN_STRING "user_start: " +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +struct qe_example_s g_qeexample; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: qe_devpath + ****************************************************************************/ + +static void qe_devpath(FAR const char *devpath) +{ + /* Get rid of any old device path */ + + if (g_qeexample.devpath) + { + free(g_qeexample.devpath); + } + + /* The set-up the new device path by copying the string */ + + g_qeexample.devpath = strdup(devpath); +} + +/**************************************************************************** + * Name: qe_help + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static void qe_help(void) +{ + message("\nUsage: qe [OPTIONS]\n\n"); + message("OPTIONS include:\n"); + message(" [-p devpath] QE device path\n"); + message(" [-n samples] Number of samples\n"); + message(" [-t msec] Delay between samples (msec)\n"); + message(" [-r] Reset the position to zero\n"); + message(" [-h] Shows this message and exits\n\n"); +} +#endif + +/**************************************************************************** + * Name: arg_string + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static int arg_string(FAR char **arg, FAR char **value) +{ + FAR char *ptr = *arg; + + if (ptr[2] == '\0') + { + *value = arg[1]; + return 2; + } + else + { + *value = &ptr[2]; + return 1; + } +} +#endif + +/**************************************************************************** + * Name: arg_decimal + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static int arg_decimal(FAR char **arg, FAR long *value) +{ + FAR char *string; + int ret; + + ret = arg_string(arg, &string); + *value = strtol(string, NULL, 10); + return ret; +} +#endif + +/**************************************************************************** + * Name: parse_args + ****************************************************************************/ + +#ifdef CONFIG_NSH_BUILTIN_APPS +static void parse_args(int argc, FAR char **argv) +{ + FAR char *ptr; + FAR char *str; + long value; + int index; + int nargs; + + g_qeexample.reset = false; + g_qeexample.nloops = 1; + g_qeexample.delay = CONFIG_EXAMPLES_QENCODER_DELAY; + + for (index = 1; index < argc; ) + { + ptr = argv[index]; + if (ptr[0] != '-') + { + message("Invalid options format: %s\n", ptr); + exit(0); + } + + switch (ptr[1]) + { + case 'n': + nargs = arg_decimal(&argv[index], &value); + if (value < 0 || value > INT_MAX) + { + message("Sample count out of range: %ld\n", value); + exit(1); + } + + g_qeexample.nloops = (unsigned int)value; + index += nargs; + break; + + case 'p': + nargs = arg_string(&argv[index], &str); + qe_devpath(str); + index += nargs; + break; + + case 't': + nargs = arg_decimal(&argv[index], &value); + if (value < 0 || value > INT_MAX) + { + message("Sample delay out of range: %ld\n", value); + exit(1); + } + + g_qeexample.delay = (unsigned int)value; + index += nargs; + break; + + case 'r': + g_qeexample.reset = true; + index++; + break; + + case 'h': + qe_help(); + exit(EXIT_SUCCESS); + + default: + message("Unsupported option: %s\n", ptr); + qe_help(); + exit(EXIT_FAILURE); + } + } +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start/qe_main + ****************************************************************************/ + +int MAIN_NAME(int argc, char *argv[]) +{ + int32_t position; + int fd; + int exitval = EXIT_SUCCESS; + int ret; +#if defined(CONFIG_NSH_BUILTIN_APPS) || defined(CONFIG_EXAMPLES_QENCODER_NSAMPLES) + int nloops; +#endif + + /* Check if we have initialized */ + + if (!g_qeexample.initialized) + { + /* Initialization of the encoder hardware is performed by logic external to + * this test. + */ + + message(MAIN_STRING "Initializing external encoder(s)\n"); + ret = qe_devinit(); + if (ret != OK) + { + message(MAIN_STRING "qe_devinit failed: %d\n", ret); + exitval = EXIT_FAILURE; + goto errout; + } + + /* Set the default values */ + + qe_devpath(CONFIG_EXAMPLES_QENCODER_DEVPATH); + g_qeexample.initialized = true; + } + + /* Parse command line arguments */ + +#ifdef CONFIG_NSH_BUILTIN_APPS + parse_args(argc, argv); +#endif + + /* Open the encoder device for reading */ + + message(MAIN_STRING "Hardware initialized. Opening the encoder device: %s\n", + g_qeexample.devpath); + + fd = open(g_qeexample.devpath, O_RDONLY); + if (fd < 0) + { + message(MAIN_STRING "open %s failed: %d\n", g_qeexample.devpath, errno); + exitval = EXIT_FAILURE; + goto errout_with_dev; + } + + /* Reset the count if so requested */ + + if (g_qeexample.reset) + { + message(MAIN_STRING "Resetting the count...\n"); + ret = ioctl(fd, QEIOC_RESET, 0); + if (ret < 0) + { + message(MAIN_STRING "ioctl(QEIOC_RESET) failed: %d\n", errno); + exitval = EXIT_FAILURE; + goto errout_with_dev; + } + } + + /* Now loop the appropriate number of times, displaying the collected + * encoder samples. + */ + +#if defined(CONFIG_NSH_BUILTIN_APPS) + message(MAIN_STRING "Number of samples: %d\n", g_qeexample.nloops); + for (nloops = 0; nloops < g_qeexample.nloops; nloops++) +#elif defined(CONFIG_EXAMPLES_QENCODER_NSAMPLES) + message(MAIN_STRING "Number of samples: %d\n", CONFIG_EXAMPLES_QENCODER_NSAMPLES); + for (nloops = 0; nloops < CONFIG_EXAMPLES_QENCODER_NSAMPLES; nloops++) +#else + for (;;) +#endif + { + /* Flush any output before the loop entered or from the previous pass + * through the loop. + */ + + msgflush(); + + /* Get the positions data using the ioctl */ + + ret = ioctl(fd, QEIOC_POSITION, (unsigned long)((uintptr_t)&position)); + if (ret < 0) + { + message(MAIN_STRING "ioctl(QEIOC_POSITION) failed: %d\n", errno); + exitval = EXIT_FAILURE; + goto errout_with_dev; + } + + /* Print the sample data on successful return */ + + else + { + message(MAIN_STRING "%3d. %d\n", nloops+1, position); + } + + /* Delay a little bit */ + +#if defined(CONFIG_NSH_BUILTIN_APPS) + usleep(g_qeexample.delay * 1000); +#else + usleep(CONFIG_EXAMPLES_QENCODER_DELAY * 1000); +#endif + } + +errout_with_dev: + close(fd); + +errout: + message("Terminating!\n"); + msgflush(); + return exitval; +} diff --git a/apps/examples/romfs/Kconfig b/apps/examples/romfs/Kconfig new file mode 100644 index 000000000..5a8c824a3 --- /dev/null +++ b/apps/examples/romfs/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_ROMFS + bool "ROMFS example" + default n + ---help--- + Enable the ROMFS example + +if EXAMPLES_ROMFS +endif diff --git a/apps/examples/romfs/Makefile b/apps/examples/romfs/Makefile new file mode 100644 index 000000000..1db45765a --- /dev/null +++ b/apps/examples/romfs/Makefile @@ -0,0 +1,111 @@ +############################################################################ +# apps/examples/romfs/Makefile +# +# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# ROMFS File System Example + +ASRCS = +CSRCS = romfs_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: checkgenromfs clean depend disclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +checkgenromfs: + @genromfs -h 1>/dev/null 2>&1 || { \ + echo "Host executable genromfs not available in PATH"; \ + echo "You may need to download in from http://romfs.sourceforge.net/"; \ + exit 1; \ + } + +testdir : testdir.tar.gz + @tar zxf $< || { echo "tar zxf $< failed" ; exit 1 ; } + +testdir.img : checkgenromfs testdir + @genromfs -f $@ -d testdir -V "ROMFS_Test" || { echo "genromfs failed" ; exit 1 ; } + +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 ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +# Register application +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend testdir.img + +-include Make.dep + diff --git a/apps/examples/romfs/romfs_main.c b/apps/examples/romfs/romfs_main.c new file mode 100644 index 000000000..32b3d2654 --- /dev/null +++ b/apps/examples/romfs/romfs_main.c @@ -0,0 +1,498 @@ +/**************************************************************************** + * examples/romfs/romfs_main.c + * + * Copyright (C) 2008-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* Mount the ROMFS image, Verifty that it contains the + * following: + * + * testdir + * |---------- [drwxr-xr-x 4096] adir + * | |------ [-rw-r--r-- 21] anotherfile.txt + * | |------ [drwxr-xr-x 4096] subdir + * | | `-- [-rw-r--r-- 21] subdirfile.txt + * | `------ [-rw-r--r-- 25] yafile.txt + * |---------- [-rw-r--r-- 15] afile.txt + * |---------- [-rw-r--r-- 21] hfile + * `---------- [lrwxrwxrwx 11] ldir -> adir/subdir + * + * testdir/ldir is a soft-link and should not be detectable. + * hfile is a hardlink to subdirfile and should be identical + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/mount.h> +#include <sys/stat.h> +#include <sys/mman.h> +#include <stdbool.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <fcntl.h> +#include <dirent.h> +#include <errno.h> + +#include <nuttx/ramdisk.h> + +#include "romfs_testdir.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* Configuration settings */ + +#ifndef CONFIG_EXAMPLES_ROMFS_RAMDEVNO +# define CONFIG_EXAMPLES_ROMFS_RAMDEVNO 1 +#endif + +#ifndef CONFIG_EXAMPLES_ROMFS_SECTORSIZE +# define CONFIG_EXAMPLES_ROMFS_SECTORSIZE 64 +#endif + +#ifndef CONFIG_EXAMPLES_ROMFS_MOUNTPOINT +# define CONFIG_EXAMPLES_ROMFS_MOUNTPOINT "/usr/local/share" +#endif + +#ifdef CONFIG_DISABLE_MOUNTPOINT +# error "Mountpoint support is disabled" +#endif + +#if CONFIG_NFILE_DESCRIPTORS < 4 +# error "Not enough file descriptors" +#endif + +#ifndef CONFIG_FS_ROMFS +# error "ROMFS support not enabled" +#endif + +#define NSECTORS(b) (((b)+CONFIG_EXAMPLES_ROMFS_SECTORSIZE-1)/CONFIG_EXAMPLES_ROMFS_SECTORSIZE) +#define STR_RAMDEVNO(m) #m +#define MKMOUNT_DEVNAME(m) "/dev/ram" STR_RAMDEVNO(m) +#define MOUNT_DEVNAME MKMOUNT_DEVNAME(CONFIG_EXAMPLES_ROMFS_RAMDEVNO) + +#define SCRATCHBUFFER_SIZE 1024 + +/* Test directory stuff */ + +#define WRITABLE_MODE (S_IWOTH|S_IWGRP|S_IWUSR) +#define READABLE_MODE (S_IROTH|S_IRGRP|S_IRUSR) +#define EXECUTABLE_MODE (S_IXOTH|S_IXGRP|S_IXUSR) + +#define DIRECTORY_MODE (S_IFDIR|READABLE_MODE|EXECUTABLE_MODE) +#define FILE_MODE (S_IFREG|READABLE_MODE) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct node_s +{ + struct node_s *peer; /* Next node in this directory */ + bool directory; /* True: directory */ + bool found; /* True: found and verified */ + const char *name; /* Node name */ + mode_t mode; /* Expected permissions */ + size_t size; /* Expected size */ + union + { + const char *filecontent; /* Context of text file */ + struct node_s *child; /* Subdirectory start */ + } u; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const char g_afilecontent[] = "This is a file\n"; +static const char g_anotherfilecontent[] = "This is another file\n"; +static const char g_yafilecontent[] = "This is yet another file\n"; +static const char g_subdirfilecontent[] = "File in subdirectory\n"; + +#define g_hfilecontent g_subdirfilecontent + +static struct node_s g_adir; +static struct node_s g_afile; +static struct node_s g_hfile; + +static struct node_s g_anotherfile; +static struct node_s g_subdir; +static struct node_s g_yafile; + +static struct node_s g_subdirfile; + +static int g_nerrors = 0; + +static char g_scratchbuffer[SCRATCHBUFFER_SIZE]; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: + ****************************************************************************/ + +static void connectem(void) +{ + g_adir.peer = &g_afile; + g_adir.directory = true; + g_adir.found = false; + g_adir.name = "adir"; + g_adir.mode = DIRECTORY_MODE; + g_adir.size = 0; + g_adir.u.child = &g_anotherfile; + + g_afile.peer = &g_hfile; + g_afile.directory = false; + g_afile.found = false; + g_afile.name = "afile.txt"; + g_afile.mode = FILE_MODE; + g_afile.size = strlen(g_afilecontent); + g_afile.u.filecontent = g_afilecontent; + + g_hfile.peer = NULL; + g_hfile.directory = false; /* Actually a hard link */ + g_hfile.found = false; + g_hfile.name = "hfile"; + g_hfile.mode = FILE_MODE; + g_hfile.size = strlen(g_hfilecontent); + g_hfile.u.filecontent = g_hfilecontent; + + g_anotherfile.peer = &g_yafile; + g_anotherfile.directory = false; + g_anotherfile.found = false; + g_anotherfile.name = "anotherfile.txt"; + g_anotherfile.mode = FILE_MODE; + g_anotherfile.size = strlen(g_anotherfilecontent); + g_anotherfile.u.filecontent = g_anotherfilecontent; + + g_yafile.peer = &g_subdir; + g_yafile.directory = false; + g_yafile.found = false; + g_yafile.name = "yafile.txt"; + g_yafile.mode = FILE_MODE; + g_yafile.size = strlen(g_yafilecontent); + g_yafile.u.filecontent = g_yafilecontent; + + g_subdir.peer = NULL; + g_subdir.directory = true; + g_subdir.found = false; + g_subdir.name = "subdir"; + g_subdir.mode = DIRECTORY_MODE; + g_subdir.size = 0; + g_subdir.u.child = &g_subdirfile; + + g_subdirfile.peer = NULL; + g_subdirfile.directory = false; + g_subdirfile.found = false; + g_subdirfile.name = "subdirfile.txt"; + g_subdirfile.mode = FILE_MODE; + g_subdirfile.size = strlen(g_subdirfilecontent); + g_subdirfile.u.filecontent = g_subdirfilecontent; +} + +/**************************************************************************** + * Name: findindirectory + ****************************************************************************/ + +static struct node_s *findindirectory(struct node_s *entry, const char *name) +{ + for (; entry; entry = entry->peer) + { + if (!entry->found && strcmp(entry->name, name) == 0) + { + entry->found = true; + return entry; + } + } + return NULL; +} + +/**************************************************************************** + * Name: checkattributes + ****************************************************************************/ + +static void checkattributes(const char *path, mode_t mode, size_t size) +{ + struct stat buf; + int ret; + + ret = stat(path, &buf); + if (ret != 0) + { + printf(" -- ERROR: Failed to stat %s: %d\n", path, errno); + g_nerrors++; + return; + } + + if (mode != buf.st_mode) + { + printf(" -- ERROR: Expected mode %08x, got %08x\n", mode, buf.st_mode); + g_nerrors++; + } + + if (size != buf.st_size) + { + printf(" -- ERROR: Expected size %d, got %d\n", mode, buf.st_size); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: checkfile + ****************************************************************************/ + +static void checkfile(const char *path, struct node_s *node) +{ + ssize_t nbytesread; + char *filedata; + int fd; + + /* Open the file */ + + fd = open(path, O_RDONLY); + if (fd < 0) + { + printf(" -- ERROR: Failed to open %s: %d\n", path, errno); + g_nerrors++; + return; + } + + /* Read and verify the file contents */ + + nbytesread = read(fd, g_scratchbuffer, SCRATCHBUFFER_SIZE); + if (nbytesread < 0) + { + printf(" -- ERROR: Failed to read from %s: %d\n", path, errno); + g_nerrors++; + } + else if (nbytesread != node->size) + { + printf(" -- ERROR: Read %d bytes, expected %d\n", nbytesread, node->size); + g_nerrors++; + } + else if (memcmp(g_scratchbuffer, node->u.filecontent, node->size) != 0) + { + g_scratchbuffer[nbytesread] = '\0'; + printf(" -- ERROR: File content read does not match expectation:\n"); + printf(" -- Read: [%s]\n", g_scratchbuffer); + printf(" -- Expected: [%s]\n", node->u.filecontent); + g_nerrors++; + } + + /* Memory map and verify the file contents */ + + filedata = (char*)mmap(NULL, node->size, PROT_READ, MAP_SHARED|MAP_FILE, fd, 0); + if (!filedata || filedata == (char*)MAP_FAILED) + { + printf(" -- ERROR: mmap of %s failed: %d\n", path, errno); + g_nerrors++; + } + else + { + if (memcmp(filedata, node->u.filecontent, node->size) != 0) + { + memcpy(g_scratchbuffer, filedata, node->size); + g_scratchbuffer[node->size] = '\0'; + printf(" -- ERROR: Mapped file content read does not match expectation:\n"); + printf(" -- Memory: [%s]\n", filedata); + printf(" -- Expected: [%s]\n", node->u.filecontent); + g_nerrors++; + } + munmap(filedata, node->size); + } + + /* Close the file */ + + if (close(fd) != OK) + { + printf(" -- ERROR: Failed to close %s: %d\n", path, errno); + g_nerrors++; + } +} + +/**************************************************************************** + * Name: readdirectories + ****************************************************************************/ + +static void readdirectories(const char *path, struct node_s *entry) +{ + DIR *dirp; + struct node_s *node; + struct dirent *direntry; + char *fullpath; + + printf("Traversing directory: %s\n", path); + dirp = opendir(path); + if (!dirp) + { + printf(" ERROR opendir(\"%s\") failed: %d\n", path, errno); + g_nerrors++; + return; + } + + for (direntry = readdir(dirp); direntry; direntry = readdir(dirp)) + { + if (strcmp(direntry->d_name, ".") == 0 || strcmp(direntry->d_name, "..") == 0) + { + printf(" Skipping %s\n", direntry->d_name); + continue; + } + + node = findindirectory(entry, direntry->d_name); + if (!node) + { + printf(" ERROR: No node found for %s\n", direntry->d_name); + g_nerrors++; + continue; + } + + /* Get the full path to the entry */ + + sprintf(g_scratchbuffer, "%s/%s", path, direntry->d_name); + fullpath = strdup(g_scratchbuffer); + + if (DIRENT_ISDIRECTORY(direntry->d_type)) + { + printf(" DIRECTORY: %s/\n", fullpath); + if (!node->directory) + { + printf(" -- ERROR: Expected type directory\n"); + g_nerrors++; + } + else + { + checkattributes(fullpath, node->mode, 0); + readdirectories(fullpath, node->u.child); + printf("Continuing directory: %s\n", path); + } + } + else + { + printf(" FILE: %s/\n", fullpath); + if (node->directory) + { + printf(" -- ERROR: Expected type file\n"); + g_nerrors++; + } + else + { + checkattributes(fullpath, node->mode, node->size); + checkfile(fullpath, node); + } + } + free(fullpath); + } + + closedir(dirp); +} + +/**************************************************************************** + * Name: checkdirectories + ****************************************************************************/ + +static void checkdirectories(struct node_s *entry) +{ + for (; entry; entry = entry->peer) + { + if (!entry->found ) + { + printf("ERROR: %s never found\n", entry->name); + g_nerrors++; + } + + if (entry->directory) + { + checkdirectories(entry->u.child); + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start + ****************************************************************************/ + +int user_start(int argc, char *argv[]) +{ + int ret; + + /* Create a RAM disk for the test */ + + ret = romdisk_register(CONFIG_EXAMPLES_ROMFS_RAMDEVNO, testdir_img, + NSECTORS(testdir_img_len), CONFIG_EXAMPLES_ROMFS_SECTORSIZE); + if (ret < 0) + { + printf("ERROR: Failed to create RAM disk\n"); + return 1; + } + + /* Mount the test file system */ + + printf("Mounting ROMFS filesystem at target=%s with source=%s\n", + CONFIG_EXAMPLES_ROMFS_MOUNTPOINT, MOUNT_DEVNAME); + + ret = mount(MOUNT_DEVNAME, CONFIG_EXAMPLES_ROMFS_MOUNTPOINT, "romfs", MS_RDONLY, NULL); + if (ret < 0) + { + printf("ERROR: Mount failed: %d\n", errno); + return 1; + } + + /* Perform the test */ + + connectem(); + readdirectories(CONFIG_EXAMPLES_ROMFS_MOUNTPOINT, &g_adir); + checkdirectories(&g_adir); + + if (g_nerrors) + { + printf("Finished with %d errors\n", g_nerrors); + return g_nerrors; + } + + printf("PASSED\n"); + return 0; +} diff --git a/apps/examples/romfs/romfs_testdir.h b/apps/examples/romfs/romfs_testdir.h new file mode 100644 index 000000000..53f93105c --- /dev/null +++ b/apps/examples/romfs/romfs_testdir.h @@ -0,0 +1,89 @@ +unsigned char testdir_img[] = { + 0x2d, 0x72, 0x6f, 0x6d, 0x31, 0x66, 0x73, 0x2d, 0x00, 0x00, 0x02, 0x60, + 0x27, 0x43, 0x4a, 0x8a, 0x52, 0x4f, 0x4d, 0x46, 0x53, 0x5f, 0x54, 0x65, + 0x73, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x49, + 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0xd1, 0xff, 0xff, 0x97, + 0x2e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x20, + 0x00, 0x00, 0x00, 0x00, 0xd1, 0xd1, 0xff, 0x80, 0x2e, 0x2e, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x93, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0b, + 0x93, 0x9b, 0x95, 0xf0, 0x6c, 0x64, 0x69, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x61, 0x64, 0x69, 0x72, + 0x2f, 0x73, 0x75, 0x62, 0x64, 0x69, 0x72, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x02, 0x19, 0x00, 0x00, 0x00, 0xb0, 0x00, 0x00, 0x00, 0x00, + 0x9e, 0x9b, 0x93, 0xc5, 0x61, 0x64, 0x69, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf2, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x15, 0x58, 0x47, 0x43, 0xf1, + 0x61, 0x6e, 0x6f, 0x74, 0x68, 0x65, 0x72, 0x66, 0x69, 0x6c, 0x65, 0x2e, + 0x74, 0x78, 0x74, 0x00, 0x54, 0x68, 0x69, 0x73, 0x20, 0x69, 0x73, 0x20, + 0x61, 0x6e, 0x6f, 0x74, 0x68, 0x65, 0x72, 0x20, 0x66, 0x69, 0x6c, 0x65, + 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, + 0xa1, 0xc5, 0x69, 0xd8, 0x79, 0x61, 0x66, 0x69, 0x6c, 0x65, 0x2e, 0x74, + 0x78, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x54, 0x68, 0x69, 0x73, + 0x20, 0x69, 0x73, 0x20, 0x79, 0x65, 0x74, 0x20, 0x61, 0x6e, 0x6f, 0x74, + 0x68, 0x65, 0x72, 0x20, 0x66, 0x69, 0x6c, 0x65, 0x0a, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x50, 0x00, 0x00, 0x00, 0x90, + 0x00, 0x00, 0x00, 0x00, 0xd1, 0xff, 0xfe, 0x20, 0x2e, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x70, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, + 0xd1, 0xd1, 0xfe, 0x70, 0x2e, 0x2e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, + 0x00, 0x00, 0x01, 0x90, 0x00, 0x00, 0x00, 0x00, 0x23, 0x18, 0x9c, 0x03, + 0x73, 0x75, 0x62, 0x64, 0x69, 0x72, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xd2, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x15, 0x3e, 0x3f, 0x06, 0xd8, 0x73, 0x75, 0x62, 0x64, + 0x69, 0x72, 0x66, 0x69, 0x6c, 0x65, 0x2e, 0x74, 0x78, 0x74, 0x00, 0x00, + 0x46, 0x69, 0x6c, 0x65, 0x20, 0x69, 0x6e, 0x20, 0x73, 0x75, 0x62, 0x64, + 0x69, 0x72, 0x65, 0x63, 0x74, 0x6f, 0x72, 0x79, 0x0a, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf0, + 0x00, 0x00, 0x01, 0x70, 0x00, 0x00, 0x00, 0x00, 0xd1, 0xff, 0xfc, 0xa0, + 0x2e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, + 0x00, 0x00, 0x00, 0x00, 0xd1, 0xd1, 0xff, 0x70, 0x2e, 0x2e, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x02, 0x30, 0x00, 0x00, 0x01, 0x90, 0x00, 0x00, 0x00, 0x00, + 0x32, 0x99, 0x92, 0xd4, 0x68, 0x66, 0x69, 0x6c, 0x65, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xc5, 0x6b, 0x22, 0x0b, + 0x61, 0x66, 0x69, 0x6c, 0x65, 0x2e, 0x74, 0x78, 0x74, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x54, 0x68, 0x69, 0x73, 0x20, 0x69, 0x73, 0x20, + 0x61, 0x20, 0x66, 0x69, 0x6c, 0x65, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00 +}; +unsigned int testdir_img_len = 1024; diff --git a/apps/examples/romfs/testdir.tar.gz b/apps/examples/romfs/testdir.tar.gz Binary files differnew file mode 100644 index 000000000..cd0e9c518 --- /dev/null +++ b/apps/examples/romfs/testdir.tar.gz diff --git a/apps/examples/romfs/testdir.txt b/apps/examples/romfs/testdir.txt new file mode 100644 index 000000000..e321d6ded --- /dev/null +++ b/apps/examples/romfs/testdir.txt @@ -0,0 +1,105 @@ +VOLUME HEADER: Name=ROMFS_Test +0000000: 2d72 6f6d 3166 732d 0000 0260 2743 4a8a -rom1fs-...`'CJ. +0000010: 524f 4d46 535f 5465 7374 0000 0000 0000 ROMFS_Test...... + +FILE HEADER 1: Name=. +0000020: 0000 0049 0000 0020 0000 0000 d1ff ff97 ...I... ........ +0000030: 2e00 0000 0000 0000 0000 0000 0000 0000 ................ + +FILE HEADER 2: Name=.. +0000040: 0000 0060 0000 0020 0000 0000 d1d1 ff80 ...`... ........ +0000050: 2e2e 0000 0000 0000 0000 0000 0000 0000 ................ + +FILE HEADER 3: Name=ldir +0000060: 0000 0093 0000 0000 0000 000b 939b 95f0 ................ +0000070: 6c64 6972 0000 0000 0000 0000 0000 0000 ldir............ + + FILE CONTENT: + 0000080: 6164 6972 2f73 7562 6469 7200 0000 0000 adir/subdir..... + +FILE HEADER 4: Name=adir +0000090: 0000 0219 0000 00b0 0000 0000 9e9b 93c5 ................ +00000a0: 6164 6972 0000 0000 0000 0000 0000 0000 adir............ + + FILE HEADER 4.1: Name=anotherfile.txt + 00000b0: 0000 00f2 0000 0000 0000 0015 5847 43f1 ............XGC. + 00000c0: 616e 6f74 6865 7266 696c 652e 7478 7400 anotherfile.txt. + + FILE CONTENT: + 00000d0: 5468 6973 2069 7320 616e 6f74 6865 7220 This is another + 00000e0: 6669 6c65 0a00 0000 0000 0000 0000 0000 file............ + + FILE HEADER 4.2: Name=yafile.txt + 00000f0: 0000 0132 0000 0000 0000 0019 a1c5 69d8 ...2..........i. + 0000100: 7961 6669 6c65 2e74 7874 0000 0000 0000 yafile.txt...... + + FILE CONTENT: + 0000110: 5468 6973 2069 7320 7965 7420 616e 6f74 This is yet anot + 0000120: 6865 7220 6669 6c65 0a00 0000 0000 0000 her file........ + + FILE HEADER 4.3: Name=. + 0000130: 0000 0150 0000 0090 0000 0000 d1ff fe20 ...P........... + 0000140: 2e00 0000 0000 0000 0000 0000 0000 0000 ................ + + FILE HEADER 4.4: Name=.. + 0000150: 0000 0170 0000 0020 0000 0000 d1d1 fe70 ...p... .......p + 0000160: 2e2e 0000 0000 0000 0000 0000 0000 0000 ................ + + FILE HEADER 4.5: Name=subdir + 0000170: 0000 0009 0000 0190 0000 0000 2318 9c03 ............#... + 0000180: 7375 6264 6972 0000 0000 0000 0000 0000 subdir.......... + + FILE HEADER 4.5.1: Name=subdirfile.txt + 0000190: 0000 01d2 0000 0000 0000 0015 3e3f 06d8 ............>?.. + 00001a0: 7375 6264 6972 6669 6c65 2e74 7874 0000 subdirfile.txt.. + + FILE CONTENT: + 00001b0: 4669 6c65 2069 6e20 7375 6264 6972 6563 File in subdirec + 00001c0: 746f 7279 0a00 0000 0000 0000 0000 0000 tory............ + + FILE HEADER 4.5.2: Name=. + 00001d0: 0000 01f0 0000 0170 0000 0000 d1ff fca0 .......p........ + 00001e0: 2e00 0000 0000 0000 0000 0000 0000 0000 ................ + + FILE HEADER 4.5.3: Name=.. + 00001f0: 0000 0000 0000 0090 0000 0000 d1d1 ff70 ...............p + 0000200: 2e2e 0000 0000 0000 0000 0000 0000 0000 ................ + +FILE HEADER 5: Name=hfile +0000210: 0000 0230 0000 0190 0000 0000 3299 92d4 ...0........2... +0000220: 6866 696c 6500 0000 0000 0000 0000 0000 hfile........... + +FILE HEADER 6: Name=afile.txt +0000230: 0000 0002 0000 0000 0000 000f c56b 220b .............k". +0000240: 6166 696c 652e 7478 7400 0000 0000 0000 afile.txt....... + + FILE CONTENT: + 0000250: 5468 6973 2069 7320 6120 6669 6c65 0a00 This is a file.. + +PADDING +0000260: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000270: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000280: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000290: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00002a0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00002b0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00002c0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00002d0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00002e0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00002f0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000300: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000310: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000320: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000330: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000340: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000350: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000360: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000370: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000380: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +0000390: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00003a0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00003b0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00003c0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00003d0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00003e0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ +00003f0: 0000 0000 0000 0000 0000 0000 0000 0000 ................ diff --git a/apps/examples/serloop/Kconfig b/apps/examples/serloop/Kconfig new file mode 100644 index 000000000..e52d35b3f --- /dev/null +++ b/apps/examples/serloop/Kconfig @@ -0,0 +1,13 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_SERLOOP + bool "Serial loopback example" + default n + ---help--- + Enable the serial loopback example + +if EXAMPLES_SERLOOP +endif diff --git a/apps/examples/serloop/Makefile b/apps/examples/serloop/Makefile new file mode 100644 index 000000000..67c805d46 --- /dev/null +++ b/apps/examples/serloop/Makefile @@ -0,0 +1,95 @@ +############################################################################ +# apps/examples/serloop/Makefile +# +# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# Mindlessly simple console loopack test + +ASRCS = +CSRCS = main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +# Register application +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep + diff --git a/apps/examples/serloop/main.c b/apps/examples/serloop/main.c new file mode 100644 index 000000000..3c635fe88 --- /dev/null +++ b/apps/examples/serloop/main.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * examples/serloop/main.c + * + * Copyright (C) 2008-2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <stdio.h> +#include <unistd.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int user_start(int argc, char *argv[]) +{ +#ifdef CONFIG_EXAMPLES_SERLOOP_BUFIO + int ch; + + for (;;) + { + ch = getchar(); + if (ch < 1) + { + ch = '!'; + } + else if ((ch < 0x20 || ch > 0x7e) && ch != '\n') + { + ch = '.'; + } + putchar(ch); + } +#else + uint8_t ch; + int ret; + + for (;;) + { + ret = read(0, &ch, 1); + if (ret < 1) + { + ch = '!'; + } + else if ((ch < 0x20 || ch > 0x7e) && ch != '\n') + { + ch = '.'; + } + ret = write(1, &ch, 1); + } +#endif + return 0; +} + diff --git a/apps/examples/watchdog/Kconfig b/apps/examples/watchdog/Kconfig new file mode 100644 index 000000000..18daf9327 --- /dev/null +++ b/apps/examples/watchdog/Kconfig @@ -0,0 +1,40 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config EXAMPLES_WATCHDOG + bool "Watchdog Timer example" + default n + ---help--- + Enable the watchdog timer example + +if EXAMPLES_WATCHDOG + +config EXAMPLES_WATCHDOG_DEVPATH + string "Watchdog device path" + default "/dev/watchdog0" + ---help--- + The path to the watchdog device. Default: /dev/watchdog0 + +config CONFIG_EXAMPLES_WATCHDOG_PINGTIME + int "Watchdog ping time" + default 5000 + ---help--- + Time in milliseconds that the example will ping the watchdog before letting the + watchdog expire. Default: 5000 milliseconds. + +config CONFIG_EXAMPLES_WATCHDOG_PINGDELAY + int "Watchdog ping delay" + default 500 + ---help--- + Time delay between pings in milliseconds. Default: 500 milliseconds. + +config EXAMPLES_WATCHDOG_TIMEOUT + int "Watchdog timeout" + default 2000 + ---help--- + The watchdog timeout value in milliseconds before the watchdog timer + expires. Default: 2000 milliseconds. + +endif diff --git a/apps/examples/watchdog/Makefile b/apps/examples/watchdog/Makefile new file mode 100644 index 000000000..d2739dbb0 --- /dev/null +++ b/apps/examples/watchdog/Makefile @@ -0,0 +1,103 @@ +############################################################################ +# apps/examples/watchdog/Makefile +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# Watchdog Timer Example. + +ASRCS = +CSRCS = watchdog_main.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Touchscreen built-in application info + +APPNAME = wdog +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Common build + +VPATH = + +all: .built +.PHONY: context clean depend distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/examples/watchdog/watchdog.h b/apps/examples/watchdog/watchdog.h new file mode 100644 index 000000000..dc2dea944 --- /dev/null +++ b/apps/examples/watchdog/watchdog.h @@ -0,0 +1,120 @@ +/**************************************************************************** + * examples/examples/watchdog/watchdog.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_EXAMPLES_WATCHDOG_WATCHDOG_H +#define __APPS_EXAMPLES_WATCHDOG_WATCHDOG_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* CONFIG_NSH_BUILTIN_APPS - Build the WATCHDOG test as an NSH built-in + * function. Default: Not built! The example can only be used as an NSH + * built-in application + * CONFIG_EXAMPLES_WATCHDOG_DEVPATH - The path to the Watchdog device. + * Default: /dev/watchdog0 + * CONFIG_EXAMPLES_WATCHDOG_PINGTIME - Time in milliseconds that the example + * will ping the watchdog before letting the watchdog expire. Default: 5000 + * milliseconds + * CONFIG_EXAMPLES_WATCHDOG_PINGDELAY - Time delay between pings in + * milliseconds. Default: 500 milliseconds. + * CONFIG_EXAMPLES_WATCHDOG_TIMEOUT - The watchdog timeout value in + * milliseconds before the watchdog timer expires. Default: 2000 + * milliseconds. + */ + +#ifndef CONFIG_WATCHDOG +# error "WATCHDOG device support is not enabled (CONFIG_WATCHDOG)" +#endif + +#ifndef CONFIG_NSH_BUILTIN_APPS +# warning "The WATCHDOG example only works as an NSH built-in application (CONFIG_NSH_BUILTIN_APPS)" +#endif + +#ifndef CONFIG_EXAMPLES_WATCHDOG_DEVPATH +# define CONFIG_EXAMPLES_WATCHDOG_DEVPATH "/dev/watchdog0" +#endif + +#ifndef CONFIG_EXAMPLES_WATCHDOG_PINGTIME +# define CONFIG_EXAMPLES_WATCHDOG_PINGTIME 5000 +#endif + +#ifndef CONFIG_EXAMPLES_WATCHDOG_PINGDELAY +# define CONFIG_EXAMPLES_WATCHDOG_PINGDELAY 500 +#endif + +#ifndef CONFIG_EXAMPLES_WATCHDOG_TIMEOUT +# define CONFIG_EXAMPLES_WATCHDOG_TIMEOUT 2000 +#endif + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#endif /* __APPS_EXAMPLES_WATCHDOG_WATCHDOG_H */ diff --git a/apps/examples/watchdog/watchdog_main.c b/apps/examples/watchdog/watchdog_main.c new file mode 100644 index 000000000..c1341e0fb --- /dev/null +++ b/apps/examples/watchdog/watchdog_main.c @@ -0,0 +1,357 @@ +/**************************************************************************** + * examples/watchdog/watchdog_main.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/ioctl.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/watchdog.h> + +#include "watchdog.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct wdog_example_s +{ + uint32_t pingtime; + uint32_t pingdelay; + uint32_t timeout; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: wdog_help + ****************************************************************************/ + +static void wdog_help(void) +{ + message("Usage: wdog [-h] [-d <pingtime] [-p <pingdelay>] [-t <timeout>]\n"); + message("\nInitialize the watchdog to the <timeout>. Start the watchdog\n"); + message("timer. Ping for the watchdog for <pingtime> seconds, then let it expire.\n"); + message("\nOptions include:\n"); + message(" [-d <pingtime>] = Selects the <delay> time in milliseconds. Default: %d\n", + CONFIG_EXAMPLES_WATCHDOG_PINGTIME); + message(" [-p <pingdelay] = Time delay between pings in milliseconds. Default: %d\n", + CONFIG_EXAMPLES_WATCHDOG_PINGDELAY); + message(" [-t timeout] = Time in milliseconds that the example will ping the watchdog\n"); + message(" before letting the watchdog expire. Default: %d\n", + CONFIG_EXAMPLES_WATCHDOG_TIMEOUT); + message(" [-h] = Shows this message and exits\n"); +} + +/**************************************************************************** + * Name: arg_string + ****************************************************************************/ + +static int arg_string(FAR char **arg, FAR char **value) +{ + FAR char *ptr = *arg; + + if (ptr[2] == '\0') + { + *value = arg[1]; + return 2; + } + else + { + *value = &ptr[2]; + return 1; + } +} + +/**************************************************************************** + * Name: arg_decimal + ****************************************************************************/ + +static int arg_decimal(FAR char **arg, FAR long *value) +{ + FAR char *string; + int ret; + + ret = arg_string(arg, &string); + *value = strtol(string, NULL, 10); + return ret; +} + +/**************************************************************************** + * Name: parse_args + ****************************************************************************/ + +static void parse_args(FAR struct wdog_example_s *wdog, int argc, FAR char **argv) +{ + FAR char *ptr; + long value; + int index; + int nargs; + + wdog->pingtime = CONFIG_EXAMPLES_WATCHDOG_PINGTIME; + wdog->pingdelay = CONFIG_EXAMPLES_WATCHDOG_PINGDELAY; + wdog->timeout = CONFIG_EXAMPLES_WATCHDOG_TIMEOUT; + + for (index = 1; index < argc; ) + { + ptr = argv[index]; + if (ptr[0] != '-') + { + message("Invalid options format: %s\n", ptr); + exit(EXIT_SUCCESS); + } + + switch (ptr[1]) + { + case 'd': + nargs = arg_decimal(&argv[index], &value); + if (value < 1) + { + message("Ping delay out of range: %ld\n", value); + exit(EXIT_FAILURE); + } + + wdog->pingdelay = (uint32_t)value; + index += nargs; + break; + + case 'p': + nargs = arg_decimal(&argv[index], &value); + if (value < 1 || value > 99) + { + message("Ping time out of range: %ld\n", value); + exit(EXIT_FAILURE); + } + + wdog->pingtime = (uint8_t)value; + index += nargs; + break; + + case 't': + nargs = arg_decimal(&argv[index], &value); + if (value < 1 || value > INT_MAX) + { + message("Duration out of range: %ld\n", value); + exit(EXIT_FAILURE); + } + + wdog->timeout = (int)value; + index += nargs; + break; + + case 'h': + wdog_help(); + exit(EXIT_SUCCESS); + + default: + message("Unsupported option: %s\n", ptr); + wdog_help(); + exit(EXIT_FAILURE); + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start/wdog_main + ****************************************************************************/ + +int wdog_main(int argc, char *argv[]) +{ + struct wdog_example_s wdog; +#ifdef CONFIG_DEBUG_WATCHDOG + struct watchdog_status_s status; +#endif + long elapsed; + int fd; + int ret; + + /* Parse the command line */ + + parse_args(&wdog, argc, argv); + + /* Initialization of the WATCHDOG hardware is performed by logic external to + * this test. + */ + + ret = up_wdginitialize(); + if (ret != OK) + { + message("wdog_main: up_wdginitialize failed: %d\n", ret); + goto errout; + } + + /* Open the watchdog device for reading */ + + fd = open(CONFIG_EXAMPLES_WATCHDOG_DEVPATH, O_RDONLY); + if (fd < 0) + { + message("wdog_main: open %s failed: %d\n", + CONFIG_EXAMPLES_WATCHDOG_DEVPATH, errno); + goto errout; + } + + /* Set the watchdog timeout */ + + ret = ioctl(fd, WDIOC_SETTIMEOUT, (unsigned long)wdog.timeout); + if (ret < 0) + { + message("wdog_main: ioctl(WDIOC_SETTIMEOUT) failed: %d\n", errno); + goto errout_with_dev; + } + + /* Then start the watchdog timer. */ + + ret = ioctl(fd, WDIOC_START, 0); + if (ret < 0) + { + message("wdog_main: ioctl(WDIOC_START) failed: %d\n", errno); + goto errout_with_dev; + } + + /* Then ping */ + + for (elapsed = 0; elapsed < wdog.pingtime; elapsed += wdog.pingdelay) + { + /* Sleep for the requested amount of time */ + + usleep(wdog.pingdelay * 1000); + + /* Show watchdog status. Only if debug is enabled because this + * could interfere with the timer. + */ + +#ifdef CONFIG_DEBUG_WATCHDOG + ret = ioctl(fd, WDIOC_GETSTATUS, (unsigned long)&status); + if (ret < 0) + { + message("wdog_main: ioctl(WDIOC_GETSTATUS) failed: %d\n", errno); + goto errout_with_dev; + } + message("wdog_main: flags=%08x timeout=%d timeleft=%d\n", + status.flags, status.timeout, status.timeleft); +#endif + + /* Then ping */ + + ret = ioctl(fd, WDIOC_KEEPALIVE, 0); + if (ret < 0) + { + message("wdog_main: ioctl(WDIOC_KEEPALIVE) failed: %d\n", errno); + goto errout_with_dev; + } + + message(" ping elapsed=%ld\n", elapsed); + msgflush(); + } + + /* Then stop pinging */ + + for (; ; elapsed += wdog.pingdelay) + { + /* Sleep for the requested amount of time */ + + usleep(wdog.pingdelay * 1000); + + /* Show watchdog status. Only if debug is enabled because this + * could interfere with the timer. + */ + +#ifdef CONFIG_DEBUG_WATCHDOG + ret = ioctl(fd, WDIOC_GETSTATUS, (unsigned long)&status); + if (ret < 0) + { + message("wdog_main: ioctl(WDIOC_GETSTATUS) failed: %d\n", errno); + goto errout_with_dev; + } + message("wdog_main: flags=%08x timeout=%d timeleft=%d\n", + status.flags, status.timeout, status.timeleft); +#endif + + message(" NO ping elapsed=%ld\n", elapsed); + msgflush(); + } + + /* We should not get here */ + + ret = ioctl(fd, WDIOC_STOP, 0); + if (ret < 0) + { + message("wdog_main: ioctl(WDIOC_STOP) failed: %d\n", errno); + goto errout_with_dev; + } + + close(fd); + msgflush(); + return OK; + +errout_with_dev: + close(fd); +errout: + msgflush(); + return ERROR; +} diff --git a/apps/fixedwing_control/Makefile b/apps/fixedwing_control/Makefile new file mode 100644 index 000000000..985708ae5 --- /dev/null +++ b/apps/fixedwing_control/Makefile @@ -0,0 +1,44 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# fixedwing_control Application +# + +APPNAME = fixedwing_control +PRIORITY = SCHED_PRIORITY_MAX - 1 +STACKSIZE = 12288 + +CSRCS = fixedwing_control.c + +include $(APPDIR)/mk/app.mk diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c new file mode 100644 index 000000000..d9d39f14d --- /dev/null +++ b/apps/fixedwing_control/fixedwing_control.c @@ -0,0 +1,938 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Ivan Ovinnikov <oivan@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file 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 <debug.h> +#include <math.h> +#include <termios.h> +#include <time.h> +#include <arch/board/up_hrt.h> +#include <arch/board/board.h> +#include <arch/board/drv_pwm_servo.h> +#include <nuttx/spi.h> +#include "../mix_and_link/mix_and_link.h" +#include "fixedwing_control.h" + +__EXPORT int fixedwing_control_main(int argc, char *argv[]); + +#define PID_DT 0.01f +#define PID_SCALER 0.1f +#define PID_DERIVMODE_CALC 0 +#define HIL_MODE 32 +#define RAD2DEG ((1.0/180.0)*M_PI) +#define AUTO -1000 +#define MANUAL 3000 +#define SERVO_MIN 1000 +#define SERVO_MAX 2000 + +pthread_t control_thread; +pthread_t nav_thread; +pthread_t servo_thread; + +/** + * Servo channels function enumerator used for + * the servo writing part + */ + +enum SERVO_CHANNELS_FUNCTION { + + AIL_1 = 0, + AIL_2 = 1, + MOT = 2, + ACT_1 = 3, + ACT_2 = 4, + ACT_3 = 5, + ACT_4 = 6, + ACT_5 = 7 +}; + +/** + * The plane_data structure. + * + * The plane data structure is the local storage of all the flight information of the aircraft + */ +typedef struct { + double lat; + double lon; + float alt; + float vx; + float vy; + float vz; + float yaw; + float hdg; + float pitch; + float roll; + float yawspeed; + float pitchspeed; + float rollspeed; + float rollb; /* body frame angles */ + float pitchb; + float yawb; + float p; + float q; + float r; /* body angular rates */ + + /* PID parameters*/ + + float Kp_att; + float Ki_att; + float Kd_att; + float Kp_pos; + float Ki_pos; + float Kd_pos; + float intmax_att; + float intmax_pos; + + /* Next waypoint*/ + + float wp_x; + float wp_y; + float wp_z; + + /* Setpoints */ + + float airspeed; + float groundspeed; + float roll_setpoint; + float pitch_setpoint; + float throttle_setpoint; + + /* Navigation mode*/ + int mode; + +} plane_data_t; + +/** + * The control_outputs structure. + * + * The control outputs structure contains the control outputs + * of the aircraft + */ +typedef struct { + float roll_ailerons; + float pitch_elevator; + float yaw_rudder; + float throttle; + // set the aux values to 0 per default + float aux1; + float aux2; + float aux3; + float aux4; + uint8_t mode; // HIL_ENABLED: 32 + uint8_t nav_mode; +} control_outputs_t; + +/** + * Generic PID algorithm with PD scaling + */ +static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax); + +/* + * Output calculations + */ + +static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed); +static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z); +static void calc_bodyframe_angles(float roll, float pitch, float yaw); +static float calc_bearing(void); +static float calc_roll_ail(void); +static float calc_pitch_elev(void); +static float calc_yaw_rudder(float hdg); +static float calc_throttle(void); +static float calc_gnd_speed(void); +static void get_parameters(void); +static float calc_roll_setpoint(void); +static float calc_pitch_setpoint(void); +static float calc_throttle_setpoint(void); +static float calc_wp_distance(void); +static void set_plane_mode(void); + +/* + * The control, navigation and servo loop threads + */ + +static void *control_loop(void *arg); +static void *nav_loop(void *arg); +static void *servo_loop(void *arg); + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +plane_data_t plane_data; +control_outputs_t control_outputs; +float scaler = 1; //M_PI; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/** + * + * Calculates the PID control output given an error. Incorporates PD scaling and low-pass filter for the derivative component. + * + * @param error the input error + * @param error_deriv the derivative of the input error + * @param dt time constant + * @param scaler PD scaler + * @param K_p P gain + * @param K_i I gain + * @param K_d D gain + * @param intmax Integration limit + * + * @return the PID control output + */ + +static float pid(float error, float error_deriv, uint16_t dt, float scale, float K_p, float K_i, float K_d, float intmax) +{ + float Kp = K_p; + float Ki = K_i; + float Kd = K_d; + float delta_time = dt; + float lerror; + float imax = intmax; + float integrator; + float derivative; + float lderiv; + int fCut = 20; /* anything above 20 Hz is considered noise - low pass filter for the derivative */ + float output = 0; + + output += error * Kp; + + if ((fabs(Kd) > 0) && (dt > 0)) { + + if (PID_DERIVMODE_CALC) { + derivative = (error - lerror) / delta_time; + + /* + * discrete low pass filter, cuts out the + * high frequency noise that can drive the controller crazy + */ + float RC = 1 / (2 * M_PI * fCut); + derivative = lderiv + + (delta_time / (RC + delta_time)) * (derivative - lderiv); + + /* update state */ + lerror = error; + lderiv = derivative; + + } else { + derivative = error_deriv; + } + + /* add in derivative component */ + output += Kd * derivative; + } + + //printf("PID derivative %i\n", (int)(1000*derivative)); + + /* scale the P and D components with the PD scaler */ + output *= scale; + + /* Compute integral component if time has elapsed */ + if ((fabs(Ki) > 0) && (dt > 0)) { + integrator += (error * Ki) * scaler * delta_time; + + if (integrator < -imax) { + integrator = -imax; + + } else if (integrator > imax) { + integrator = imax; + } + + output += integrator; + } + + //printf("PID Integrator %i\n", (int)(1000*integrator)); + + return output; +} + +/** + * Load parameters from global storage. + * + * Fetches the current parameters from the global parameter storage and writes them + * to the plane_data structure + */ + +static void get_parameters() +{ + plane_data.Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P]; + plane_data.Ki_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I]; + plane_data.Kd_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D]; + plane_data.Kp_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P]; + plane_data.Ki_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I]; + plane_data.Kd_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D]; + plane_data.intmax_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]; + plane_data.intmax_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]; + plane_data.airspeed = global_data_parameter_storage->pm.param_values[PARAM_AIRSPEED]; + plane_data.wp_x = global_data_parameter_storage->pm.param_values[PARAM_WPLON]; + plane_data.wp_y = global_data_parameter_storage->pm.param_values[PARAM_WPLAT]; + plane_data.wp_z = global_data_parameter_storage->pm.param_values[PARAM_WPALT]; + plane_data.mode = global_data_parameter_storage->pm.param_values[PARAM_FLIGHTMODE]; +} + +/** + * Calculates the body angular rates. + * + * Calculates the rates of the plane using inertia matrix and + * writes them to the plane_data structure + * + * @param roll + * @param pitch + * @param yaw + * @param rollspeed + * @param pitchspeed + * @param yawspeed + * + */ +static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ + plane_data.p = rollspeed - sinf(pitch) * yawspeed; + plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cos(pitch) * yawspeed; + plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cos(pitch) * yawspeed; +} + +/** + * + * Calculates the attitude angles in the body reference frame. + * + * Writes them to the plane data structure + * + * @param roll + * @param pitch + * @param yaw + */ + +static void calc_bodyframe_angles(float roll, float pitch, float yaw) +{ + plane_data.rollb = cosf(yaw) * cosf(pitch) * roll + + (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * pitch + + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * yaw; + plane_data.pitchb = -sinf(yaw) * cosf(pitch) * roll + + (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * pitch + + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * yaw; + plane_data.yawb = sinf(pitch) * roll - cosf(pitch) * sinf(roll) * pitch + cosf(pitch) * cosf(roll) * yaw; +} + +/** + * calc_rotation_matrix + * + * Calculates the rotation matrix + * + * @param roll + * @param pitch + * @param yaw + * @param x + * @param y + * @param z + * + */ + +static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z) +{ + plane_data.rollb = cosf(yaw) * cosf(pitch) * x + + (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * y + + (-cosf(yaw) * sinf(pitch) * cosf(roll) + sinf(yaw) * sinf(roll)) * z; + plane_data.pitchb = -sinf(yaw) * cosf(pitch) * x + + (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * y + + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * z; + plane_data.yawb = sinf(pitch) * x - cosf(pitch) * sinf(roll) * y + cosf(pitch) * cosf(roll) * z; +} + +/** + * calc_bearing + * + * Calculates the bearing error of the plane compared to the waypoints + * + * @return bearing Bearing error + * + */ + +static float calc_bearing() +{ + float bearing = 90 + atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)) * RAD2DEG; + + if (bearing < 0) + bearing += 360; + + return bearing; +} + +/** + * calc_roll_ail + * + * Calculates the roll ailerons control output + * + * @return Roll ailerons control output (-1,1) + */ + +static float calc_roll_ail() +{ + float ret = pid((plane_data.roll_setpoint - plane_data.roll) / scaler, plane_data.rollspeed, PID_DT, PID_SCALER, + plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); + + if (ret < -1) + return -1; + + if (ret > 1) + return 1; + + return ret; +} + +/** + * calc_pitch_elev + * + * Calculates the pitch elevators control output + * + * @return Pitch elevators control output (-1,1) + */ + +static float calc_pitch_elev() +{ + float ret = pid((plane_data.pitch_setpoint - plane_data.pitch) / scaler, plane_data.pitchspeed, PID_DT, PID_SCALER, + plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att); + + if (ret < -1) + return -1; + + if (ret > 1) + return 1; + + return ret; +} + +/** + * calc_yaw_rudder + * + * Calculates the yaw rudder control output (only if yaw rudder exists on the model) + * + * @return Yaw rudder control output (-1,1) + * + */ + +static float calc_yaw_rudder(float hdg) +{ + float ret = pid((plane_data.yaw / RAD2DEG - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER, + plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); + + if (ret < -1) + return -1; + + if (ret > 1) + return 1; + + return ret; +} + +/** + * calc_throttle + * + * Calculates the throttle control output + * + * @return Throttle control output (0,1) + */ + +static float calc_throttle() +{ + float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER, + plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); + + if (ret < 0.2) + return 0.2; + + if (ret > 1) + return 1; + + return ret; +} + +/** + * calc_gnd_speed + * + * Calculates the ground speed using the x and y components + * + * Input: none (operation on global data) + * + * Output: Ground speed of the plane + * + */ + +static float calc_gnd_speed() +{ + float gnd_speed = sqrtf(plane_data.vx * plane_data.vx + plane_data.vy * plane_data.vy); + return gnd_speed; +} + +/** + * calc_wp_distance + * + * Calculates the distance to the next waypoint + * + * @return the distance to the next waypoint + * + */ + +static float calc_wp_distance() +{ + return sqrtf((plane_data.lat - plane_data.wp_y) * (plane_data.lat - plane_data.wp_y) + + (plane_data.lon - plane_data.wp_x) * (plane_data.lon - plane_data.wp_x)); +} + +/** + * calc_roll_setpoint + * + * Calculates the offset angle for the roll plane, + * saturates at +- 35 deg. + * + * @return setpoint on which attitude control should stabilize while changing heading + * + */ + +static float calc_roll_setpoint() +{ + float setpoint = 0; + + if (plane_data.mode == TAKEOFF) { + setpoint = 0; + + } else { + setpoint = calc_bearing() - plane_data.yaw; + + if (setpoint < -35) + return -35; + + if (setpoint > 35) + return 35; + } + + return setpoint; +} + +/** + * calc_pitch_setpoint + * + * Calculates the offset angle for the pitch plane + * saturates at +- 35 deg. + * + * @return setpoint on which attitude control should stabilize while changing altitude + * + */ + +static float calc_pitch_setpoint() +{ + float setpoint = 0; + + if (plane_data.mode == TAKEOFF) { + setpoint = 35; + + } else { + setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()) * RAD2DEG; + + if (setpoint < -35) + return -35; + + if (setpoint > 35) + return 35; + } + + return setpoint; +} + +/** + * calc_throttle_setpoint + * + * Calculates the throttle setpoint for different flight modes + * + * @return throttle output setpoint + * + */ + +static float calc_throttle_setpoint() +{ + float setpoint = 0; + + // if TAKEOFF full throttle + if (plane_data.mode == TAKEOFF) { + setpoint = 60; + } + + // if CRUISE - parameter value + if (plane_data.mode == CRUISE) { + setpoint = plane_data.airspeed; + } + + // if LAND no throttle + if (plane_data.mode == LAND) { + setpoint = 0; + } + + return setpoint; +} + +/** + * set_plane_mode + * + * Sets the plane mode + * (TAKEOFF, CRUISE, LOITER or LAND) + * + */ + +static void set_plane_mode() +{ + if (plane_data.alt < 10) { + plane_data.mode = TAKEOFF; + + } else { + plane_data.mode = CRUISE; + // TODO: if reached waypoint and no further waypoint exists, go to LOITER mode + } + + // Debug override - don't need TAKEOFF mode for now + plane_data.mode = CRUISE; +} + +/* + * fixedwing_control_main + * + * @param argc number of arguments + * @param argv argument array + * + * @return 0 + * + */ + +int fixedwing_control_main(int argc, char *argv[]) +{ + /* print text */ + printf("Fixedwing control started\n"); + usleep(100000); + + /* default values for arguments */ + char *fixedwing_uart_name = "/dev/ttyS1"; + char *commandline_usage = "\tusage: fixedwing_control -d fixedwing-devicename\n"; + + /* read arguments */ + int i; + + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { + if (argc > i + 1) { + fixedwing_uart_name = argv[i + 1]; + + } else { + printf(commandline_usage); + return 0; + } + } + + /* Set up to publish fixed wing control messages */ + struct fixedwing_control_s control; + int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); + + /* Subscribe to global position, attitude and rc */ + struct vehicle_global_position_s global_pos; + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_attitude_s att; + int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + struct rc_channels_s rc; + int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + + /* Control constants */ + control_outputs.mode = HIL_MODE; + control_outputs.nav_mode = 0; + + /* Servo setup */ + + int fd; + servo_position_t data[PWM_SERVO_MAX_CHANNELS]; + + fd = open("/dev/pwm_servo", O_RDWR); + + if (fd < 0) { + printf("failed opening /dev/pwm_servo\n"); + } + + ioctl(fd, PWM_SERVO_ARM, 0); + + int16_t buffer_rc[3]; + int16_t buffer_servo[3]; + mixer_data_t mixer_buffer; + mixer_buffer.input = buffer_rc; + mixer_buffer.output = buffer_servo; + + mixer_conf_t mixers[3]; + + mixers[0].source = PITCH; + mixers[0].nr_actuators = 2; + mixers[0].dest[0] = AIL_1; + mixers[0].dest[1] = AIL_2; + mixers[0].dual_rate[0] = 1; + mixers[0].dual_rate[1] = 1; + + mixers[1].source = ROLL; + mixers[1].nr_actuators = 2; + mixers[1].dest[0] = AIL_1; + mixers[1].dest[1] = AIL_2; + mixers[1].dual_rate[0] = 1; + mixers[1].dual_rate[1] = -1; + + mixers[2].source = THROTTLE; + mixers[2].nr_actuators = 1; + mixers[2].dest[0] = MOT; + mixers[2].dual_rate[0] = 1; + + /* + * Main control, navigation and servo routine + */ + + while(1) + { + /* + * DATA Handling + * Fetch current flight data + */ + + /* get position, attitude and rc inputs */ + // XXX add error checking + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att); + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + + /* scaling factors are defined by the data from the APM Planner + * TODO: ifdef for other parameters (HIL/Real world switch) + */ + + /* position values*/ + plane_data.lat = global_pos.lat / 10000000; + plane_data.lon = global_pos.lon / 10000000; + plane_data.alt = global_pos.alt / 1000; + plane_data.vx = global_pos.vx / 100; + plane_data.vy = global_pos.vy / 100; + plane_data.vz = global_pos.vz / 100; + + /* attitude values*/ + plane_data.roll = att.roll; + plane_data.pitch = att.pitch; + plane_data.yaw = att.yaw; + plane_data.rollspeed = att.rollspeed; + plane_data.pitchspeed = att.pitchspeed; + plane_data.yawspeed = att.yawspeed; + + /* parameter values */ + get_parameters(); + + /* Attitude control part */ + +//#define MUTE +#ifndef MUTE + /******************************** DEBUG OUTPUT ************************************************************/ + + printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, + (int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos, + (int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed, + (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z); + +// printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint); +// printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint); +// printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint()); + +// printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed())); + +// printf("Current Altitude: %i\n\n", (int)plane_data.alt); + + printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ", + (int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw), + (int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed)); + +// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n", +// (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r)); + + printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n", + (int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator), + (int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle)); + + /************************************************************************************************************/ + +#endif + + /* + * Computation section + * + * The function calls to compute the required control values happen + * in this section. + */ + + /* Set plane mode */ + set_plane_mode(); + + /* Calculate the P,Q,R body rates of the aircraft */ + calc_body_angular_rates(plane_data.roll / RAD2DEG, plane_data.pitch / RAD2DEG, plane_data.yaw / RAD2DEG, + plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed); + + /* Calculate the body frame angles of the aircraft */ + //calc_bodyframe_angles(plane_data.roll/RAD2DEG,plane_data.pitch/RAD2DEG,plane_data.yaw/RAD2DEG); + + /* Calculate the output values */ + control_outputs.roll_ailerons = calc_roll_ail(); + control_outputs.pitch_elevator = calc_pitch_elev(); + //control_outputs.yaw_rudder = calc_yaw_rudder(); + control_outputs.throttle = calc_throttle(); + + if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode + + if (plane_data.mode == TAKEOFF) { + control.attitude_control_output[ROLL] = 0; + control.attitude_control_output[PITCH] = 5000; + control.attitude_control_output[THROTTLE] = 10000; + //global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder); + } + + if (plane_data.mode == CRUISE) { + control.attitude_control_output[ROLL] = (int16_t)(10000 * control_outputs.roll_ailerons); + control.attitude_control_output[PITCH] = (int16_t)(10000 * control_outputs.pitch_elevator); + control.attitude_control_output[THROTTLE] = (int16_t)(10000 * control_outputs.throttle); + //control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder); + } + + control.counter++; + control.timestamp = hrt_absolute_time(); + } + + /* Navigation part */ + + // Get GPS Waypoint + + // if(global_data_wait(&global_data_position_setpoint->access_conf) == 0) + // { + // plane_data.wp_x = global_data_position_setpoint->x; + // plane_data.wp_y = global_data_position_setpoint->y; + // plane_data.wp_z = global_data_position_setpoint->z; + // } + // global_data_unlock(&global_data_position_setpoint->access_conf); + + if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // AUTO mode + // AUTO/HYBRID switch + + if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) { + plane_data.roll_setpoint = calc_roll_setpoint(); + plane_data.pitch_setpoint = calc_pitch_setpoint(); + plane_data.throttle_setpoint = calc_throttle_setpoint(); + + } else { + plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200; + plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200; + plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200; + } + + //control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg); + + // 10 Hz loop + usleep(100000); + + } else { + control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale; + control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale; + control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale; + // since we don't have a yaw rudder + //control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale; + + control.counter++; + control.timestamp = hrt_absolute_time(); + } + + /* publish the control data */ + + orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control); + + /* Servo part */ + + buffer_rc[ROLL] = control.attitude_control_output[ROLL]; + buffer_rc[PITCH] = control.attitude_control_output[PITCH]; + buffer_rc[THROTTLE] = control.attitude_control_output[THROTTLE]; + + //mix_and_link(mixers, 3, 2, &mixer_buffer); + + // Scaling and saturation of servo outputs happens here + + data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] + + global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM]; + + if (data[AIL_1] > SERVO_MAX) + data[AIL_1] = SERVO_MAX; + + if (data[AIL_1] < SERVO_MIN) + data[AIL_1] = SERVO_MIN; + + data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] + + global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM]; + + if (data[AIL_2] > SERVO_MAX) + data[AIL_2] = SERVO_MAX; + + if (data[AIL_2] < SERVO_MIN) + data[AIL_2] = SERVO_MIN; + + data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE] + + global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM]; + + if (data[MOT] > SERVO_MAX) + data[MOT] = SERVO_MAX; + + if (data[MOT] < SERVO_MIN) + data[MOT] = SERVO_MIN; + + int result = write(fd, &data, sizeof(data)); + + if (result != sizeof(data)) { + printf("failed writing servo outputs\n"); + } + + /* 20Hz loop*/ + usleep(50000); + } + + return 0; +} diff --git a/apps/fixedwing_control/fixedwing_control.h b/apps/fixedwing_control/fixedwing_control.h new file mode 100644 index 000000000..6023e3967 --- /dev/null +++ b/apps/fixedwing_control/fixedwing_control.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Ivan Ovinnikov <oivan@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Definition of attitude controller + */ + +#ifndef FIXEDWING_CONTROL_H_ +#define FIXEDWING_CONTROL_H_ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <uORB/uORB.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/fixedwing_control.h> + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Internal definitions + ****************************************************************************/ + + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#endif /* FIXEDWING_CONTROL_H_ */ diff --git a/apps/fixedwing_control/fixedwing_control.zip b/apps/fixedwing_control/fixedwing_control.zip Binary files differnew file mode 100644 index 000000000..edaac33d5 --- /dev/null +++ b/apps/fixedwing_control/fixedwing_control.zip diff --git a/apps/fixedwing_control/pid.c b/apps/fixedwing_control/pid.c new file mode 100644 index 000000000..81b4deac1 --- /dev/null +++ b/apps/fixedwing_control/pid.c @@ -0,0 +1,118 @@ +/**************************************************************************** + * pid.c + * + * Copyright (C) 2012 Ivan Ovinnikov. All rights reserved. + * Authors: Ivan Ovinnikov <oivan@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "pid.h" +#include "fixedwing_control.h" + +/******************************************************************************* + * pid() + * + * Calculates the PID control output given an error + * + * Input: float error, uint16_t dt, float scaler, float K_p, float K_i, float K_d + * + * Output: PID control value + * + ******************************************************************************/ + +static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax) +{ + // PID parameters + + float Kp = K_p; + float Ki = K_i; + float Kd = K_d; + float delta_time = dt; // delta time + float lerror; // last error value + float imax = intmax; // max integral value + float integrator; + float derivative; + float lderiv; + int fCut = 20; // anything above 20 Hz is considered noise - low pass filter for the derivative + float output = 0; // the output of the PID controller + + output += error * Kp; + + if ((fabs(Kd) > 0) && (dt > 0)) { + + if (PID_DERIVMODE_CALC) { + derivative = (error - lerror) / delta_time; + + // discrete low pass filter, cuts out the + // high frequency noise that can drive the controller crazy + float RC = 1 / (2 * M_PI * fCut); + derivative = lderiv + + (delta_time / (RC + delta_time)) * (derivative - lderiv); + + // update state + lerror = error; + lderiv = derivative; + + } else { + derivative = error_deriv; + } + + // add in derivative component + output += Kd * derivative; + } + + printf("PID derivative %i\n", (int)(1000 * derivative)); + + // scale the P and D components + output *= scaler; + + // Compute integral component if time has elapsed + if ((fabs(Ki) > 0) && (dt > 0)) { + integrator += (error * Ki) * scaler * delta_time; + + if (integrator < -imax) { + integrator = -imax; + + } else if (integrator > imax) { + integrator = imax; + } + + output += integrator; + } + + printf("PID Integrator %i\n", (int)(1000 * integrator)); + + return output; +} + diff --git a/apps/fixedwing_control/pid.h b/apps/fixedwing_control/pid.h new file mode 100644 index 000000000..2f85c6c30 --- /dev/null +++ b/apps/fixedwing_control/pid.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * pid.h + * + * Copyright (C) 2012 Ivan Ovinnikov. All rights reserved. + * Authors: Ivan Ovinnikov <oivan@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + + +#ifndef PID_H_ +#define PID_H_ + +static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax); + +#endif /* PID_H_ */ diff --git a/apps/gps/.context b/apps/gps/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/gps/.context diff --git a/apps/gps/.gitignore b/apps/gps/.gitignore new file mode 100644 index 000000000..a02827195 --- /dev/null +++ b/apps/gps/.gitignore @@ -0,0 +1,3 @@ +include +mavlink-* +pymavlink-* diff --git a/apps/gps/Makefile b/apps/gps/Makefile new file mode 100644 index 000000000..7aaaf50cb --- /dev/null +++ b/apps/gps/Makefile @@ -0,0 +1,59 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the GPS receiver application +# + +APPNAME = gps +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +CSRCS = gps.c \ + ubx.c \ + mtk.c \ + nmea_helper.c \ + nmealib/context.c \ + nmealib/generate.c \ + nmealib/generator.c \ + nmealib/gmath.c \ + nmealib/info.c \ + nmealib/parse.c \ + nmealib/parser.c \ + nmealib/sentence.c \ + nmealib/time.c \ + nmealib/tok.c + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/gps/gps.c b/apps/gps/gps.c new file mode 100644 index 000000000..6d3ff0d6b --- /dev/null +++ b/apps/gps/gps.c @@ -0,0 +1,505 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file gps.c + * GPS app main loop. + */ + +#include "gps.h" + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include "nmealib/nmea/nmea.h" // the nmea library +#include "nmea_helper.h" //header files for interacting with the nmea library +#include "mtk.h" //header files for the custom protocol for the mediatek diydrones chip +#include "ubx.h" //header files for the ubx protocol +#include <termios.h> +#include <signal.h> +#include <pthread.h> +#include <sys/prctl.h> +#include <errno.h> +#include <signal.h> +#include <v1.0/common/mavlink.h> +#include <mavlink/mavlink_log.h> + +/** + * GPS module readout and publishing. + * + * This function reads the onboard gps and publishes the vehicle_gps_positon topic. + * + * @see vehicle_gps_position_s + * @ingroup apps + */ +__EXPORT int gps_main(int argc, char *argv[]); + +/**************************************************************************** + * Definitions + ****************************************************************************/ +#define IMPORTANT_GPS_BAUD_RATES_N 2 +#define RETRY_INTERVAL_SECONDS 10 + +//gps_bin_ubx_state_t * ubx_state; +bool gps_mode_try_all; +bool gps_baud_try_all; +bool gps_mode_success; +bool terminate_gps_thread; +bool gps_verbose; +int current_gps_speed; + +enum GPS_MODES { + GPS_MODE_START = 0, + GPS_MODE_UBX = 1, + GPS_MODE_MTK = 2, + GPS_MODE_NMEA = 3, + GPS_MODE_END = 4 +}; + + + +#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 + +/**************************************************************************** + * Private functions + ****************************************************************************/ +int open_port(char *port); + +void close_port(int fd); + +void setup_port(char *device, int speed, int *fd) +{ + + /* open port (baud rate is set in defconfig file) */ + *fd = open_port(device); + + if (*fd != -1) { + if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed); + + } else { + fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n"); + fflush(stdout); + } + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) { + fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state); + close(*fd); + } + + /* Set baud rate */ + cfsetispeed(&uart_config, speed); + cfsetospeed(&uart_config, speed); + + if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) { + fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device); + close(*fd); + } +} + + +/* + * Main function of gps app. + */ +int gps_main(int argc, char *argv[]) +{ + /* welcome message */ + printf("[gps] Initialized. Searching for GPS receiver..\n"); + + /* default values */ + const char *commandline_usage = "\tusage: %s -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n"; + char *device = "/dev/ttyS3"; + char mode[10]; + strcpy(mode, "all"); + int baudrate = -1; + gps_mode_try_all = false; + gps_baud_try_all = false; + gps_mode_success = true; + terminate_gps_thread = false; + bool retry = false; + gps_verbose = false; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + /* read arguments */ + int i; + + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set + printf(commandline_usage, argv[0]); + return 0; + } + + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set + if (argc > i + 1) { + device = argv[i + 1]; + + } else { + printf(commandline_usage, argv[0]); + return 0; + } + } + + if (strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--retry") == 0) { + if (argc > i + 1) { + retry = atoi(argv[i + 1]); + + } else { + printf(commandline_usage, argv[0]); + return 0; + } + } + + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) { + if (argc > i + 1) { + baudrate = atoi(argv[i + 1]); + + } else { + printf(commandline_usage, argv[0]); + return 0; + } + } + + if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) { + if (argc > i + 1) { + strcpy(mode, argv[i + 1]); + + } else { + printf(commandline_usage, argv[0]); + return 0; + } + } + + if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { + gps_verbose = true; + } + } + + /* + * In case a baud rate is set only this baud rate will be tried, + * otherwise a array of usual baud rates for gps receivers is used + */ + + +// printf("baudrate = %d\n",baudrate); + switch (baudrate) { + case -1: gps_baud_try_all = true; break; + + case 0: current_gps_speed = B0; break; + + case 50: current_gps_speed = B50; break; + + case 75: current_gps_speed = B75; break; + + case 110: current_gps_speed = B110; break; + + case 134: current_gps_speed = B134; break; + + case 150: current_gps_speed = B150; break; + + case 200: current_gps_speed = B200; break; + + case 300: current_gps_speed = B300; break; + + case 600: current_gps_speed = B600; break; + + case 1200: current_gps_speed = B1200; break; + + case 1800: current_gps_speed = B1800; break; + + case 2400: current_gps_speed = B2400; break; + + case 4800: current_gps_speed = B4800; break; + + case 9600: current_gps_speed = B9600; break; + + case 19200: current_gps_speed = B19200; break; + + case 38400: current_gps_speed = B38400; break; + + case 57600: current_gps_speed = B57600; break; + + case 115200: current_gps_speed = B115200; break; + + case 230400: current_gps_speed = B230400; break; + + case 460800: current_gps_speed = B460800; break; + + case 921600: current_gps_speed = B921600; break; + + default: + fprintf(stderr, "[gps] ERROR: Unsupported baudrate: %d\n", baudrate); + fflush(stdout); + return -EINVAL; + } + + + enum GPS_MODES current_gps_mode = GPS_MODE_UBX; + + if (strcmp(mode, "ubx") == 0) { + current_gps_mode = GPS_MODE_UBX; + + } else if (strcmp(mode, "mtkcustom") == 0) { + current_gps_mode = GPS_MODE_MTK; + + } else if (strcmp(mode, "nmea") == 0) { + current_gps_mode = GPS_MODE_NMEA; + + } else if (strcmp(mode, "all") == 0) { + gps_mode_try_all = true; + + } else { + fprintf(stderr, "\t[gps] Invalid mode argument\n"); + printf(commandline_usage); + return ERROR; + } + + + while (true) { + /* Infinite retries or break if retry == false */ + + /* Loop over all configurations of baud rate and protocol */ + for (i = 0; i < AUTO_DETECTION_COUNT; i++) { + if (gps_mode_try_all) { + current_gps_mode = autodetection_gpsmodes[i]; + + if (false == gps_baud_try_all && autodetection_baudrates[i] != current_gps_speed) //there is no need to try modes which are not configured to run with the selcted baud rate + continue; + } + + if (gps_baud_try_all) { + current_gps_speed = autodetection_baudrates[i]; + + if (false == gps_mode_try_all && autodetection_gpsmodes[i] != current_gps_mode) //there is no need to try baud rates which are not usual for the selected mode + continue; + } + + + /* + * The watchdog_thread will return and set gps_mode_success to false if no data can be parsed. + * if the gps was once running the wtachdog thread will not return but instead try to reconfigure the gps (depending on the mode/protocol) + */ + + if (current_gps_mode == GPS_MODE_UBX) { //TODO: make a small enum with all modes to avoid all the strcpy + + if (gps_verbose) printf("[gps] Trying UBX mode at %d baud\n", current_gps_speed); + + mavlink_log_info(mavlink_fd, "GPS: trying to connect to a ubx module"); + + int fd; + setup_port(device, current_gps_speed, &fd); + + /* start ubx thread and watchdog */ + pthread_t ubx_thread; + pthread_t ubx_watchdog_thread; + + pthread_mutex_t ubx_mutex_d; + ubx_mutex = &ubx_mutex_d; + pthread_mutex_init(ubx_mutex, NULL); + gps_bin_ubx_state_t ubx_state_d; + ubx_state = &ubx_state_d; + ubx_decode_init(); + + pthread_attr_t ubx_loop_attr; + pthread_attr_init(&ubx_loop_attr); + pthread_attr_setstacksize(&ubx_loop_attr, 3000); + pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&fd); + 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); + pthread_attr_setstacksize(&ubx_wd_attr, 1400); + int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&fd); + + if (pthread_create_res != 0) fprintf(stderr, "[gps] ERROR: could not create ubx watchdog thread, pthread_create =%d\n", pthread_create_res); + + /* wait for threads to complete */ + pthread_join(ubx_watchdog_thread, NULL); + + if (gps_mode_success == false) { + if (gps_verbose) printf("[gps] no success with UBX mode and %d baud\n", current_gps_speed); + + terminate_gps_thread = true; + pthread_join(ubx_thread, NULL); + + gps_mode_success = true; + terminate_gps_thread = false; + } + + close_port(fd); + } + + if (current_gps_mode == GPS_MODE_MTK) { + if (gps_verbose) printf("[gps] trying MTK binary mode at %d baud\n", current_gps_speed); + + mavlink_log_info(mavlink_fd, "[gps] trying to connect to a MTK module"); + + + int fd; + setup_port(device, current_gps_speed, &fd); + + /* start mtk thread and watchdog */ + pthread_t mtk_thread; + pthread_t mtk_watchdog_thread; + + pthread_mutex_t mtk_mutex_d; + mtk_mutex = &mtk_mutex_d; + pthread_mutex_init(mtk_mutex, NULL); + + + gps_bin_mtk_state_t mtk_state_d; + mtk_state = &mtk_state_d; + mtk_decode_init(); + + + pthread_attr_t mtk_loop_attr; + pthread_attr_init(&mtk_loop_attr); + pthread_attr_setstacksize(&mtk_loop_attr, 2048); + pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&fd); + sleep(2); + pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&fd); + + /* wait for threads to complete */ + pthread_join(mtk_watchdog_thread, (void *)&fd); + + if (gps_mode_success == false) { + if (gps_verbose) printf("[gps] No success with MTK binary mode and %d baud\n", current_gps_speed); + + terminate_gps_thread = true; + pthread_join(mtk_thread, NULL); + + // if(true == gps_mode_try_all) + // strcpy(mode, "nmea"); + + gps_mode_success = true; + terminate_gps_thread = false; + } + + close_port(fd); + + } + + if (current_gps_mode == GPS_MODE_NMEA) { + if (gps_verbose) printf("[gps] Trying NMEA mode at %d baud\n", current_gps_speed); + + mavlink_log_info(mavlink_fd, "[gps] trying to connect to a NMEA module"); + + int fd; + setup_port(device, current_gps_speed, &fd); + + /* start nmea thread and watchdog */ + pthread_t nmea_thread; + pthread_t nmea_watchdog_thread; + + pthread_mutex_t nmea_mutex_d; + nmea_mutex = &nmea_mutex_d; + pthread_mutex_init(nmea_mutex, NULL); + + gps_bin_nmea_state_t nmea_state_d; + nmea_state = &nmea_state_d; + + pthread_attr_t nmea_loop_attr; + pthread_attr_init(&nmea_loop_attr); + pthread_attr_setstacksize(&nmea_loop_attr, 4096); + pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&fd); + sleep(2); + pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&fd); + + /* wait for threads to complete */ + pthread_join(nmea_watchdog_thread, (void *)&fd); + + if (gps_mode_success == false) { + if (gps_verbose) printf("[gps] No success with NMEA mode and %d baud\r\n", current_gps_speed); + + terminate_gps_thread = true; + pthread_join(nmea_thread, NULL); + + gps_mode_success = true; + terminate_gps_thread = false; + } + + close_port(fd); + } + + /* if both, mode and baud is set by argument, we only need one loop*/ + if (gps_mode_try_all == false && gps_baud_try_all == false) + break; + } + + if (retry) { + printf("[gps] No configuration was successful, retrying in %d seconds \n", RETRY_INTERVAL_SECONDS); + mavlink_log_info(mavlink_fd, "[gps] No configuration was successful, retrying..."); + fflush(stdout); + + } else { + fprintf(stderr, "[gps] No configuration was successful, exiting... \n"); + fflush(stdout); + mavlink_log_info(mavlink_fd, "[gps] No configuration was successful, exiting..."); + break; + } + + sleep(RETRY_INTERVAL_SECONDS); + } + + close(mavlink_fd); + + return ERROR; +} + +int open_port(char *port) +{ + int fd; /**< File descriptor for the gps port */ + + /* Open serial port */ + fd = open(port, O_CREAT | O_RDWR | O_NOCTTY); /* O_RDWR - Read and write O_NOCTTY - Ignore special chars like CTRL-C */ + return (fd); +} + + +void close_port(int fd) +{ + /* Close serial port */ + close(fd); +} + + + diff --git a/apps/gps/gps.h b/apps/gps/gps.h new file mode 100644 index 000000000..313a3a2c2 --- /dev/null +++ b/apps/gps/gps.h @@ -0,0 +1,20 @@ +/* + * gps.h + * + * Created on: Mar 8, 2012 + * Author: thomasgubler + */ + +#ifndef GPS_H_ +#define GPS_H_ +/**************************************************************************** + * Included Files + ****************************************************************************/ + +int gps_fd; + +//extern gps_bin_ubx_state_t * ubx_state; + + + +#endif /* GPS_H_ */ diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c new file mode 100644 index 000000000..cef70601b --- /dev/null +++ b/apps/gps/mtk.c @@ -0,0 +1,423 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.ethz.ch> + * 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 + * 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 MTK custom binary (3DR) protocol implementation */ + +#include "mtk.h" +#include <nuttx/config.h> +#include <unistd.h> +#include <sys/prctl.h> +#include <pthread.h> +#include <poll.h> +#include <arch/board/up_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <mavlink/mavlink_log.h> + +#define MTK_HEALTH_SUCCESS_COUNTER_LIMIT 2 +#define MTK_HEALTH_FAIL_COUNTER_LIMIT 2 + +// XXX decrease this substantially, it should be only a few dozen bytes max. +#warning XXX trying 128 for now +#define MTK_BUFFER_SIZE 128 + +pthread_mutex_t *mtk_mutex; +gps_bin_mtk_state_t *mtk_state; +static struct vehicle_gps_position_s *mtk_gps; + +extern bool gps_mode_try_all; +extern bool gps_mode_success; +extern bool terminate_gps_thread; +extern bool gps_baud_try_all; +extern bool gps_verbose; +extern int current_gps_speed; + + +void mtk_decode_init(void) +{ + mtk_state->ck_a = 0; + mtk_state->ck_b = 0; + mtk_state->rx_count = 0; + mtk_state->decode_state = MTK_DECODE_UNINIT; + mtk_state->print_errors = false; +} + +void mtk_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b) +{ + *(ck_a) = *(ck_a) + b; + *(ck_b) = *(ck_b) + *(ck_a); +// printf("Checksum now: %d\n",*(ck_b)); +} + + + +int mtk_parse(uint8_t b, char *gps_rx_buffer) +{ +// printf("b=%x\n",b); + // Debug output to telemetry port + // PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, &b, 1); + if (mtk_state->decode_state == MTK_DECODE_UNINIT) { + + if (b == 0xd0) { + mtk_state->decode_state = MTK_DECODE_GOT_CK_A; + } + + } else if (mtk_state->decode_state == MTK_DECODE_GOT_CK_A) { + if (b == 0xdd) { + mtk_state->decode_state = MTK_DECODE_GOT_CK_B; + + } else { + // Second start symbol was wrong, reset state machine + mtk_decode_init(); + } + + } else if (mtk_state->decode_state == MTK_DECODE_GOT_CK_B) { + // Add to checksum + if (mtk_state->rx_count < 33) mtk_checksum(b, &(mtk_state->ck_a), &(mtk_state->ck_b)); + + // Fill packet buffer + gps_rx_buffer[mtk_state->rx_count] = b; + (mtk_state->rx_count)++; +// printf("Rx count: %d\n",mtk_state->rx_count); + uint8_t ret = 0; + + /* Packet size minus checksum */ + if (mtk_state->rx_count >= 35) { + gps_bin_mtk_packet_t *packet = (gps_bin_mtk_packet_t *) gps_rx_buffer; + + /* Check if checksum is valid */ + if (mtk_state->ck_a == packet->ck_a && mtk_state->ck_b == packet->ck_b) { + mtk_gps->lat = packet->latitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7 + mtk_gps->lon = packet->longitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7 + mtk_gps->alt = (int32_t)packet->msl_altitude * 10; // conversion from centimeters to millimeters, and from uint32_t to int16_t + mtk_gps->fix_type = packet->fix_type; + mtk_gps->eph = packet->hdop; + mtk_gps->epv = 65535; //unknown in mtk custom mode + mtk_gps->vel = packet->ground_speed; + mtk_gps->cog = 65535; //unknown in mtk custom mode + mtk_gps->satellites_visible = packet->satellites; + + /* convert time and date information to unix timestamp */ + struct tm timeinfo; //TODO: test this conversion + uint32_t timeinfo_conversion_temp; + + timeinfo.tm_mday = packet->date * 1e-4; + timeinfo_conversion_temp = packet->date - timeinfo.tm_mday * 1e4; + timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; + timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; + + timeinfo.tm_hour = packet->utc_time * 1e-7; + timeinfo_conversion_temp = packet->utc_time - timeinfo.tm_hour * 1e7; + timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; + timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; + timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; + timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; + time_t epoch = mktime(&timeinfo); + mtk_gps->timestamp = hrt_absolute_time(); + mtk_gps->time_gps_usec = epoch * 1e6; //TODO: test this + mtk_gps->time_gps_usec += timeinfo_conversion_temp * 1e3; + + mtk_gps->counter_pos_valid++; + + mtk_gps->timestamp = hrt_absolute_time(); + +// printf("%lu; %lu; %d.%d.%d %d:%d:%d:%d\n", packet->date, packet->utc_time,timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, timeinfo_conversion_temp); + + pthread_mutex_lock(mtk_mutex); +// printf("Write timestamp /n"); + mtk_state->last_message_timestamp = hrt_absolute_time(); + pthread_mutex_unlock(mtk_mutex); + + ret = 1; +// printf("found package\n"); + + } else { + if (gps_verbose) printf("[gps] Checksum invalid\r\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + mtk_decode_init(); +// printf("prepared for next state\n"); + return ret; + } + } + + return 0; // no valid packet found + +} + +int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if the thread should terminate +{ +// printf("in read_gps_mtk\n"); + uint8_t ret = 0; + + uint8_t c; + + int rx_count = 0; + int gpsRxOverflow = 0; + + struct pollfd fds; + fds.fd = fd; + fds.events = POLLIN; + + // 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); + if (rx_count >= buffer_size) { + // The buffer is already full and we haven't found a valid NMEA sentence. + // Flush the buffer and note the overflow event. + gpsRxOverflow++; + rx_count = 0; + mtk_decode_init(); + + if (gps_verbose) printf("[gps] Buffer full\r\n"); + + } else { + //gps_rx_buffer[rx_count] = c; + rx_count++; + + } + + int msg_read = mtk_parse(c, gps_rx_buffer); + + if (msg_read > 0) { + // printf("Found sequence\n"); + break; + } + + } else { + break; + } + + } else { + break; + } + + } + + return ret; +} + +int configure_gps_mtk(int fd) +{ + int success = 0; + size_t result_write; + result_write = write(fd, MEDIATEK_REFRESH_RATE_10HZ, strlen(MEDIATEK_REFRESH_RATE_10HZ)); + + if (result_write != strlen(MEDIATEK_REFRESH_RATE_10HZ)) { + printf("[gps] Set update speed to 10 Hz failed\r\n"); + success = 1; + + } else { + if (gps_verbose) printf("[gps] Attempted to set update speed to 10 Hz..\r\n"); + } + + //set custom mode + result_write = write(fd, MEDIATEK_CUSTOM_BINARY_MODE, strlen(MEDIATEK_CUSTOM_BINARY_MODE)); + + if (result_write != strlen(MEDIATEK_CUSTOM_BINARY_MODE)) { + //global_data_send_subsystem_info(&mtk_present); + printf("[gps] Set MTK custom mode failed\r\n"); + success = 1; + + } else { + //global_data_send_subsystem_info(&mtk_present_enabled); + if (gps_verbose) printf("[gps] Attempted to set MTK custom mode..\r\n"); + } + + return success; +} + +void *mtk_loop(void *arg) +{ +// int oldstate; +// pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, oldstate); +// +// printf("in mtk loop\n"); + /* Set thread name */ + prctl(PR_SET_NAME, "gps mtk read", getpid()); + + /* Retrieve file descriptor */ + int fd = *((int *)arg); + + /* Initialize gps stuff */ +// int buffer_size = 1000; +// char * gps_rx_buffer = malloc(buffer_size*sizeof(char)); + char gps_rx_buffer[MTK_BUFFER_SIZE]; + + /* set parameters for mtk custom */ + + if (configure_gps_mtk(fd) != 0) { + printf("[gps] Could not write serial port..\r\n"); + + /* Write shared variable sys_status */ + + //global_data_send_subsystem_info(&mtk_present); + + } else { + if (gps_verbose) printf("[gps] Configuration finished, awaiting GPS data..\r\n"); + + + /* Write shared variable sys_status */ + + //global_data_send_subsystem_info(&mtk_present_enabled); + } + + /* advertise GPS topic */ + struct vehicle_gps_position_s mtk_gps_d; + mtk_gps = &mtk_gps_d; + int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps); + + while (1) { + /* Parse a message from the gps receiver */ + if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) { + + /* publish new GPS position */ + orb_publish(ORB_ID(vehicle_gps_position), gps_handle, &mtk_gps); + + } else { + break; + } + + } + + return NULL; +} + +void *mtk_watchdog_loop(void *arg) +{ +// printf("in mtk watchdog loop\n"); + fflush(stdout); + + /* Set thread name */ + prctl(PR_SET_NAME, "gps mtk watchdog", getpid()); + + /* Retrieve file descriptor */ + int fd = *((int *)arg); + + bool mtk_healthy = false; + + uint8_t mtk_fail_count = 0; + uint8_t mtk_success_count = 0; + bool once_ok = false; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + + while (1) { + fflush(stdout); + + /* if we have no update for a long time reconfigure gps */ + pthread_mutex_lock(mtk_mutex); + uint64_t timestamp_now = hrt_absolute_time(); + bool all_okay = true; + + if (timestamp_now - mtk_state->last_message_timestamp > MTK_WATCHDOG_CRITICAL_TIME_MICROSECONDS) { + all_okay = false; + } + + pthread_mutex_unlock(mtk_mutex); + + if (!all_okay) { +// printf("mtk unhealthy\n"); + mtk_fail_count++; + /* gps error */ +// if (err_skip_counter == 0) +// { +// printf("[gps] GPS module not connected or not responding..\n"); +// err_skip_counter = 20; +// } +// err_skip_counter--; + +// printf("gps_mode_try_all =%u, mtk_fail_count=%u, mtk_healthy=%u, once_ok=%u\n", gps_mode_try_all, mtk_fail_count, mtk_healthy, once_ok); + + /* If we have too many failures and another mode or baud should be tried, exit... */ + if ((gps_mode_try_all == true || gps_baud_try_all == true) && (mtk_fail_count >= MTK_HEALTH_FAIL_COUNTER_LIMIT) && (mtk_healthy == false) && once_ok == false) { + if (gps_verbose) printf("[gps] Connection attempt failed, no MTK module found\r\n"); + + gps_mode_success = false; + break; + } + + if (mtk_healthy && mtk_fail_count >= MTK_HEALTH_FAIL_COUNTER_LIMIT) { + printf("[gps] ERROR: MTK GPS module stopped responding\r\n"); + // global_data_send_subsystem_info(&mtk_present_enabled); + mavlink_log_critical(mavlink_fd, "[gps] MTK module stopped responding\n"); + mtk_healthy = false; + mtk_success_count = 0; + + } + + /* trying to reconfigure the gps configuration */ + configure_gps_mtk(fd); + fflush(stdout); + + } else { + /* gps healthy */ + mtk_success_count++; + + if (!mtk_healthy && mtk_success_count >= MTK_HEALTH_SUCCESS_COUNTER_LIMIT) { + printf("[gps] MTK module found, status ok (baud=%d)\r\n", current_gps_speed); + /* MTK never has sat info */ + // XXX Check if lock makes sense here + mtk_gps->satellite_info_available = 0; + // global_data_send_subsystem_info(&mtk_present_enabled_healthy); + mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n"); + mtk_healthy = true; + mtk_fail_count = 0; + once_ok = true; + } + } + + usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS); + } + + close(mavlink_fd); + + return NULL; +} diff --git a/apps/gps/mtk.h b/apps/gps/mtk.h new file mode 100644 index 000000000..1c65a5865 --- /dev/null +++ b/apps/gps/mtk.h @@ -0,0 +1,98 @@ +/* + * mtk.h + * + * Created on: Mar 6, 2012 + * Author: thomasgubler + */ + +#ifndef MTK_H_ +#define MTK_H_ + +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <stdbool.h> +#include <unistd.h> +#include <pthread.h> + +//Definition for mtk custom mode +#define MEDIATEK_REFRESH_RATE_4HZ "$PMTK220,250*29\r\n" //refresh rate - 4Hz - 250 milliseconds +#define MEDIATEK_REFRESH_RATE_5HZ "$PMTK220,200*2C\r\n" +#define MEDIATEK_REFRESH_RATE_10HZ "$PMTK220,100*2F\r\n" //refresh rate - 10Hz - 100 milliseconds +#define MEDIATEK_FACTORY_RESET "$PMTK104*37\r\n" //clear current settings +#define MEDIATEK_CUSTOM_BINARY_MODE "$PGCMD,16,0,0,0,0,0*6A\r\n" +#define MEDIATEK_FULL_COLD_RESTART "$PMTK104*37\r\n" +//#define NMEA_GGA_ENABLE "$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*27\r\n" //Set GGA messages + +//definitions for watchdog +#define MTK_WATCHDOG_CRITICAL_TIME_MICROSECONDS 2000000 +#define MTK_WATCHDOG_WAIT_TIME_MICROSECONDS 800000 + + + + +// ************ +// the structure of the binary packet + +typedef struct { + uint8_t payload; ///< Number of payload bytes + int32_t latitude; ///< Latitude in degrees * 10^7 + int32_t longitude; ///< Longitude in degrees * 10^7 + uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 + uint32_t ground_speed; ///< FIXME SPEC UNCLEAR + int32_t heading; + uint8_t satellites; + uint8_t fix_type; + uint32_t date; + uint32_t utc_time; + uint16_t hdop; + uint8_t ck_a; + uint8_t ck_b; +} __attribute__((__packed__)) type_gps_bin_mtk_packet; + +typedef type_gps_bin_mtk_packet gps_bin_mtk_packet_t; + +enum MTK_DECODE_STATES { + MTK_DECODE_UNINIT = 0, + MTK_DECODE_GOT_CK_A = 1, + MTK_DECODE_GOT_CK_B = 2 +}; + +typedef struct { + union { + uint16_t ck; + struct { + uint8_t ck_a; + uint8_t ck_b; + }; + }; + uint8_t decode_state; +// bool new_data; +// uint8_t fix; + bool print_errors; + int16_t rx_count; + + uint64_t last_message_timestamp; +} __attribute__((__packed__)) type_gps_bin_mtk_state; + +typedef type_gps_bin_mtk_state gps_bin_mtk_state_t; + +extern pthread_mutex_t *mtk_mutex; +extern gps_bin_mtk_state_t *mtk_state; + +void mtk_decode_init(void); + +void mtk_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b); + +int mtk_parse(uint8_t b, char *gps_rx_buffer); + +int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size); + +int configure_gps_mtk(int fd); + +void *mtk_loop(void *arg); + +void *mtk_watchdog_loop(void *arg); + +#endif /* MTK_H_ */ diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c new file mode 100644 index 000000000..4b520d403 --- /dev/null +++ b/apps/gps/nmea_helper.c @@ -0,0 +1,335 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.ethz.ch> + * 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 + * 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 NMEA protocol implementation */ + +#include "nmea_helper.h" +#include <sys/prctl.h> +#include <poll.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <mavlink/mavlink_log.h> + +#define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2 +#define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2 + +#define NMEA_BUFFER_SIZE 1000 + +pthread_mutex_t *nmea_mutex; +gps_bin_nmea_state_t *nmea_state; +static struct vehicle_gps_position_s *nmea_gps; + +extern bool gps_mode_try_all; +extern bool gps_mode_success; +extern bool terminate_gps_thread; +extern bool gps_baud_try_all; +extern bool gps_verbose; +extern int current_gps_speed; + + +int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser) +{ + int ret = 1; + char c; + int start_flag = 0; + int found_cr = 0; + int rx_count = 0; + int gpsRxOverflow = 0; + + struct pollfd fds; + fds.fd = fd; + fds.events = POLLIN; + + // NMEA or SINGLE-SENTENCE GPS mode + + + 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 = 2; + break; + } + + if (poll(&fds, 1, 1000) > 0) { + if (read(fd, &c, 1) > 0) { + // detect start while acquiring stream + // printf("Char = %c\n", c); + if (!start_flag && (c == '$')) { + start_flag = 1; + found_cr = 0; + rx_count = 0; + + } else if (!start_flag) { // keep looking for start sign + continue; + } + + if (rx_count >= buffer_size) { + // The buffer is already full and we haven't found a valid NMEA sentence. + // Flush the buffer and note the overflow event. + gpsRxOverflow++; + start_flag = 0; + found_cr = 0; + rx_count = 0; + + if (gps_verbose) printf("\t[gps] Buffer full\n"); + + } else { + // store chars in buffer + gps_rx_buffer[rx_count] = c; + rx_count++; + } + + // look for carriage return CR + if (start_flag && c == 0x0d) { + found_cr = 1; + } + + // and then look for line feed LF + if (start_flag && found_cr && c == 0x0a) { + // parse one NMEA line, use buffer up to rx_count + if (nmea_parse(parser, gps_rx_buffer, rx_count, info) > 0) { + ret = 0; + } + + break; + } + + } else { + break; + } + + } else { + break; + } + } + + + + // As soon as one NMEA message has been parsed, we break out of the loop and end here + return(ret); +} + + +/** + * \brief Convert NDEG (NMEA degree) to fractional degree + */ +float ndeg2degree(float val) +{ + float deg = ((int)(val / 100)); + val = deg + (val - deg * 100) / 60; + return val; +} + +void *nmea_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "gps nmea read", getpid()); + + /* Retrieve file descriptor */ + int fd = *((int *)arg); + + /* Initialize gps stuff */ + nmeaINFO info_d; + nmeaINFO *info = &info_d; + char gps_rx_buffer[NMEA_BUFFER_SIZE]; + + /* gps parser (nmea) */ + nmeaPARSER parser; + nmea_parser_init(&parser); + nmea_zero_INFO(info); + + /* advertise GPS topic */ + struct vehicle_gps_position_s nmea_gps_d = {0}; + nmea_gps = &nmea_gps_d; + int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); + + while (1) { + /* Parse a message from the gps receiver */ + uint8_t read_res = read_gps_nmea(fd, gps_rx_buffer, NMEA_BUFFER_SIZE, info, &parser); + + if (0 == read_res) { + + /* convert data, ready it for publishing */ + + /* convert nmea utc time to usec */ + struct tm timeinfo; + timeinfo.tm_year = info->utc.year; + timeinfo.tm_mon = info->utc.mon; + timeinfo.tm_mday = info->utc.day; + timeinfo.tm_hour = info->utc.hour; + timeinfo.tm_min = info->utc.min; + timeinfo.tm_sec = info->utc.sec; + + time_t epoch = mktime(&timeinfo); + + // printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, info->utc.hsec); + + nmea_gps->timestamp = hrt_absolute_time(); + nmea_gps->time_gps_usec = epoch * 1e6 + info->utc.hsec * 1e4; + nmea_gps->fix_type = (uint8_t)info->fix; + nmea_gps->lat = (int32_t)ndeg2degree(info->lat) * 1e7; + nmea_gps->lon = (int32_t)ndeg2degree(info->lon) * 1e7; + nmea_gps->alt = (int32_t)(info->elv * 1e3); + nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling + nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling + nmea_gps->vel = (uint16_t)(info->speed * 1000 / 36); //*1000/3600*100 + nmea_gps->cog = 65535; + nmea_gps->satellites_visible = (uint8_t)info->satinfo.inview; + + int i = 0; + + /* Write info about individual satellites */ + for (i = 0; i < 12; i++) { + nmea_gps->satellite_prn[i] = (uint8_t)info->satinfo.sat[i].id; + nmea_gps->satellite_used[i] = (uint8_t)info->satinfo.sat[i].in_use; + nmea_gps->satellite_elevation[i] = (uint8_t)info->satinfo.sat[i].elv; + nmea_gps->satellite_azimuth[i] = (uint8_t)info->satinfo.sat[i].azimuth; + nmea_gps->satellite_snr[i] = (uint8_t)info->satinfo.sat[i].sig; + } + + if (nmea_gps->satellites_visible > 0) { + nmea_gps->satellite_info_available = 1; + + } else { + nmea_gps->satellite_info_available = 0; + } + + nmea_gps->counter_pos_valid++; + + nmea_gps->timestamp = hrt_absolute_time(); + nmea_gps->counter++; + + pthread_mutex_lock(nmea_mutex); + nmea_state->last_message_timestamp = hrt_absolute_time(); + pthread_mutex_unlock(nmea_mutex); + + /* publish new GPS position */ + orb_publish(ORB_ID(vehicle_gps_position), gps_handle, nmea_gps); + + } else if (read_res == 2) { //termination + /* de-advertise */ + close(gps_handle); + break; + } + + } + + //destroy gps parser + nmea_parser_destroy(&parser); + + return NULL; + +} + +void *nmea_watchdog_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "gps nmea watchdog", getpid()); + + bool nmea_healthy = false; + + uint8_t nmea_fail_count = 0; + uint8_t nmea_success_count = 0; + bool once_ok = false; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + while (1) { +// printf("nmea_watchdog_loop : while "); + /* if we have no update for a long time print warning (in nmea mode there is no reconfigure) */ + pthread_mutex_lock(nmea_mutex); + uint64_t timestamp_now = hrt_absolute_time(); + bool all_okay = true; + + if (timestamp_now - nmea_state->last_message_timestamp > NMEA_WATCHDOG_CRITICAL_TIME_MICROSECONDS) { + all_okay = false; + } + + pthread_mutex_unlock(nmea_mutex); + + if (!all_okay) { + /* gps error */ + nmea_fail_count++; +// printf("nmea error, nmea_fail_count=%u\n", nmea_fail_count); +// fflush(stdout); + + /* If we have too many failures and another mode or baud should be tried, exit... */ + if ((gps_mode_try_all == true || gps_baud_try_all == true) && (nmea_fail_count >= NMEA_HEALTH_FAIL_COUNTER_LIMIT) && (nmea_healthy == false) && once_ok == false) { + if (gps_verbose) printf("\t[gps] no NMEA module found\n"); + + gps_mode_success = false; + break; + } + + if (nmea_healthy && nmea_fail_count >= NMEA_HEALTH_FAIL_COUNTER_LIMIT) { + printf("\t[gps] ERROR: NMEA GPS module stopped responding\n"); + // global_data_send_subsystem_info(&nmea_present_enabled); + mavlink_log_critical(mavlink_fd, "[gps] NMEA module stopped responding\n"); + nmea_healthy = false; + nmea_success_count = 0; + + } + + + + fflush(stdout); + sleep(1); + + } else { + /* gps healthy */ +// printf("\t[gps] nmea success\n"); + nmea_success_count++; + + if (!nmea_healthy && nmea_success_count >= NMEA_HEALTH_SUCCESS_COUNTER_LIMIT) { + printf("[gps] NMEA module found, status ok (baud=%d)\r\n", current_gps_speed); + // global_data_send_subsystem_info(&nmea_present_enabled_healthy); + mavlink_log_info(mavlink_fd, "[gps] NMEA module found, status ok\n"); + nmea_healthy = true; + nmea_fail_count = 0; + once_ok = true; + } + + } + + usleep(NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS); + } + + close(mavlink_fd); + + return NULL; +} diff --git a/apps/gps/nmea_helper.h b/apps/gps/nmea_helper.h new file mode 100644 index 000000000..8fd630bcd --- /dev/null +++ b/apps/gps/nmea_helper.h @@ -0,0 +1,47 @@ +/* + * nmea_helper.h + * + * Created on: Mar 15, 2012 + * Author: thomasgubler + */ + +#ifndef NMEA_H_ +#define NMEA_H_ + +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include "nmealib/nmea/nmea.h" + + +//definitions for watchdog +#define NMEA_WATCHDOG_CRITICAL_TIME_MICROSECONDS 2000000 +#define NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS 800000 + +typedef struct { + uint64_t last_message_timestamp; +} __attribute__((__packed__)) type_gps_bin_nmea_state; + +typedef type_gps_bin_nmea_state gps_bin_nmea_state_t; + +extern gps_bin_nmea_state_t *nmea_state; +extern pthread_mutex_t *nmea_mutex; + + + +int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser); + +void *nmea_loop(void *arg); + +void *nmea_watchdog_loop(void *arg); + +/** + * \brief Convert NDEG (NMEA degree) to fractional degree + */ +float ndeg2degree(float val); + +void nmea_init(void); + + +#endif /* NMEA_H_ */ diff --git a/apps/gps/nmealib/LICENSE.TXT b/apps/gps/nmealib/LICENSE.TXT new file mode 100644 index 000000000..807db7916 --- /dev/null +++ b/apps/gps/nmealib/LICENSE.TXT @@ -0,0 +1,506 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms of the +ordinary General Public License). + + To apply these terms, attach the following notices to the library. It is +safest to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least the +"copyright" line and a pointer to where the full notice is found. + + <one line to give the library's name and a brief idea of what it does.> + Copyright (C) <year> <name of author> + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Also add information on how to contact you by electronic and paper mail. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James Random Hacker. + + <signature of Ty Coon>, 1 April 1990 + Ty Coon, President of Vice + +That's all there is to it! + + + + diff --git a/apps/gps/nmealib/README.TXT b/apps/gps/nmealib/README.TXT new file mode 100644 index 000000000..8ede6a036 --- /dev/null +++ b/apps/gps/nmealib/README.TXT @@ -0,0 +1,26 @@ +NMEA library see: http://nmea.sourceforge.net/ + +Disclaimer + +The National Marine Electronics Association (NMEA) has developed a specification that defines the interface between various pieces of marine electronic equipment. The standard permits marine electronics to send information to computers and to other marine equipment. +Most computer programs that provide real time position information understand and expect data to be in NMEA format. This data includes the complete PVT (position, velocity, time) solution computed by the GPS receiver. The idea of NMEA is to send a line of data called a sentence that is totally self contained and independent from other sentences. All NMEA sentences is sequences of ACSII symbols begins with a '$' and ends with a carriage return/line feed sequence and can be no longer than 80 characters of visible text (plus the line terminators). + +Introduction + +We present library in 'C' programming language for work with NMEA protocol. Small and easy to use. The library build on different compilers under different platforms (see below). The code was tested in real projects. Just download and try... + +Features + +- Analysis NMEA sentences and granting GPS data in C structures +- Generate NMEA sentences +- Supported sentences: GPGGA, GPGSA, GPGSV, GPRMC, GPVTG +- Multilevel architecture of algorithms +- Additional functions of geographical mathematics and work with navigation data + +Supported (tested) platforms + +- Microsoft Windows (MS Visual Studio 8.0, GCC) +- Windows Mobile, Windows CE (MS Visual Studio 8.0) +- UNIX (GCC) + +Licence: LGPL diff --git a/apps/gps/nmealib/context.c b/apps/gps/nmealib/context.c new file mode 100644 index 000000000..6ee2f5ad3 --- /dev/null +++ b/apps/gps/nmealib/context.c @@ -0,0 +1,67 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: context.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/context.h" + +#include <string.h> +#include <stdarg.h> +#include <stdio.h> + +nmeaPROPERTY * nmea_property(void) +{ + static nmeaPROPERTY prop = { + 0, 0, NMEA_DEF_PARSEBUFF + }; + + return ∝ +} + +void nmea_trace(const char *str, ...) +{ + int size; + va_list arg_list; + char buff[NMEA_DEF_PARSEBUFF]; + nmeaTraceFunc func = nmea_property()->trace_func; + + if(func) + { + va_start(arg_list, str); + size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); + va_end(arg_list); + + if(size > 0) + (*func)(&buff[0], size); + } +} + +void nmea_trace_buff(const char *buff, int buff_size) +{ + nmeaTraceFunc func = nmea_property()->trace_func; + if(func && buff_size) + (*func)(buff, buff_size); +} + +void nmea_error(const char *str, ...) +{ + int size; + va_list arg_list; + char buff[NMEA_DEF_PARSEBUFF]; + nmeaErrorFunc func = nmea_property()->error_func; + + if(func) + { + va_start(arg_list, str); + size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); + va_end(arg_list); + + if(size > 0) + (*func)(&buff[0], size); + } +} diff --git a/apps/gps/nmealib/generate.c b/apps/gps/nmealib/generate.c new file mode 100644 index 000000000..4c9389300 --- /dev/null +++ b/apps/gps/nmealib/generate.c @@ -0,0 +1,229 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generate.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/tok.h" +#include "nmea/sentence.h" +#include "nmea/generate.h" +#include "nmea/units.h" + +#include <string.h> +#include <stdlib.h> +#include <math.h> + +int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGGA,%02d%02d%02d.%02d,%07.4f,%C,%07.4f,%C,%1d,%02d,%03.1f,%03.1f,%C,%03.1f,%C,%03.1f,%04d", + pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec, + pack->lat, pack->ns, pack->lon, pack->ew, + pack->sig, pack->satinuse, pack->HDOP, pack->elv, pack->elv_units, + pack->diff, pack->diff_units, pack->dgps_age, pack->dgps_sid); +} + +int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGSA,%C,%1d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%02d,%03.1f,%03.1f,%03.1f", + pack->fix_mode, pack->fix_type, + pack->sat_prn[0], pack->sat_prn[1], pack->sat_prn[2], pack->sat_prn[3], pack->sat_prn[4], pack->sat_prn[5], + pack->sat_prn[6], pack->sat_prn[7], pack->sat_prn[8], pack->sat_prn[9], pack->sat_prn[10], pack->sat_prn[11], + pack->PDOP, pack->HDOP, pack->VDOP); +} + +int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPGSV,%1d,%1d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d," + "%02d,%02d,%03d,%02d", + pack->pack_count, pack->pack_index + 1, pack->sat_count, + pack->sat_data[0].id, pack->sat_data[0].elv, pack->sat_data[0].azimuth, pack->sat_data[0].sig, + pack->sat_data[1].id, pack->sat_data[1].elv, pack->sat_data[1].azimuth, pack->sat_data[1].sig, + pack->sat_data[2].id, pack->sat_data[2].elv, pack->sat_data[2].azimuth, pack->sat_data[2].sig, + pack->sat_data[3].id, pack->sat_data[3].elv, pack->sat_data[3].azimuth, pack->sat_data[3].sig); +} + +int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPRMC,%02d%02d%02d.%02d,%C,%07.4f,%C,%07.4f,%C,%03.1f,%03.1f,%02d%02d%02d,%03.1f,%C,%C", + pack->utc.hour, pack->utc.min, pack->utc.sec, pack->utc.hsec, + pack->status, pack->lat, pack->ns, pack->lon, pack->ew, + pack->speed, pack->direction, + pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100, + pack->declination, pack->declin_ew, pack->mode); +} + +int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack) +{ + return nmea_printf(buff, buff_sz, + "$GPVTG,%.1f,%C,%.1f,%C,%.1f,%C,%.1f,%C", + pack->dir, pack->dir_t, + pack->dec, pack->dec_m, + pack->spn, pack->spn_n, + pack->spk, pack->spk_k); +} + +void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack) +{ + nmea_zero_GPGGA(pack); + + pack->utc = info->utc; + pack->lat = fabs(info->lat); + pack->ns = ((info->lat > 0)?'N':'S'); + pack->lon = fabs(info->lon); + pack->ew = ((info->lon > 0)?'E':'W'); + pack->sig = info->sig; + pack->satinuse = info->satinfo.inuse; + pack->HDOP = info->HDOP; + pack->elv = info->elv; +} + +void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack) +{ + int it; + + nmea_zero_GPGSA(pack); + + pack->fix_type = info->fix; + pack->PDOP = info->PDOP; + pack->HDOP = info->HDOP; + pack->VDOP = info->VDOP; + + for(it = 0; it < NMEA_MAXSAT; ++it) + { + pack->sat_prn[it] = + ((info->satinfo.sat[it].in_use)?info->satinfo.sat[it].id:0); + } +} + +int nmea_gsv_npack(int sat_count) +{ + int pack_count = (int)ceil(((float)sat_count) / NMEA_SATINPACK); + + if(0 == pack_count) + pack_count = 1; + + return pack_count; +} + +void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx) +{ + int sit, pit; + + nmea_zero_GPGSV(pack); + + pack->sat_count = (info->satinfo.inview <= NMEA_MAXSAT)?info->satinfo.inview:NMEA_MAXSAT; + pack->pack_count = nmea_gsv_npack(pack->sat_count); + + if(pack->pack_count == 0) + pack->pack_count = 1; + + if(pack_idx >= pack->pack_count) + pack->pack_index = pack_idx % pack->pack_count; + else + pack->pack_index = pack_idx; + + for(pit = 0, sit = pack->pack_index * NMEA_SATINPACK; pit < NMEA_SATINPACK; ++pit, ++sit) + pack->sat_data[pit] = info->satinfo.sat[sit]; +} + +void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack) +{ + nmea_zero_GPRMC(pack); + + pack->utc = info->utc; + pack->status = ((info->sig > 0)?'A':'V'); + pack->lat = fabs(info->lat); + pack->ns = ((info->lat > 0)?'N':'S'); + pack->lon = fabs(info->lon); + pack->ew = ((info->lon > 0)?'E':'W'); + pack->speed = info->speed / NMEA_TUD_KNOTS; + pack->direction = info->direction; + pack->declination = info->declination; + pack->declin_ew = 'E'; + pack->mode = ((info->sig > 0)?'A':'N'); +} + +void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack) +{ + nmea_zero_GPVTG(pack); + + pack->dir = info->direction; + pack->dec = info->declination; + pack->spn = info->speed / NMEA_TUD_KNOTS; + pack->spk = info->speed; +} + +int nmea_generate( + char *buff, int buff_sz, + const nmeaINFO *info, + int generate_mask + ) +{ + int gen_count = 0, gsv_it, gsv_count; + int pack_mask = generate_mask; + + nmeaGPGGA gga; + nmeaGPGSA gsa; + nmeaGPGSV gsv; + nmeaGPRMC rmc; + nmeaGPVTG vtg; + + if(!buff) + return 0; + + while(pack_mask) + { + if(pack_mask & GPGGA) + { + nmea_info2GPGGA(info, &gga); + gen_count += nmea_gen_GPGGA(buff + gen_count, buff_sz - gen_count, &gga); + pack_mask &= ~GPGGA; + } + else if(pack_mask & GPGSA) + { + nmea_info2GPGSA(info, &gsa); + gen_count += nmea_gen_GPGSA(buff + gen_count, buff_sz - gen_count, &gsa); + pack_mask &= ~GPGSA; + } + else if(pack_mask & GPGSV) + { + gsv_count = nmea_gsv_npack(info->satinfo.inview); + for(gsv_it = 0; gsv_it < gsv_count && buff_sz - gen_count > 0; ++gsv_it) + { + nmea_info2GPGSV(info, &gsv, gsv_it); + gen_count += nmea_gen_GPGSV(buff + gen_count, buff_sz - gen_count, &gsv); + } + pack_mask &= ~GPGSV; + } + else if(pack_mask & GPRMC) + { + nmea_info2GPRMC(info, &rmc); + gen_count += nmea_gen_GPRMC(buff + gen_count, buff_sz - gen_count, &rmc); + pack_mask &= ~GPRMC; + } + else if(pack_mask & GPVTG) + { + nmea_info2GPVTG(info, &vtg); + gen_count += nmea_gen_GPVTG(buff + gen_count, buff_sz - gen_count, &vtg); + pack_mask &= ~GPVTG; + } + else + break; + + if(buff_sz - gen_count <= 0) + break; + } + + return gen_count; +} diff --git a/apps/gps/nmealib/generator.c b/apps/gps/nmealib/generator.c new file mode 100644 index 000000000..ce40b0f1a --- /dev/null +++ b/apps/gps/nmealib/generator.c @@ -0,0 +1,399 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generator.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/gmath.h" +#include "nmea/generate.h" +#include "nmea/generator.h" +#include "nmea/context.h" + +#include <string.h> +#include <stdlib.h> + +#if defined(NMEA_WIN) && defined(_MSC_VER) +# pragma warning(disable: 4100) /* unreferenced formal parameter */ +#endif + +float nmea_random(float min, float max) +{ + static float rand_max = MAX_RAND;//RAND_MAX; //nuttx defines MAX_RAND instead of RAND_MAX + float rand_val = rand(); + float bounds = max - min; + return min + (rand_val * bounds) / rand_max; +} + +/* + * low level + */ + +int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; int smask = info->smask; + nmeaGENERATOR *igen = gen; + + nmea_zero_INFO(info); + info->smask = smask; + + info->lat = NMEA_DEF_LAT; + info->lon = NMEA_DEF_LON; + + while(RetVal && igen) + { + if(igen->init_call) + RetVal = (*igen->init_call)(igen, info); + igen = igen->next; + } + + return RetVal; +} + +int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; + + if(gen->loop_call) + RetVal = (*gen->loop_call)(gen, info); + + if(RetVal && gen->next) + RetVal = nmea_gen_loop(gen->next, info); + + return RetVal; +} + +int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int RetVal = 1; + + if(gen->reset_call) + RetVal = (*gen->reset_call)(gen, info); + + return RetVal; +} + +void nmea_gen_destroy(nmeaGENERATOR *gen) +{ + if(gen->next) + { + nmea_gen_destroy(gen->next); + gen->next = 0; + } + + if(gen->destroy_call) + (*gen->destroy_call)(gen); + + free(gen); +} + +void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen) +{ + if(to->next) + nmea_gen_add(to->next, gen); + else + to->next = gen; +} + +int nmea_generate_from( + char *buff, int buff_sz, + nmeaINFO *info, + nmeaGENERATOR *gen, + int generate_mask + ) +{ + int retval; + + if(0 != (retval = nmea_gen_loop(gen, info))) + retval = nmea_generate(buff, buff_sz, info, generate_mask); + + return retval; +} + +/* + * NOISE generator + */ + +int nmea_igen_noise_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + return 1; +} + +int nmea_igen_noise_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + int in_use; + + info->sig = (int)nmea_random(1, 3); + info->PDOP = nmea_random(0, 9); + info->HDOP = nmea_random(0, 9); + info->VDOP = nmea_random(0, 9); + info->fix = (int)nmea_random(2, 3); + info->lat = nmea_random(0, 100); + info->lon = nmea_random(0, 100); + info->speed = nmea_random(0, 100); + info->direction = nmea_random(0, 360); + info->declination = nmea_random(0, 360); + info->elv = (int)nmea_random(-100, 100); + + info->satinfo.inuse = 0; + info->satinfo.inview = 0; + + for(it = 0; it < 12; ++it) + { + info->satinfo.sat[it].id = it; + info->satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3); + info->satinfo.sat[it].elv = (int)nmea_random(0, 90); + info->satinfo.sat[it].azimuth = (int)nmea_random(0, 359); + info->satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40)); + + if(in_use) + info->satinfo.inuse++; + if(info->satinfo.sat[it].sig > 0) + info->satinfo.inview++; + } + + return 1; +} + +int nmea_igen_noise_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + return 1; +} + +/* + * STATIC generator + */ + +int nmea_igen_static_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + nmea_time_now(&info->utc); + return 1; +}; + +int nmea_igen_static_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->satinfo.inuse = 4; + info->satinfo.inview = 4; + + info->satinfo.sat[0].id = 1; + info->satinfo.sat[0].in_use = 1; + info->satinfo.sat[0].elv = 50; + info->satinfo.sat[0].azimuth = 0; + info->satinfo.sat[0].sig = 99; + + info->satinfo.sat[1].id = 2; + info->satinfo.sat[1].in_use = 1; + info->satinfo.sat[1].elv = 50; + info->satinfo.sat[1].azimuth = 90; + info->satinfo.sat[1].sig = 99; + + info->satinfo.sat[2].id = 3; + info->satinfo.sat[2].in_use = 1; + info->satinfo.sat[2].elv = 50; + info->satinfo.sat[2].azimuth = 180; + info->satinfo.sat[2].sig = 99; + + info->satinfo.sat[3].id = 4; + info->satinfo.sat[3].in_use = 1; + info->satinfo.sat[3].elv = 50; + info->satinfo.sat[3].azimuth = 270; + info->satinfo.sat[3].sig = 99; + + return 1; +} + +int nmea_igen_static_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + + nmea_igen_static_reset(gen, info); + + return 1; +} + +/* + * SAT_ROTATE generator + */ + +int nmea_igen_rotate_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + int count = info->satinfo.inview; + float deg = 360 / (count?count:1); + float srt = (count?(info->satinfo.sat[0].azimuth):0) + 5; + + nmea_time_now(&info->utc); + + for(it = 0; it < count; ++it) + { + info->satinfo.sat[it].azimuth = + (int)((srt >= 360)?srt - 360:srt); + srt += deg; + } + + return 1; +}; + +int nmea_igen_rotate_reset(nmeaGENERATOR *gen, nmeaINFO *info) +{ + int it; + float deg = 360 / 8; + float srt = 0; + + info->satinfo.inuse = 8; + info->satinfo.inview = 8; + + for(it = 0; it < info->satinfo.inview; ++it) + { + info->satinfo.sat[it].id = it + 1; + info->satinfo.sat[it].in_use = 1; + info->satinfo.sat[it].elv = 5; + info->satinfo.sat[it].azimuth = (int)srt; + info->satinfo.sat[it].sig = 80; + srt += deg; + } + + return 1; +} + +int nmea_igen_rotate_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + + nmea_igen_rotate_reset(gen, info); + + return 1; +} + +/* + * POS_RANDMOVE generator + */ + +int nmea_igen_pos_rmove_init(nmeaGENERATOR *gen, nmeaINFO *info) +{ + info->sig = 3; + info->fix = 3; + info->direction = info->declination = 0; + info->speed = 20; + return 1; +} + +int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen, nmeaINFO *info) +{ + nmeaPOS crd; + + info->direction += nmea_random(-10, 10); + info->speed += nmea_random(-2, 3); + + if(info->direction < 0) + info->direction = 359 + info->direction; + if(info->direction > 359) + info->direction -= 359; + + if(info->speed > 40) + info->speed = 40; + if(info->speed < 1) + info->speed = 1; + + nmea_info2pos(info, &crd); + nmea_move_horz(&crd, &crd, info->direction, info->speed / 3600); + nmea_pos2info(&crd, info); + + info->declination = info->direction; + + return 1; +}; + +int nmea_igen_pos_rmove_destroy(nmeaGENERATOR *gen) +{ + return 1; +}; + +/* + * generator create + */ + +nmeaGENERATOR * __nmea_create_generator(int type, nmeaINFO *info) +{ + nmeaGENERATOR *gen = 0; + + switch(type) + { + case NMEA_GEN_NOISE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_noise_init; + gen->loop_call = &nmea_igen_noise_loop; + gen->reset_call = &nmea_igen_noise_reset; + } + break; + case NMEA_GEN_STATIC: + case NMEA_GEN_SAT_STATIC: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_static_init; + gen->loop_call = &nmea_igen_static_loop; + gen->reset_call = &nmea_igen_static_reset; + } + break; + case NMEA_GEN_SAT_ROTATE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_rotate_init; + gen->loop_call = &nmea_igen_rotate_loop; + gen->reset_call = &nmea_igen_rotate_reset; + } + break; + case NMEA_GEN_POS_RANDMOVE: + if(0 == (gen = malloc(sizeof(nmeaGENERATOR)))) + nmea_error("Insufficient memory!"); + else + { + memset(gen, 0, sizeof(nmeaGENERATOR)); + gen->init_call = &nmea_igen_pos_rmove_init; + gen->loop_call = &nmea_igen_pos_rmove_loop; + gen->destroy_call = &nmea_igen_pos_rmove_destroy; + } + break; + case NMEA_GEN_ROTATE: + gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info); + nmea_gen_add(gen, __nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info)); + break; + }; + + return gen; +} + +nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info) +{ + nmeaGENERATOR *gen = __nmea_create_generator(type, info); + + if(gen) + nmea_gen_init(gen, info); + + return gen; +} + +void nmea_destroy_generator(nmeaGENERATOR *gen) +{ + nmea_gen_destroy(gen); +} + +#if defined(NMEA_WIN) && defined(_MSC_VER) +# pragma warning(default: 4100) +#endif diff --git a/apps/gps/nmealib/gmath.c b/apps/gps/nmealib/gmath.c new file mode 100644 index 000000000..327b982ef --- /dev/null +++ b/apps/gps/nmealib/gmath.c @@ -0,0 +1,376 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: gmath.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file gmath.h */ +#include "nmea/gmath.h" + +#include <math.h> +#include <float.h> + +/** + * \fn nmea_degree2radian + * \brief Convert degree to radian + */ +float nmea_degree2radian(float val) +{ return (val * NMEA_PI180); } + +/** + * \fn nmea_radian2degree + * \brief Convert radian to degree + */ +float nmea_radian2degree(float val) +{ return (val / NMEA_PI180); } + +/** + * \brief Convert NDEG (NMEA degree) to fractional degree + */ +float nmea_ndeg2degree(float val) +{ + float deg = ((int)(val / 100)); + val = deg + (val - deg * 100) / 60; + return val; +} + +/** + * \brief Convert fractional degree to NDEG (NMEA degree) + */ +float nmea_degree2ndeg(float val) +{ + float int_part; + float fra_part; + fra_part = modf(val, &int_part); + val = int_part * 100 + fra_part * 60; + return val; +} + +/** + * \fn nmea_ndeg2radian + * \brief Convert NDEG (NMEA degree) to radian + */ +float nmea_ndeg2radian(float val) +{ return nmea_degree2radian(nmea_ndeg2degree(val)); } + +/** + * \fn nmea_radian2ndeg + * \brief Convert radian to NDEG (NMEA degree) + */ +float nmea_radian2ndeg(float val) +{ return nmea_degree2ndeg(nmea_radian2degree(val)); } + +/** + * \brief Calculate PDOP (Position Dilution Of Precision) factor + */ +float nmea_calc_pdop(float hdop, float vdop) +{ + return sqrt(pow(hdop, 2) + pow(vdop, 2)); +} + +float nmea_dop2meters(float dop) +{ return (dop * NMEA_DOP_FACTOR); } + +float nmea_meters2dop(float meters) +{ return (meters / NMEA_DOP_FACTOR); } + +/** + * \brief Calculate distance between two points + * \return Distance in meters + */ +float nmea_distance( + const nmeaPOS *from_pos, /**< From position in radians */ + const nmeaPOS *to_pos /**< To position in radians */ + ) +{ + float dist = ((float)NMEA_EARTHRADIUS_M) * acos( + sin(to_pos->lat) * sin(from_pos->lat) + + cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon) + ); + return dist; +} + +/** + * \brief Calculate distance between two points + * This function uses an algorithm for an oblate spheroid earth model. + * The algorithm is described here: + * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf + * \return Distance in meters + */ +float nmea_distance_ellipsoid( + const nmeaPOS *from_pos, /**< From position in radians */ + const nmeaPOS *to_pos, /**< To position in radians */ + float *from_azimuth, /**< (O) azimuth at "from" position in radians */ + float *to_azimuth /**< (O) azimuth at "to" position in radians */ + ) +{ + /* All variables */ + float f, a, b, sqr_a, sqr_b; + float L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; + float sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; + int remaining_steps; + float sqr_u, A, B, delta_sigma; + + /* Check input */ + //NMEA_ASSERT(from_pos != 0); + //NMEA_ASSERT(to_pos != 0); + + if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon)) + { /* Identical points */ + if ( from_azimuth != 0 ) + *from_azimuth = 0; + if ( to_azimuth != 0 ) + *to_azimuth = 0; + return 0; + } /* Identical points */ + + /* Earth geometry */ + f = NMEA_EARTH_FLATTENING; + a = NMEA_EARTH_SEMIMAJORAXIS_M; + b = (1 - f) * a; + sqr_a = a * a; + sqr_b = b * b; + + /* Calculation */ + L = to_pos->lon - from_pos->lon; + phi1 = from_pos->lat; + phi2 = to_pos->lat; + U1 = atan((1 - f) * tan(phi1)); + U2 = atan((1 - f) * tan(phi2)); + sin_U1 = sin(U1); + sin_U2 = sin(U2); + cos_U1 = cos(U1); + cos_U2 = cos(U2); + + /* Initialize iteration */ + sigma = 0; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + cos_2_sigmam = 0; + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sqr_cos_alpha = 0; + lambda = L; + sin_lambda = sin(lambda); + cos_lambda = cos(lambda); + delta_lambda = lambda; + remaining_steps = 20; + + while ((delta_lambda > 1e-12) && (remaining_steps > 0)) + { /* Iterate */ + /* Variables */ + float tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; + + /* Calculation */ + tmp1 = cos_U2 * sin_lambda; + tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; + sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); + cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; + tan_sigma = sin_sigma / cos_sigma; + sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; + cos_alpha = cos(asin(sin_alpha)); + sqr_cos_alpha = cos_alpha * cos_alpha; + cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); + lambda_prev = lambda; + sigma = asin(sin_sigma); + lambda = L + + (1 - C) * f * sin_alpha + * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); + delta_lambda = lambda_prev - lambda; + if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; + sin_lambda = sin(lambda); + cos_lambda = cos(lambda); + remaining_steps--; + } /* Iterate */ + + /* More calculation */ + sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; + A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); + B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); + delta_sigma = B * sin_sigma * ( + cos_2_sigmam + B / 4 * ( + cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - + B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) + )); + + /* Calculate result */ + if ( from_azimuth != 0 ) + { + float tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); + *from_azimuth = atan(tan_alpha_1); + } + if ( to_azimuth != 0 ) + { + float tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); + *to_azimuth = atan(tan_alpha_2); + } + + return b * A * (sigma - delta_sigma); +} + +/** + * \brief Horizontal move of point position + */ +int nmea_move_horz( + const nmeaPOS *start_pos, /**< Start position in radians */ + nmeaPOS *end_pos, /**< Result position in radians */ + float azimuth, /**< Azimuth (degree) [0, 359] */ + float distance /**< Distance (km) */ + ) +{ + nmeaPOS p1 = *start_pos; + int RetVal = 1; + + distance /= NMEA_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ + azimuth = nmea_degree2radian(azimuth); + + end_pos->lat = asin( + sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); + end_pos->lon = p1.lon + atan2( + sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat)); + + if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) + { + end_pos->lat = 0; end_pos->lon = 0; + RetVal = 0; + } + + return RetVal; +} + +/** + * \brief Horizontal move of point position + * This function uses an algorithm for an oblate spheroid earth model. + * The algorithm is described here: + * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf + */ +int nmea_move_horz_ellipsoid( + const nmeaPOS *start_pos, /**< Start position in radians */ + nmeaPOS *end_pos, /**< (O) Result position in radians */ + float azimuth, /**< Azimuth in radians */ + float distance, /**< Distance (km) */ + float *end_azimuth /**< (O) Azimuth at end position in radians */ + ) +{ + /* Variables */ + float f, a, b, sqr_a, sqr_b; + float phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1; + float tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B; + float sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma; + int remaining_steps; + float tmp1, phi2, lambda, C, L; + + /* Check input */ + //NMEA_ASSERT(start_pos != 0); + //NMEA_ASSERT(end_pos != 0); + + if (fabs(distance) < 1e-12) + { /* No move */ + *end_pos = *start_pos; + if ( end_azimuth != 0 ) *end_azimuth = azimuth; + return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); + } /* No move */ + + /* Earth geometry */ + f = NMEA_EARTH_FLATTENING; + a = NMEA_EARTH_SEMIMAJORAXIS_M; + b = (1 - f) * a; + sqr_a = a * a; + sqr_b = b * b; + + /* Calculation */ + phi1 = start_pos->lat; + tan_U1 = (1 - f) * tan(phi1); + cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1); + sin_U1 = tan_U1 * cos_U1; + s = distance; + alpha1 = azimuth; + sin_alpha1 = sin(alpha1); + cos_alpha1 = cos(alpha1); + tan_sigma1 = tan_U1 / cos_alpha1; + sigma1 = atan2(tan_U1, cos_alpha1); + sin_alpha = cos_U1 * sin_alpha1; + sqr_cos_alpha = 1 - sin_alpha * sin_alpha; + cos_alpha = sqrt(sqr_cos_alpha); + sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; + A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); + B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); + + /* Initialize iteration */ + sigma_initial = s / (b * A); + sigma = sigma_initial; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + cos_2_sigmam = cos(2 * sigma1 + sigma); + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + delta_sigma = 0; + sigma_prev = 2 * NMEA_PI; + remaining_steps = 20; + + while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0)) + { /* Iterate */ + cos_2_sigmam = cos(2 * sigma1 + sigma); + sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; + sin_sigma = sin(sigma); + cos_sigma = cos(sigma); + delta_sigma = B * sin_sigma * ( + cos_2_sigmam + B / 4 * ( + cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - + B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) + )); + sigma_prev = sigma; + sigma = sigma_initial + delta_sigma; + remaining_steps --; + } /* Iterate */ + + /* Calculate result */ + tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1); + phi2 = atan2( + sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1, + (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1) + ); + lambda = atan2( + sin_sigma * sin_alpha1, + cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1 + ); + C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); + L = lambda - + (1 - C) * f * sin_alpha * ( + sigma + C * sin_sigma * + (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)) + ); + + /* Result */ + end_pos->lon = start_pos->lon + L; + end_pos->lat = phi2; + if ( end_azimuth != 0 ) + { + *end_azimuth = atan2( + sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1 + ); + } + return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); +} + +/** + * \brief Convert position from INFO to radians position + */ +void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos) +{ + pos->lat = nmea_ndeg2radian(info->lat); + pos->lon = nmea_ndeg2radian(info->lon); +} + +/** + * \brief Convert radians position to INFOs position + */ +void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info) +{ + info->lat = nmea_radian2ndeg(pos->lat); + info->lon = nmea_radian2ndeg(pos->lon); +} diff --git a/apps/gps/nmealib/info.c b/apps/gps/nmealib/info.c new file mode 100644 index 000000000..1d531ffc4 --- /dev/null +++ b/apps/gps/nmealib/info.c @@ -0,0 +1,21 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: info.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include <string.h> + +#include "nmea/info.h" + +void nmea_zero_INFO(nmeaINFO *info) +{ + memset(info, 0, sizeof(nmeaINFO)); + nmea_time_now(&info->utc); + info->sig = NMEA_SIG_BAD; + info->fix = NMEA_FIX_BAD; +} diff --git a/apps/gps/nmealib/nmea/config.h b/apps/gps/nmealib/nmea/config.h new file mode 100644 index 000000000..501466218 --- /dev/null +++ b/apps/gps/nmealib/nmea/config.h @@ -0,0 +1,51 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: config.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_CONFIG_H__ +#define __NMEA_CONFIG_H__ + +#define NMEA_VERSION ("0.5.3") +#define NMEA_VERSION_MAJOR (0) +#define NMEA_VERSION_MINOR (5) +#define NMEA_VERSION_PATCH (3) + +#define NMEA_CONVSTR_BUF (256) +#define NMEA_TIMEPARSE_BUF (256) + +#if defined(WINCE) || defined(UNDER_CE) +# define NMEA_CE +#endif + +#if defined(WIN32) || defined(NMEA_CE) +# define NMEA_WIN +#else +# define NMEA_UNI +#endif + +#if defined(NMEA_WIN) && (_MSC_VER >= 1400) +# pragma warning(disable: 4996) /* declared deprecated */ +#endif + +#if defined(_MSC_VER) +# define NMEA_POSIX(x) _##x +# define NMEA_INLINE __inline +#else +# define NMEA_POSIX(x) x +# define NMEA_INLINE inline +#endif + +#if !defined(NDEBUG) && !defined(NMEA_CE) +# include <assert.h> +# define NMEA_ASSERT(x) assert(x) +#else +# define NMEA_ASSERT(x) +#endif + +#endif /* __NMEA_CONFIG_H__ */ diff --git a/apps/gps/nmealib/nmea/context.h b/apps/gps/nmealib/nmea/context.h new file mode 100644 index 000000000..06e558327 --- /dev/null +++ b/apps/gps/nmealib/nmea/context.h @@ -0,0 +1,44 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: context.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_CONTEXT_H__ +#define __NMEA_CONTEXT_H__ + +#include "config.h" + +#define NMEA_DEF_PARSEBUFF (1024) +#define NMEA_MIN_PARSEBUFF (256) + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void (*nmeaTraceFunc)(const char *str, int str_size); +typedef void (*nmeaErrorFunc)(const char *str, int str_size); + +typedef struct _nmeaPROPERTY +{ + nmeaTraceFunc trace_func; + nmeaErrorFunc error_func; + int parse_buff_size; + +} nmeaPROPERTY; + +nmeaPROPERTY * nmea_property(void); + +void nmea_trace(const char *str, ...); +void nmea_trace_buff(const char *buff, int buff_size); +void nmea_error(const char *str, ...); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_CONTEXT_H__ */ diff --git a/apps/gps/nmealib/nmea/generate.h b/apps/gps/nmealib/nmea/generate.h new file mode 100644 index 000000000..9d7fdee51 --- /dev/null +++ b/apps/gps/nmealib/nmea/generate.h @@ -0,0 +1,44 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generate.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_GENERATE_H__ +#define __NMEA_GENERATE_H__ + +#include "sentence.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int nmea_generate( + char *buff, int buff_sz, /* buffer */ + const nmeaINFO *info, /* source info */ + int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */ + ); + +int nmea_gen_GPGGA(char *buff, int buff_sz, nmeaGPGGA *pack); +int nmea_gen_GPGSA(char *buff, int buff_sz, nmeaGPGSA *pack); +int nmea_gen_GPGSV(char *buff, int buff_sz, nmeaGPGSV *pack); +int nmea_gen_GPRMC(char *buff, int buff_sz, nmeaGPRMC *pack); +int nmea_gen_GPVTG(char *buff, int buff_sz, nmeaGPVTG *pack); + +void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack); +void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack); +void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack); +void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack); + +int nmea_gsv_npack(int sat_count); +void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GENERATE_H__ */ diff --git a/apps/gps/nmealib/nmea/generator.h b/apps/gps/nmealib/nmea/generator.h new file mode 100644 index 000000000..a97b91b13 --- /dev/null +++ b/apps/gps/nmealib/nmea/generator.h @@ -0,0 +1,79 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: generator.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_GENERATOR_H__ +#define __NMEA_GENERATOR_H__ + +#include "info.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * high level + */ + +struct _nmeaGENERATOR; + +enum nmeaGENTYPE +{ + NMEA_GEN_NOISE = 0, + NMEA_GEN_STATIC, + NMEA_GEN_ROTATE, + + NMEA_GEN_SAT_STATIC, + NMEA_GEN_SAT_ROTATE, + NMEA_GEN_POS_RANDMOVE, + + NMEA_GEN_LAST +}; + +struct _nmeaGENERATOR * nmea_create_generator(int type, nmeaINFO *info); +void nmea_destroy_generator(struct _nmeaGENERATOR *gen); + +int nmea_generate_from( + char *buff, int buff_sz, /* buffer */ + nmeaINFO *info, /* source info */ + struct _nmeaGENERATOR *gen, /* generator */ + int generate_mask /* mask of sentence`s (e.g. GPGGA | GPGSA) */ + ); + +/* + * low level + */ + +typedef int (*nmeaNMEA_GEN_INIT)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_LOOP)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_RESET)(struct _nmeaGENERATOR *gen, nmeaINFO *info); +typedef int (*nmeaNMEA_GEN_DESTROY)(struct _nmeaGENERATOR *gen); + +typedef struct _nmeaGENERATOR +{ + void *gen_data; + nmeaNMEA_GEN_INIT init_call; + nmeaNMEA_GEN_LOOP loop_call; + nmeaNMEA_GEN_RESET reset_call; + nmeaNMEA_GEN_DESTROY destroy_call; + struct _nmeaGENERATOR *next; + +} nmeaGENERATOR; + +int nmea_gen_init(nmeaGENERATOR *gen, nmeaINFO *info); +int nmea_gen_loop(nmeaGENERATOR *gen, nmeaINFO *info); +int nmea_gen_reset(nmeaGENERATOR *gen, nmeaINFO *info); +void nmea_gen_destroy(nmeaGENERATOR *gen); +void nmea_gen_add(nmeaGENERATOR *to, nmeaGENERATOR *gen); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GENERATOR_H__ */ diff --git a/apps/gps/nmealib/nmea/gmath.h b/apps/gps/nmealib/nmea/gmath.h new file mode 100644 index 000000000..3133b7228 --- /dev/null +++ b/apps/gps/nmealib/nmea/gmath.h @@ -0,0 +1,92 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: gmath.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_GMATH_H__ +#define __NMEA_GMATH_H__ + +#include "info.h" + +#define NMEA_PI (3.141592653589793) /**< PI value */ +#define NMEA_PI180 (NMEA_PI / 180) /**< PI division by 180 */ +#define NMEA_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ +#define NMEA_EARTHRADIUS_M (NMEA_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ +#define NMEA_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ +#define NMEA_EARTH_SEMIMAJORAXIS_KM (NMEA_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ +#define NMEA_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ +#define NMEA_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * degree VS radian + */ + +float nmea_degree2radian(float val); +float nmea_radian2degree(float val); + +/* + * NDEG (NMEA degree) + */ + +float nmea_ndeg2degree(float val); +float nmea_degree2ndeg(float val); + +float nmea_ndeg2radian(float val); +float nmea_radian2ndeg(float val); + +/* + * DOP + */ + +float nmea_calc_pdop(float hdop, float vdop); +float nmea_dop2meters(float dop); +float nmea_meters2dop(float meters); + +/* + * positions work + */ + +void nmea_info2pos(const nmeaINFO *info, nmeaPOS *pos); +void nmea_pos2info(const nmeaPOS *pos, nmeaINFO *info); + +float nmea_distance( + const nmeaPOS *from_pos, + const nmeaPOS *to_pos + ); + +float nmea_distance_ellipsoid( + const nmeaPOS *from_pos, + const nmeaPOS *to_pos, + float *from_azimuth, + float *to_azimuth + ); + +int nmea_move_horz( + const nmeaPOS *start_pos, + nmeaPOS *end_pos, + float azimuth, + float distance + ); + +int nmea_move_horz_ellipsoid( + const nmeaPOS *start_pos, + nmeaPOS *end_pos, + float azimuth, + float distance, + float *end_azimuth + ); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_GMATH_H__ */ diff --git a/apps/gps/nmealib/nmea/info.h b/apps/gps/nmealib/nmea/info.h new file mode 100644 index 000000000..09ccd4c09 --- /dev/null +++ b/apps/gps/nmealib/nmea/info.h @@ -0,0 +1,112 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: info.h 10 2007-11-15 14:50:15Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_INFO_H__ +#define __NMEA_INFO_H__ + +#include "time.h" + +#define NMEA_SIG_BAD (0) +#define NMEA_SIG_LOW (1) +#define NMEA_SIG_MID (2) +#define NMEA_SIG_HIGH (3) + +#define NMEA_FIX_BAD (1) +#define NMEA_FIX_2D (2) +#define NMEA_FIX_3D (3) + +#define NMEA_MAXSAT (12) +#define NMEA_SATINPACK (4) +#define NMEA_NSATPACKS (NMEA_MAXSAT / NMEA_SATINPACK) + +#define NMEA_DEF_LAT (5001.2621) +#define NMEA_DEF_LON (3613.0595) + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Position data in fractional degrees or radians + */ +typedef struct _nmeaPOS +{ + float lat; /**< Latitude */ + float lon; /**< Longitude */ + +} nmeaPOS; + +/** + * Information about satellite + * @see nmeaSATINFO + * @see nmeaGPGSV + */ +typedef struct _nmeaSATELLITE +{ + int id; /**< Satellite PRN number */ + int in_use; /**< Used in position fix */ + int elv; /**< Elevation in degrees, 90 maximum */ + int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */ + int sig; /**< Signal, 00-99 dB */ + +} nmeaSATELLITE; + +/** + * Information about all satellites in view + * @see nmeaINFO + * @see nmeaGPGSV + */ +typedef struct _nmeaSATINFO +{ + int inuse; /**< Number of satellites in use (not those in view) */ + int inview; /**< Total number of satellites in view */ + nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */ + +} nmeaSATINFO; + +/** + * Summary GPS information from all parsed packets, + * used also for generating NMEA stream + * @see nmea_parse + * @see nmea_GPGGA2info, nmea_...2info + */ +typedef struct _nmeaINFO +{ + int smask; /**< Mask specifying types of packages from which data have been obtained */ + + nmeaTIME utc; /**< UTC of position */ + + int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ + int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ + + float PDOP; /**< Position Dilution Of Precision */ + float HDOP; /**< Horizontal Dilution Of Precision */ + float VDOP; /**< Vertical Dilution Of Precision */ + + float lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */ + float lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */ + float elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */ + float speed; /**< Speed over the ground in kilometers/hour */ + float direction; /**< Track angle in degrees True */ + float declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ + + nmeaSATINFO satinfo; /**< Satellites information */ + +} nmeaINFO; + +void nmea_zero_INFO(nmeaINFO *info); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_INFO_H__ */ diff --git a/apps/gps/nmealib/nmea/nmea.h b/apps/gps/nmealib/nmea/nmea.h new file mode 100644 index 000000000..62692230f --- /dev/null +++ b/apps/gps/nmealib/nmea/nmea.h @@ -0,0 +1,25 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: nmea.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#ifndef __NMEA_H__ +#define __NMEA_H__ + +#include "./config.h" +#include "./units.h" +#include "./gmath.h" +#include "./info.h" +#include "./sentence.h" +#include "./generate.h" +#include "./generator.h" +#include "./parse.h" +#include "./parser.h" +#include "./context.h" + +#endif /* __NMEA_H__ */ diff --git a/apps/gps/nmealib/nmea/parse.h b/apps/gps/nmealib/nmea/parse.h new file mode 100644 index 000000000..3e6b425db --- /dev/null +++ b/apps/gps/nmealib/nmea/parse.h @@ -0,0 +1,39 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parse.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_PARSE_H__ +#define __NMEA_PARSE_H__ + +#include "sentence.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int nmea_pack_type(const char *buff, int buff_sz); +int nmea_find_tail(const char *buff, int buff_sz, int *res_crc); + +int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack); +int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack); +int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack); +int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack); +int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack); + +void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info); +void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info); +void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info); +void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info); +void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_PARSE_H__ */ diff --git a/apps/gps/nmealib/nmea/parser.h b/apps/gps/nmealib/nmea/parser.h new file mode 100644 index 000000000..51a3fab7f --- /dev/null +++ b/apps/gps/nmealib/nmea/parser.h @@ -0,0 +1,59 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parser.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_PARSER_H__ +#define __NMEA_PARSER_H__ + +#include "info.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * high level + */ + +typedef struct _nmeaPARSER +{ + void *top_node; + void *end_node; + unsigned char *buffer; + int buff_size; + int buff_use; + +} nmeaPARSER; + +int nmea_parser_init(nmeaPARSER *parser); +void nmea_parser_destroy(nmeaPARSER *parser); + +int nmea_parse( + nmeaPARSER *parser, + const char *buff, int buff_sz, + nmeaINFO *info + ); + +/* + * low level + */ + +int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz); +int nmea_parser_top(nmeaPARSER *parser); +int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr); +int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr); +int nmea_parser_drop(nmeaPARSER *parser); +int nmea_parser_buff_clear(nmeaPARSER *parser); +int nmea_parser_queue_clear(nmeaPARSER *parser); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_PARSER_H__ */ diff --git a/apps/gps/nmealib/nmea/sentence.h b/apps/gps/nmealib/nmea/sentence.h new file mode 100644 index 000000000..2aa975c71 --- /dev/null +++ b/apps/gps/nmealib/nmea/sentence.h @@ -0,0 +1,128 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: sentence.h 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_SENTENCE_H__ +#define __NMEA_SENTENCE_H__ + +#include "info.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * NMEA packets type which parsed and generated by library + */ +enum nmeaPACKTYPE +{ + GPNON = 0x0000, /**< Unknown packet type. */ + GPGGA = 0x0001, /**< GGA - Essential fix data which provide 3D location and accuracy data. */ + GPGSA = 0x0002, /**< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values. */ + GPGSV = 0x0004, /**< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values. */ + GPRMC = 0x0008, /**< RMC - Recommended Minimum Specific GPS/TRANSIT Data. */ + GPVTG = 0x0010 /**< VTG - Actual track made good and speed over ground. */ +}; + +/** + * GGA packet information structure (Global Positioning System Fix Data) + */ +typedef struct _nmeaGPGGA +{ + nmeaTIME utc; /**< UTC of position (just time) */ + float lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ + char ns; /**< [N]orth or [S]outh */ + float lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ + char ew; /**< [E]ast or [W]est */ + int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ + int satinuse; /**< Number of satellites in use (not those in view) */ + float HDOP; /**< Horizontal dilution of precision */ + float elv; /**< Antenna altitude above/below mean sea level (geoid) */ + char elv_units; /**< [M]eters (Antenna height unit) */ + float diff; /**< Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level. '-' = geoid is below WGS-84 ellipsoid) */ + char diff_units; /**< [M]eters (Units of geoidal separation) */ + float dgps_age; /**< Time in seconds since last DGPS update */ + int dgps_sid; /**< DGPS station ID number */ + +} nmeaGPGGA; + +/** + * GSA packet information structure (Satellite status) + */ +typedef struct _nmeaGPGSA +{ + char fix_mode; /**< Mode (M = Manual, forced to operate in 2D or 3D; A = Automatic, 3D/2D) */ + int fix_type; /**< Type, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ + int sat_prn[NMEA_MAXSAT]; /**< PRNs of satellites used in position fix (null for unused fields) */ + float PDOP; /**< Dilution of precision */ + float HDOP; /**< Horizontal dilution of precision */ + float VDOP; /**< Vertical dilution of precision */ + +} nmeaGPGSA; + +/** + * GSV packet information structure (Satellites in view) + */ +typedef struct _nmeaGPGSV +{ + int pack_count; /**< Total number of messages of this type in this cycle */ + int pack_index; /**< Message number */ + int sat_count; /**< Total number of satellites in view */ + nmeaSATELLITE sat_data[NMEA_SATINPACK]; + +} nmeaGPGSV; + +/** + * RMC packet information structure (Recommended Minimum sentence C) + */ +typedef struct _nmeaGPRMC +{ + nmeaTIME utc; /**< UTC of position */ + char status; /**< Status (A = active or V = void) */ + float lat; /**< Latitude in NDEG - [degree][min].[sec/60] */ + char ns; /**< [N]orth or [S]outh */ + float lon; /**< Longitude in NDEG - [degree][min].[sec/60] */ + char ew; /**< [E]ast or [W]est */ + float speed; /**< Speed over the ground in knots */ + float direction; /**< Track angle in degrees True */ + float declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ + char declin_ew; /**< [E]ast or [W]est */ + char mode; /**< Mode indicator of fix type (A = autonomous, D = differential, E = estimated, N = not valid, S = simulator) */ + +} nmeaGPRMC; + +/** + * VTG packet information structure (Track made good and ground speed) + */ +typedef struct _nmeaGPVTG +{ + float dir; /**< True track made good (degrees) */ + char dir_t; /**< Fixed text 'T' indicates that track made good is relative to true north */ + float dec; /**< Magnetic track made good */ + char dec_m; /**< Fixed text 'M' */ + float spn; /**< Ground speed, knots */ + char spn_n; /**< Fixed text 'N' indicates that speed over ground is in knots */ + float spk; /**< Ground speed, kilometers per hour */ + char spk_k; /**< Fixed text 'K' indicates that speed over ground is in kilometers/hour */ + +} nmeaGPVTG; + +void nmea_zero_GPGGA(nmeaGPGGA *pack); +void nmea_zero_GPGSA(nmeaGPGSA *pack); +void nmea_zero_GPGSV(nmeaGPGSV *pack); +void nmea_zero_GPRMC(nmeaGPRMC *pack); +void nmea_zero_GPVTG(nmeaGPVTG *pack); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_SENTENCE_H__ */ diff --git a/apps/gps/nmealib/nmea/time.h b/apps/gps/nmealib/nmea/time.h new file mode 100644 index 000000000..bbe59f65c --- /dev/null +++ b/apps/gps/nmealib/nmea/time.h @@ -0,0 +1,47 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: time.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +/*! \file */ + +#ifndef __NMEA_TIME_H__ +#define __NMEA_TIME_H__ + +#include "config.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Date and time data + * @see nmea_time_now + */ +typedef struct _nmeaTIME +{ + int year; /**< Years since 1900 */ + int mon; /**< Months since January - [0,11] */ + int day; /**< Day of the month - [1,31] */ + int hour; /**< Hours since midnight - [0,23] */ + int min; /**< Minutes after the hour - [0,59] */ + int sec; /**< Seconds after the minute - [0,59] */ + int hsec; /**< Hundredth part of second - [0,99] */ + +} nmeaTIME; + +/** + * \brief Get time now to nmeaTIME structure + */ +void nmea_time_now(nmeaTIME *t); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_TIME_H__ */ diff --git a/apps/gps/nmealib/nmea/tok.h b/apps/gps/nmealib/nmea/tok.h new file mode 100644 index 000000000..8f948dd2d --- /dev/null +++ b/apps/gps/nmealib/nmea/tok.h @@ -0,0 +1,31 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: tok.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_TOK_H__ +#define __NMEA_TOK_H__ + +#include "config.h" +#include <string.h> + +#ifdef __cplusplus +extern "C" { +#endif + +int nmea_calc_crc(const char *buff, int buff_sz); +int nmea_atoi(const char *str, int str_sz, int radix); +float nmea_atof(const char *str, int str_sz); +int nmea_printf(char *buff, int buff_sz, const char *format, ...); +int nmea_scanf(const char *buff, int buff_sz, const char *format, ...); + +#ifdef __cplusplus +} +#endif + +#endif /* __NMEA_TOK_H__ */ diff --git a/apps/gps/nmealib/nmea/units.h b/apps/gps/nmealib/nmea/units.h new file mode 100644 index 000000000..767f980e2 --- /dev/null +++ b/apps/gps/nmealib/nmea/units.h @@ -0,0 +1,30 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: units.h 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +#ifndef __NMEA_UNITS_H__ +#define __NMEA_UNITS_H__ + +#include "config.h" + +/* + * Distance units + */ + +#define NMEA_TUD_YARDS (1.0936) /**< Yeards, meter * NMEA_TUD_YARDS = yard */ +#define NMEA_TUD_KNOTS (1.852) /**< Knots, kilometer / NMEA_TUD_KNOTS = knot */ +#define NMEA_TUD_MILES (1.609) /**< Miles, kilometer / NMEA_TUD_MILES = mile */ + +/* + * Speed units + */ + +#define NMEA_TUS_MS (3.6) /**< Meters per seconds, (k/h) / NMEA_TUS_MS= (m/s) */ + +#endif /* __NMEA_UNITS_H__ */ diff --git a/apps/gps/nmealib/parse.c b/apps/gps/nmealib/parse.c new file mode 100644 index 000000000..99bdf075b --- /dev/null +++ b/apps/gps/nmealib/parse.c @@ -0,0 +1,501 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parse.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/** + * \file parse.h + * \brief Functions of a low level for analysis of + * packages of NMEA stream. + * + * \code + * ... + * ptype = nmea_pack_type( + * (const char *)parser->buffer + nparsed + 1, + * parser->buff_use - nparsed - 1); + * + * if(0 == (node = malloc(sizeof(nmeaParserNODE)))) + * goto mem_fail; + * + * node->pack = 0; + * + * switch(ptype) + * { + * case GPGGA: + * if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) + * goto mem_fail; + * node->packType = GPGGA; + * if(!nmea_parse_GPGGA( + * (const char *)parser->buffer + nparsed, + * sen_sz, (nmeaGPGGA *)node->pack)) + * { + * free(node); + * node = 0; + * } + * break; + * case GPGSA: + * if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) + * goto mem_fail; + * node->packType = GPGSA; + * if(!nmea_parse_GPGSA( + * (const char *)parser->buffer + nparsed, + * sen_sz, (nmeaGPGSA *)node->pack)) + * { + * free(node); + * node = 0; + * } + * break; + * ... + * \endcode + */ +#include "nmea/tok.h" +#include "nmea/parse.h" +#include "nmea/context.h" +#include "nmea/gmath.h" +#include "nmea/units.h" + +#include <string.h> +#include <stdio.h> + +int _nmea_parse_time(const char *buff, int buff_sz, nmeaTIME *res) +{ + int success = 0; + + switch(buff_sz) + { + case sizeof("hhmmss") - 1: + success = (3 == nmea_scanf(buff, buff_sz, + "%2d%2d%2d", &(res->hour), &(res->min), &(res->sec) + )); + break; + case sizeof("hhmmss.s") - 1: + case sizeof("hhmmss.ss") - 1: + case sizeof("hhmmss.sss") - 1: + success = (4 == nmea_scanf(buff, buff_sz, + "%2d%2d%2d.%d", &(res->hour), &(res->min), &(res->sec), &(res->hsec) + )); + break; + default: + nmea_error("Parse of time error (format error)!"); + success = 0; + break; + } + + return (success?0:-1); +} + +/** + * \brief Define packet type by header (nmeaPACKTYPE). + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @return The defined packet type + * @see nmeaPACKTYPE + */ +int nmea_pack_type(const char *buff, int buff_sz) +{ + static const char *pheads[] = { + "GPGGA", + "GPGSA", + "GPGSV", + "GPRMC", + "GPVTG", + }; + + //NMEA_ASSERT(buff); + + if(buff_sz < 5) + return GPNON; + else if(0 == memcmp(buff, pheads[0], 5)) + return GPGGA; + else if(0 == memcmp(buff, pheads[1], 5)) + return GPGSA; + else if(0 == memcmp(buff, pheads[2], 5)) + return GPGSV; + else if(0 == memcmp(buff, pheads[3], 5)) + return GPRMC; + else if(0 == memcmp(buff, pheads[4], 5)) + return GPVTG; + + return GPNON; +} + +/** + * \brief Find tail of packet ("\r\n") in buffer and check control sum (CRC). + * @param buff a constant character pointer of packets buffer. + * @param buff_sz buffer size. + * @param res_crc a integer pointer for return CRC of packet (must be defined). + * @return Number of bytes to packet tail. + */ +int nmea_find_tail(const char *buff, int buff_sz, int *res_crc) +{ + static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */; + + const char *end_buff = buff + buff_sz; + int nread = 0; + int crc = 0; + + //NMEA_ASSERT(buff && res_crc); + + *res_crc = -1; + + for(;buff < end_buff; ++buff, ++nread) + { + if(('$' == *buff) && nread) + { + buff = 0; + break; + } + else if('*' == *buff) + { + if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4]) + { + *res_crc = nmea_atoi(buff + 1, 2, 16); + nread = buff_sz - (int)(end_buff - (buff + tail_sz)); + if(*res_crc != crc) + { + *res_crc = -1; + buff = 0; + } + } + + break; + } + else if(nread) + crc ^= (int)*buff; + } + + if(*res_crc < 0 && buff) + nread = 0; + + return nread; +} + +/** + * \brief Parse GGA packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack) +{ + char time_buff[NMEA_TIMEPARSE_BUF]; + + //NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPGGA)); + + nmea_trace_buff(buff, buff_sz); + + if(14 != nmea_scanf(buff, buff_sz, + "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*", + &(time_buff[0]), + &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), + &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units), + &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid))) + { + nmea_error("GPGGA parse error!"); + return 0; + } + + if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) + { + nmea_error("GPGGA time parse error!"); + return 0; + } + + return 1; +} + +/** + * \brief Parse GSA packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPGSA(const char *buff, int buff_sz, nmeaGPGSA *pack) +{ + //NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPGSA)); + + nmea_trace_buff(buff, buff_sz); + + if(17 != nmea_scanf(buff, buff_sz, + "$GPGSA,%C,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*", + &(pack->fix_mode), &(pack->fix_type), + &(pack->sat_prn[0]), &(pack->sat_prn[1]), &(pack->sat_prn[2]), &(pack->sat_prn[3]), &(pack->sat_prn[4]), &(pack->sat_prn[5]), + &(pack->sat_prn[6]), &(pack->sat_prn[7]), &(pack->sat_prn[8]), &(pack->sat_prn[9]), &(pack->sat_prn[10]), &(pack->sat_prn[11]), + &(pack->PDOP), &(pack->HDOP), &(pack->VDOP))) + { + nmea_error("GPGSA parse error!"); + return 0; + } + + return 1; +} + +/** + * \brief Parse GSV packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack) +{ + int nsen, nsat; + + //NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPGSV)); + + nmea_trace_buff(buff, buff_sz); + + nsen = nmea_scanf(buff, buff_sz, + "$GPGSV,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d," + "%d,%d,%d,%d*", + &(pack->pack_count), &(pack->pack_index), &(pack->sat_count), + &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig), + &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig), + &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig), + &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig)); + + nsat = (pack->pack_index - 1) * NMEA_SATINPACK; + nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; + nsat = nsat * 4 + 3 /* first three sentence`s */; + + if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3)) + { + nmea_error("GPGSV parse error!"); + return 0; + } + + return 1; +} + +/** + * \brief Parse RMC packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPRMC(const char *buff, int buff_sz, nmeaGPRMC *pack) +{ + int nsen; + char time_buff[NMEA_TIMEPARSE_BUF]; + + //NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPRMC)); + + nmea_trace_buff(buff, buff_sz); + + nsen = nmea_scanf(buff, buff_sz, + "$GPRMC,%s,%C,%f,%C,%f,%C,%f,%f,%2d%2d%2d,%f,%C,%C*", + &(time_buff[0]), + &(pack->status), &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew), + &(pack->speed), &(pack->direction), + &(pack->utc.day), &(pack->utc.mon), &(pack->utc.year), + &(pack->declination), &(pack->declin_ew), &(pack->mode)); + + if(nsen != 13 && nsen != 14) + { + nmea_error("GPRMC parse error!"); + return 0; + } + + if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc))) + { + nmea_error("GPRMC time parse error!"); + return 0; + } + + if(pack->utc.year < 90) + pack->utc.year += 100; + pack->utc.mon -= 1; + + return 1; +} + +/** + * \brief Parse VTG packet from buffer. + * @param buff a constant character pointer of packet buffer. + * @param buff_sz buffer size. + * @param pack a pointer of packet which will filled by function. + * @return 1 (true) - if parsed successfully or 0 (false) - if fail. + */ +int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack) +{ + //NMEA_ASSERT(buff && pack); + + memset(pack, 0, sizeof(nmeaGPVTG)); + + nmea_trace_buff(buff, buff_sz); + + if(8 != nmea_scanf(buff, buff_sz, + "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*", + &(pack->dir), &(pack->dir_t), + &(pack->dec), &(pack->dec_m), + &(pack->spn), &(pack->spn_n), + &(pack->spk), &(pack->spk_k))) + { + nmea_error("GPVTG parse error!"); + return 0; + } + + if( pack->dir_t != 'T' || + pack->dec_m != 'M' || + pack->spn_n != 'N' || + pack->spk_k != 'K') + { + nmea_error("GPVTG parse error (format error)!"); + return 0; + } + + return 1; +} + +/** + * \brief Fill nmeaINFO structure by GGA packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPGGA2info(nmeaGPGGA *pack, nmeaINFO *info) +{ + //NMEA_ASSERT(pack && info); + + info->utc.hour = pack->utc.hour; + info->utc.min = pack->utc.min; + info->utc.sec = pack->utc.sec; + info->utc.hsec = pack->utc.hsec; + info->sig = pack->sig; + info->HDOP = pack->HDOP; + info->elv = pack->elv; + info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); + info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); + info->smask |= GPGGA; +} + +/** + * \brief Fill nmeaINFO structure by GSA packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info) +{ + int i, j, nuse = 0; + + //NMEA_ASSERT(pack && info); + + info->fix = pack->fix_type; + info->PDOP = pack->PDOP; + info->HDOP = pack->HDOP; + info->VDOP = pack->VDOP; + + for(i = 0; i < NMEA_MAXSAT; ++i) + { + for(j = 0; j < info->satinfo.inview; ++j) + { + if(pack->sat_prn[i] && pack->sat_prn[i] == info->satinfo.sat[j].id) + { + info->satinfo.sat[j].in_use = 1; + nuse++; + } + } + } + + info->satinfo.inuse = nuse; + info->smask |= GPGSA; +} + +/** + * \brief Fill nmeaINFO structure by GSV packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info) +{ + int isat, isi, nsat; + + //NMEA_ASSERT(pack && info); + + if(pack->pack_index > pack->pack_count || + pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT) + return; + + if(pack->pack_index < 1) + pack->pack_index = 1; + + info->satinfo.inview = pack->sat_count; + + nsat = (pack->pack_index - 1) * NMEA_SATINPACK; + nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK; + + for(isat = 0; isat < nsat; ++isat) + { + isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat; + info->satinfo.sat[isi].id = pack->sat_data[isat].id; + info->satinfo.sat[isi].elv = pack->sat_data[isat].elv; + info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth; + info->satinfo.sat[isi].sig = pack->sat_data[isat].sig; + } + + info->smask |= GPGSV; +} + +/** + * \brief Fill nmeaINFO structure by RMC packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPRMC2info(nmeaGPRMC *pack, nmeaINFO *info) +{ + //NMEA_ASSERT(pack && info); + + if('A' == pack->status) + { + if(NMEA_SIG_BAD == info->sig) + info->sig = NMEA_SIG_MID; + if(NMEA_FIX_BAD == info->fix) + info->fix = NMEA_FIX_2D; + } + else if('V' == pack->status) + { + info->sig = NMEA_SIG_BAD; + info->fix = NMEA_FIX_BAD; + } + + + info->utc = pack->utc; + info->lat = ((pack->ns == 'N')?pack->lat:-(pack->lat)); + info->lon = ((pack->ew == 'E')?pack->lon:-(pack->lon)); + info->speed = pack->speed * NMEA_TUD_KNOTS; + info->direction = pack->direction; + info->smask |= GPRMC; +} + +/** + * \brief Fill nmeaINFO structure by VTG packet data. + * @param pack a pointer of packet structure. + * @param info a pointer of summary information structure. + */ +void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info) +{ + //NMEA_ASSERT(pack && info); + + info->direction = pack->dir; + info->declination = pack->dec; + info->speed = pack->spk; + info->smask |= GPVTG; +} diff --git a/apps/gps/nmealib/parser.c b/apps/gps/nmealib/parser.c new file mode 100644 index 000000000..f8b415866 --- /dev/null +++ b/apps/gps/nmealib/parser.c @@ -0,0 +1,400 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: parser.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/** + * \file parser.h + */ +#include "nmea/tok.h" +#include "nmea/parse.h" +#include "nmea/parser.h" +#include "nmea/context.h" + +#include <string.h> +#include <stdlib.h> + +typedef struct _nmeaParserNODE +{ + int packType; + void *pack; + struct _nmeaParserNODE *next_node; + +} nmeaParserNODE; + +/* + * high level + */ + +/** + * \brief Initialization of parser object + * @return true (1) - success or false (0) - fail + */ +int nmea_parser_init(nmeaPARSER *parser) +{ + int resv = 0; + int buff_size = nmea_property()->parse_buff_size; + + //NMEA_ASSERT(parser); + + if(buff_size < NMEA_MIN_PARSEBUFF) + buff_size = NMEA_MIN_PARSEBUFF; + + memset(parser, 0, sizeof(nmeaPARSER)); + + if(0 == (parser->buffer = malloc(buff_size))) + nmea_error("Insufficient memory!"); + else + { + parser->buff_size = buff_size; + resv = 1; + } + + return resv; +} + +/** + * \brief Destroy parser object + */ +void nmea_parser_destroy(nmeaPARSER *parser) +{ + //NMEA_ASSERT(parser && parser->buffer); + free(parser->buffer); + nmea_parser_queue_clear(parser); + memset(parser, 0, sizeof(nmeaPARSER)); +} + +/** + * \brief Analysis of buffer and put results to information structure + * @return Number of packets wos parsed + */ +int nmea_parse( + nmeaPARSER *parser, + const char *buff, int buff_sz, + nmeaINFO *info + ) +{ + int ptype, nread = 0; + void *pack = 0; + + //NMEA_ASSERT(parser && parser->buffer); + + nmea_parser_push(parser, buff, buff_sz); + + while(GPNON != (ptype = nmea_parser_pop(parser, &pack))) + { + nread++; + + switch(ptype) + { + case GPGGA: + nmea_GPGGA2info((nmeaGPGGA *)pack, info); + break; + case GPGSA: + nmea_GPGSA2info((nmeaGPGSA *)pack, info); + break; + case GPGSV: + nmea_GPGSV2info((nmeaGPGSV *)pack, info); + break; + case GPRMC: + nmea_GPRMC2info((nmeaGPRMC *)pack, info); + break; + case GPVTG: + nmea_GPVTG2info((nmeaGPVTG *)pack, info); + break; + }; + + free(pack); + } + + return nread; +} + +/* + * low level + */ + +int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz) +{ + int nparsed = 0, crc, sen_sz, ptype; + nmeaParserNODE *node = 0; + + //NMEA_ASSERT(parser && parser->buffer); + + /* clear unuse buffer (for debug) */ + /* + memset( + parser->buffer + parser->buff_use, 0, + parser->buff_size - parser->buff_use + ); + */ + + /* add */ + if(parser->buff_use + buff_sz >= parser->buff_size) + nmea_parser_buff_clear(parser); + + memcpy(parser->buffer + parser->buff_use, buff, buff_sz); + parser->buff_use += buff_sz; + + /* parse */ + for(;;node = 0) + { + sen_sz = nmea_find_tail( + (const char *)parser->buffer + nparsed, + (int)parser->buff_use - nparsed, &crc); + + if(!sen_sz) + { + if(nparsed) + memcpy( + parser->buffer, + parser->buffer + nparsed, + parser->buff_use -= nparsed); + break; + } + else if(crc >= 0) + { + ptype = nmea_pack_type( + (const char *)parser->buffer + nparsed + 1, + parser->buff_use - nparsed - 1); + + if(0 == (node = malloc(sizeof(nmeaParserNODE)))) + goto mem_fail; + + node->pack = 0; + + switch(ptype) + { + case GPGGA: + if(0 == (node->pack = malloc(sizeof(nmeaGPGGA)))) + goto mem_fail; + node->packType = GPGGA; + if(!nmea_parse_GPGGA( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPGGA *)node->pack)) + { + free(node); + node = 0; + } + break; + case GPGSA: + if(0 == (node->pack = malloc(sizeof(nmeaGPGSA)))) + goto mem_fail; + node->packType = GPGSA; + if(!nmea_parse_GPGSA( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPGSA *)node->pack)) + { + free(node); + node = 0; + } + break; + case GPGSV: + if(0 == (node->pack = malloc(sizeof(nmeaGPGSV)))) + goto mem_fail; + node->packType = GPGSV; + if(!nmea_parse_GPGSV( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPGSV *)node->pack)) + { + free(node); + node = 0; + } + break; + case GPRMC: + if(0 == (node->pack = malloc(sizeof(nmeaGPRMC)))) + goto mem_fail; + node->packType = GPRMC; + if(!nmea_parse_GPRMC( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPRMC *)node->pack)) + { + free(node); + node = 0; + } + break; + case GPVTG: + if(0 == (node->pack = malloc(sizeof(nmeaGPVTG)))) + goto mem_fail; + node->packType = GPVTG; + if(!nmea_parse_GPVTG( + (const char *)parser->buffer + nparsed, + sen_sz, (nmeaGPVTG *)node->pack)) + { + free(node); + node = 0; + } + break; + default: + free(node); + node = 0; + break; + }; + + if(node) + { + if(parser->end_node) + ((nmeaParserNODE *)parser->end_node)->next_node = node; + parser->end_node = node; + if(!parser->top_node) + parser->top_node = node; + node->next_node = 0; + } + } + + nparsed += sen_sz; + } + + return nparsed; + +mem_fail: + if(node) + free(node); + + nmea_error("Insufficient memory!"); + + return -1; +} + +/** + * \brief Analysis of buffer and keep results into parser + * @return Number of bytes wos parsed from buffer + */ +int nmea_parser_push(nmeaPARSER *parser, const char *buff, int buff_sz) +{ + int nparse, nparsed = 0; + + do + { + if(buff_sz > parser->buff_size) + nparse = parser->buff_size; + else + nparse = buff_sz; + + nparsed += nmea_parser_real_push( + parser, buff, nparse); + + buff_sz -= nparse; + + } while(buff_sz); + + return nparsed; +} + +/** + * \brief Get type of top packet keeped into parser + * @return Type of packet + * @see nmeaPACKTYPE + */ +int nmea_parser_top(nmeaPARSER *parser) +{ + int retval = GPNON; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + //NMEA_ASSERT(parser && parser->buffer); + + if(node) + retval = node->packType; + + return retval; +} + +/** + * \brief Withdraw top packet from parser + * @return Received packet type + * @see nmeaPACKTYPE + */ +int nmea_parser_pop(nmeaPARSER *parser, void **pack_ptr) +{ + int retval = GPNON; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + //NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + *pack_ptr = node->pack; + retval = node->packType; + parser->top_node = node->next_node; + if(!parser->top_node) + parser->end_node = 0; + free(node); + } + + return retval; +} + +/** + * \brief Get top packet from parser without withdraw + * @return Received packet type + * @see nmeaPACKTYPE + */ +int nmea_parser_peek(nmeaPARSER *parser, void **pack_ptr) +{ + int retval = GPNON; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + //NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + *pack_ptr = node->pack; + retval = node->packType; + } + + return retval; +} + +/** + * \brief Delete top packet from parser + * @return Deleted packet type + * @see nmeaPACKTYPE + */ +int nmea_parser_drop(nmeaPARSER *parser) +{ + int retval = GPNON; + nmeaParserNODE *node = (nmeaParserNODE *)parser->top_node; + + //NMEA_ASSERT(parser && parser->buffer); + + if(node) + { + if(node->pack) + free(node->pack); + retval = node->packType; + parser->top_node = node->next_node; + if(!parser->top_node) + parser->end_node = 0; + free(node); + } + + return retval; +} + +/** + * \brief Clear cache of parser + * @return true (1) - success + */ +int nmea_parser_buff_clear(nmeaPARSER *parser) +{ + //NMEA_ASSERT(parser && parser->buffer); + parser->buff_use = 0; + return 1; +} + +/** + * \brief Clear packets queue into parser + * @return true (1) - success + */ +int nmea_parser_queue_clear(nmeaPARSER *parser) +{ + //NMEA_ASSERT(parser); + while(parser->top_node) + nmea_parser_drop(parser); + return 1; +} diff --git a/apps/gps/nmealib/sentence.c b/apps/gps/nmealib/sentence.c new file mode 100644 index 000000000..a66393a4d --- /dev/null +++ b/apps/gps/nmealib/sentence.c @@ -0,0 +1,54 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: sentence.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +#include "nmea/sentence.h" + +#include <string.h> + +void nmea_zero_GPGGA(nmeaGPGGA *pack) +{ + memset(pack, 0, sizeof(nmeaGPGGA)); + nmea_time_now(&pack->utc); + pack->ns = 'N'; + pack->ew = 'E'; + pack->elv_units = 'M'; + pack->diff_units = 'M'; +} + +void nmea_zero_GPGSA(nmeaGPGSA *pack) +{ + memset(pack, 0, sizeof(nmeaGPGSA)); + pack->fix_mode = 'A'; + pack->fix_type = NMEA_FIX_BAD; +} + +void nmea_zero_GPGSV(nmeaGPGSV *pack) +{ + memset(pack, 0, sizeof(nmeaGPGSV)); +} + +void nmea_zero_GPRMC(nmeaGPRMC *pack) +{ + memset(pack, 0, sizeof(nmeaGPRMC)); + nmea_time_now(&pack->utc); + pack->status = 'V'; + pack->ns = 'N'; + pack->ew = 'E'; + pack->declin_ew = 'E'; +} + +void nmea_zero_GPVTG(nmeaGPVTG *pack) +{ + memset(pack, 0, sizeof(nmeaGPVTG)); + pack->dir_t = 'T'; + pack->dec_m = 'M'; + pack->spn_n = 'N'; + pack->spk_k = 'K'; +} diff --git a/apps/gps/nmealib/time.c b/apps/gps/nmealib/time.c new file mode 100644 index 000000000..a24319630 --- /dev/null +++ b/apps/gps/nmealib/time.c @@ -0,0 +1,63 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: time.c 4 2007-08-27 13:11:03Z xtimor $ + * + */ + +/*! \file time.h */ + +#include "nmea/time.h" + +#ifdef NMEA_WIN +# pragma warning(disable: 4201) +# pragma warning(disable: 4214) +# pragma warning(disable: 4115) +# include <windows.h> +# pragma warning(default: 4201) +# pragma warning(default: 4214) +# pragma warning(default: 4115) +#else +# include <time.h> +#endif + +#ifdef NMEA_WIN + +void nmea_time_now(nmeaTIME *stm) +{ + SYSTEMTIME st; + + GetSystemTime(&st); + + stm->year = st.wYear - 1900; + stm->mon = st.wMonth - 1; + stm->day = st.wDay; + stm->hour = st.wHour; + stm->min = st.wMinute; + stm->sec = st.wSecond; + stm->hsec = st.wMilliseconds / 10; +} + +#else /* NMEA_WIN */ + +void nmea_time_now(nmeaTIME *stm) +{ + time_t lt; + struct tm *tt; + + time(<); + tt = gmtime(<); + + stm->year = tt->tm_year; + stm->mon = tt->tm_mon; + stm->day = tt->tm_mday; + stm->hour = tt->tm_hour; + stm->min = tt->tm_min; + stm->sec = tt->tm_sec; + stm->hsec = 0; +} + +#endif diff --git a/apps/gps/nmealib/tok.c b/apps/gps/nmealib/tok.c new file mode 100644 index 000000000..c7cf2f4dc --- /dev/null +++ b/apps/gps/nmealib/tok.c @@ -0,0 +1,267 @@ +/* + * + * NMEA library + * URL: http://nmea.sourceforge.net + * Author: Tim (xtimor@gmail.com) + * Licence: http://www.gnu.org/licenses/lgpl.html + * $Id: tok.c 17 2008-03-11 11:56:11Z xtimor $ + * + */ + +/*! \file tok.h */ + +#include "nmea/tok.h" + +#include <stdarg.h> +#include <stdlib.h> +#include <stdio.h> +#include <ctype.h> +#include <string.h> +#include <limits.h> + +#define NMEA_TOKS_COMPARE (1) +#define NMEA_TOKS_PERCENT (2) +#define NMEA_TOKS_WIDTH (3) +#define NMEA_TOKS_TYPE (4) + +//memchr replacement +void * +memchr(s, c, n) + const void *s; + unsigned char c; + size_t n; +{ + if (n != 0) { + const unsigned char *p = s; + + do { + if (*p++ == c) + return ((void *)(p - 1)); + } while (--n != 0); + } + return (NULL); +} + + +/** + * \brief Calculate control sum of binary buffer + */ +int nmea_calc_crc(const char *buff, int buff_sz) +{ + int chsum = 0, + it; + + for(it = 0; it < buff_sz; ++it) + chsum ^= (int)buff[it]; + + return chsum; +} + +/** + * \brief Convert string to number + */ +int nmea_atoi(const char *str, int str_sz, int radix) +{ + char *tmp_ptr; + char buff[NMEA_CONVSTR_BUF]; + int res = 0; + + if(str_sz < NMEA_CONVSTR_BUF) + { + memcpy(&buff[0], str, str_sz); + buff[str_sz] = '\0'; + res = strtol(&buff[0], &tmp_ptr, radix); + } + return res; +} + +/** + * \brief Convert string to fraction number + */ +float nmea_atof(const char *str, int str_sz) +{ + char *tmp_ptr; + char buff[NMEA_CONVSTR_BUF]; + float res = 0; + + if(str_sz < NMEA_CONVSTR_BUF) + { + memcpy(&buff[0], str, str_sz); + buff[str_sz] = '\0'; + res = (float)strtod(&buff[0], &tmp_ptr); + } + return res; +} + +/** + * \brief Formating string (like standart printf) with CRC tail (*CRC) + */ +int nmea_printf(char *buff, int buff_sz, const char *format, ...) +{ + int retval, add = 0; + va_list arg_ptr; + + if(buff_sz <= 0) + return 0; + + va_start(arg_ptr, format); + + retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr); + + if(retval > 0) + { + add = NMEA_POSIX(snprintf)( + buff + retval, buff_sz - retval, "*%02x\r\n", + nmea_calc_crc(buff + 1, retval - 1)); + } + + retval += add; + + if(retval < 0 || retval > buff_sz) + { + memset(buff, ' ', buff_sz); + retval = buff_sz; + } + + va_end(arg_ptr); + + return retval; +} + +/** + * \brief Analyse string (specificate for NMEA sentences) + */ +int nmea_scanf(const char *buff, int buff_sz, const char *format, ...) +{ + const char *beg_tok; + const char *end_buf = buff + buff_sz; + + va_list arg_ptr; + int tok_type = NMEA_TOKS_COMPARE; + int width = 0; + const char *beg_fmt = 0; + int snum = 0, unum = 0; + + int tok_count = 0; + void *parg_target; + + va_start(arg_ptr, format); + + for(; *format && buff < end_buf; ++format) + { + switch(tok_type) + { + case NMEA_TOKS_COMPARE: + if('%' == *format) + tok_type = NMEA_TOKS_PERCENT; + else if(*buff++ != *format) + goto fail; + break; + case NMEA_TOKS_PERCENT: + width = 0; + beg_fmt = format; + tok_type = NMEA_TOKS_WIDTH; + case NMEA_TOKS_WIDTH: + if(isdigit(*format)) + break; + { + tok_type = NMEA_TOKS_TYPE; + if(format > beg_fmt) + width = nmea_atoi(beg_fmt, (int)(format - beg_fmt), 10); + } + case NMEA_TOKS_TYPE: + beg_tok = buff; + + if(!width && ('c' == *format || 'C' == *format) && *buff != format[1]) + width = 1; + + if(width) + { + if(buff + width <= end_buf) + buff += width; + else + goto fail; + } + else + { + if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff)))) + buff = end_buf; + } + + if(buff > end_buf) + goto fail; + + tok_type = NMEA_TOKS_COMPARE; + tok_count++; + + parg_target = 0; width = (int)(buff - beg_tok); + + switch(*format) + { + case 'c': + case 'C': + parg_target = (void *)va_arg(arg_ptr, char *); + if(width && 0 != (parg_target)) + *((char *)parg_target) = *beg_tok; + break; + case 's': + case 'S': + parg_target = (void *)va_arg(arg_ptr, char *); + if(width && 0 != (parg_target)) + { + memcpy(parg_target, beg_tok, width); + ((char *)parg_target)[width] = '\0'; + } + break; + case 'f': + case 'g': + case 'G': + case 'e': + case 'E': + parg_target = (void *)va_arg(arg_ptr, float *); + if(width && 0 != (parg_target)) + *((float *)parg_target) = nmea_atof(beg_tok, width); + break; + }; + + if(parg_target) + break; + if(0 == (parg_target = (void *)va_arg(arg_ptr, int *))) + break; + if(!width) + break; + + switch(*format) + { + case 'd': + case 'i': + snum = nmea_atoi(beg_tok, width, 10); + memcpy(parg_target, &snum, sizeof(int)); + break; + case 'u': + unum = nmea_atoi(beg_tok, width, 10); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + case 'x': + case 'X': + unum = nmea_atoi(beg_tok, width, 16); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + case 'o': + unum = nmea_atoi(beg_tok, width, 8); + memcpy(parg_target, &unum, sizeof(unsigned int)); + break; + default: + goto fail; + }; + + break; + }; + } + +fail: + + va_end(arg_ptr); + + return tok_count; +} diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c new file mode 100644 index 000000000..771dfbd6c --- /dev/null +++ b/apps/gps/ubx.c @@ -0,0 +1,863 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol implementation */ + +#include "ubx.h" +#include <sys/prctl.h> +#include <poll.h> +#include <arch/board/up_hrt.h> +#include <uORB/uORB.h> +#include <string.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <mavlink/mavlink_log.h> + +#define UBX_HEALTH_SUCCESS_COUNTER_LIMIT 2 +#define UBX_HEALTH_FAIL_COUNTER_LIMIT 2 + +#define UBX_BUFFER_SIZE 1000 + +extern bool gps_mode_try_all; +extern bool gps_mode_success; +extern bool terminate_gps_thread; +extern bool gps_baud_try_all; +extern bool gps_verbose; +extern int current_gps_speed; + +pthread_mutex_t *ubx_mutex; +gps_bin_ubx_state_t *ubx_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) +{ + ubx_state->ck_a = 0; + ubx_state->ck_b = 0; + ubx_state->rx_count = 0; + ubx_state->decode_state = UBX_DECODE_UNINIT; + ubx_state->message_class = CLASS_UNKNOWN; + ubx_state->message_id = ID_UNKNOWN; + ubx_state->payload_size = 0; + ubx_state->print_errors = false; +} + +void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b) +{ + *(ck_a) = *(ck_a) + b; + *(ck_b) = *(ck_b) + *(ck_a); +} + + + + + +int ubx_parse(uint8_t b, char *gps_rx_buffer) +{ +// printf("b=%x\n",b); + if (ubx_state->decode_state == UBX_DECODE_UNINIT) { + + if (b == 0xb5) { + ubx_state->decode_state = UBX_DECODE_GOT_SYNC1; + } + + } else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) { + if (b == 0x62) { + ubx_state->decode_state = UBX_DECODE_GOT_SYNC2; + + } else { + // Second start symbol was wrong, reset state machine + ubx_decode_init(); + } + + } else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC2) { + // Add to checksum + ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); + + //check for known class + switch (b) { + case UBX_CLASS_NAV: //NAV + ubx_state->decode_state = UBX_DECODE_GOT_CLASS; + ubx_state->message_class = NAV; + break; + + case UBX_CLASS_RXM: //RXM + ubx_state->decode_state = UBX_DECODE_GOT_CLASS; + ubx_state->message_class = RXM; + break; + + default: //unknown class: reset state machine + ubx_decode_init(); + break; + } + + } else if (ubx_state->decode_state == UBX_DECODE_GOT_CLASS) { + // Add to checksum + ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); + + //depending on class look for message id + switch (ubx_state->message_class) { + case NAV: + switch (b) { + case UBX_MESSAGE_NAV_POSLLH: //NAV-POSLLH: Geodetic Position Solution + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = NAV_POSLLH; + break; + + case UBX_MESSAGE_NAV_SOL: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = NAV_SOL; + break; + + case UBX_MESSAGE_NAV_TIMEUTC: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = NAV_TIMEUTC; + break; + + case UBX_MESSAGE_NAV_DOP: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = NAV_DOP; + break; + + case UBX_MESSAGE_NAV_SVINFO: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = NAV_SVINFO; + break; + + case UBX_MESSAGE_NAV_VELNED: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = NAV_VELNED; + break; + + default: //unknown class: reset state machine, should not happen + ubx_decode_init(); + break; + } + + break; + + case RXM: + switch (b) { + case UBX_MESSAGE_RXM_SVSI: + ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID; + ubx_state->message_id = RXM_SVSI; + break; + + default: //unknown class: reset state machine, should not happen + ubx_decode_init(); + break; + } + + break; + + default: //should not happen + ubx_decode_init(); + break; + } + + } else if (ubx_state->decode_state == UBX_DECODE_GOT_MESSAGEID) { + // Add to checksum + ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); + + ubx_state->payload_size = b; + ubx_state->decode_state = UBX_DECODE_GOT_LENGTH1; + + } else if (ubx_state->decode_state == UBX_DECODE_GOT_LENGTH1) { + // Add to checksum + ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); + + ubx_state->payload_size += b << 8; + ubx_state->decode_state = UBX_DECODE_GOT_LENGTH2; + + } else if (ubx_state->decode_state == UBX_DECODE_GOT_LENGTH2) { + uint8_t ret = 0; + + // Add to checksum if not yet at checksum byte + if (ubx_state->rx_count < ubx_state->payload_size) ubx_checksum(b, &(ubx_state->ck_a), &(ubx_state->ck_b)); + + // Fill packet buffer + gps_rx_buffer[ubx_state->rx_count] = b; + + //if whole payload + checksum is in buffer: + if (ubx_state->rx_count >= ubx_state->payload_size + 1) { + //convert to correct struct + switch (ubx_state->message_id) { //this enum is unique for all ids --> no need to check the class + case NAV_POSLLH: { +// printf("GOT NAV_POSLLH MESSAGE\n"); + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) gps_rx_buffer; + + //Check if checksum is valid and the store the gps information + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { + ubx_gps->lat = packet->lat; + ubx_gps->lon = packet->lon; + ubx_gps->alt = packet->height_msl; + + ubx_gps->counter_pos_valid++; + + ubx_gps->timestamp = hrt_absolute_time(); + ubx_gps->counter++; + + pthread_mutex_lock(ubx_mutex); + ubx_state->last_message_timestamps[NAV_POSLLH - 1] = hrt_absolute_time(); + pthread_mutex_unlock(ubx_mutex); + ret = 1; + + } else { + if (gps_verbose) printf("[gps] NAV_POSLLH: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) gps_rx_buffer; + + //Check if checksum is valid and the store the gps information + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { + + ubx_gps->fix_type = packet->gpsFix; + + ubx_gps->timestamp = hrt_absolute_time(); + ubx_gps->counter++; + + + pthread_mutex_lock(ubx_mutex); + ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time(); + pthread_mutex_unlock(ubx_mutex); + ret = 1; + + } else { + if (gps_verbose) printf("[gps] NAV_SOL: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + + case NAV_DOP: { +// printf("GOT NAV_DOP MESSAGE\n"); + gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) gps_rx_buffer; + + //Check if checksum is valid and the store the gps information + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { + + ubx_gps->eph = packet->hDOP; + ubx_gps->epv = packet->vDOP; + + ubx_gps->timestamp = hrt_absolute_time(); + ubx_gps->counter++; + + + pthread_mutex_lock(ubx_mutex); + ubx_state->last_message_timestamps[NAV_DOP - 1] = hrt_absolute_time(); + pthread_mutex_unlock(ubx_mutex); + ret = 1; + + } else { + if (gps_verbose) printf("[gps] NAV_DOP: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) gps_rx_buffer; + + //Check if checksum is valid and the store the gps information + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + + time_t epoch = mktime(&timeinfo); + +// printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, packet->time_nanoseconds); + + + + ubx_gps->time_gps_usec = (uint64_t)epoch * 1e6; //TODO: test this + ubx_gps->time_gps_usec += packet->time_nanoseconds * 1e-3; + + ubx_gps->timestamp = hrt_absolute_time(); + ubx_gps->counter++; + + + pthread_mutex_lock(ubx_mutex); + ubx_state->last_message_timestamps[NAV_TIMEUTC - 1] = hrt_absolute_time(); + pthread_mutex_unlock(ubx_mutex); + ret = 1; + + } else { + if (gps_verbose) printf("\t[gps] NAV_TIMEUTC: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message + const int length_part1 = 8; + char gps_rx_buffer_part1[length_part1]; + memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) gps_rx_buffer_part1; + + //read checksum + const int length_part3 = 2; + char gps_rx_buffer_part3[length_part3]; + memcpy(gps_rx_buffer_part3, &(gps_rx_buffer[ubx_state->rx_count - 1]), length_part3); + gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) gps_rx_buffer_part3; + + //Check if checksum is valid and then store the gps information + if (ubx_state->ck_a == packet_part3->ck_a && ubx_state->ck_b == packet_part3->ck_b) { + //definitions needed to read numCh elements from the buffer: + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char gps_rx_buffer_part2[length_part2]; //for temporal storage + + + int i; + + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* Get satellite information from the buffer */ + + memcpy(gps_rx_buffer_part2, &(gps_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) gps_rx_buffer_part2; + + + /* Write satellite information in the global storage */ + + ubx_gps->satellite_prn[i] = packet_part2->svid; + + //if satellite information is healthy store the data + uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + + if (!unhealthy) { + + if ((packet_part2->flags) & 1) { //flags is a bitfield + ubx_gps->satellite_used[i] = 1; + + } else { + ubx_gps->satellite_used[i] = 0; + } + + ubx_gps->satellite_snr[i] = packet_part2->cno; + ubx_gps->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + ubx_gps->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + + } else { + ubx_gps->satellite_used[i] = 0; + ubx_gps->satellite_snr[i] = 0; + ubx_gps->satellite_elevation[i] = 0; + ubx_gps->satellite_azimuth[i] = 0; + } + + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + ubx_gps->satellite_prn[i] = 0; + ubx_gps->satellite_used[i] = 0; + ubx_gps->satellite_snr[i] = 0; + ubx_gps->satellite_elevation[i] = 0; + ubx_gps->satellite_azimuth[i] = 0; + } + + /* set flag if any sat info is available */ + if (!packet_part1->numCh > 0) { + ubx_gps->satellite_info_available = 1; + + } else { + ubx_gps->satellite_info_available = 0; + } + + ubx_gps->timestamp = hrt_absolute_time(); + ubx_gps->counter++; + + + pthread_mutex_lock(ubx_mutex); + ubx_state->last_message_timestamps[NAV_SVINFO - 1] = hrt_absolute_time(); + pthread_mutex_unlock(ubx_mutex); + ret = 1; + + } else { + if (gps_verbose) printf("\t[gps] NAV_SVINFO: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + + case NAV_VELNED: { +// printf("GOT NAV_VELNED MESSAGE\n"); + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) gps_rx_buffer; + + //Check if checksum is valid and the store the gps information + if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) { + + ubx_gps->vel = (uint16_t)packet->speed; + ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3); + + ubx_gps->timestamp = hrt_absolute_time(); + ubx_gps->counter++; + + + pthread_mutex_lock(ubx_mutex); + ubx_state->last_message_timestamps[NAV_VELNED - 1] = hrt_absolute_time(); + pthread_mutex_unlock(ubx_mutex); + ret = 1; + + } else { + if (gps_verbose) printf("[gps] NAV_VELNED: checksum invalid\n"); + + ret = 0; + } + + // Reset state machine to decode next packet + ubx_decode_init(); + return ret; + + break; + } + + case RXM_SVSI: { +// printf("GOT RXM_SVSI MESSAGE\n"); + const int length_part1 = 7; + char gps_rx_buffer_part1[length_part1]; + memcpy(gps_rx_buffer_part1, gps_rx_buffer, length_part1); + gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) gps_rx_buffer_part1; + + //Check if checksum is valid and the store the gps information + if (ubx_state->ck_a == gps_rx_buffer[ubx_state->rx_count - 1] && ubx_state->ck_b == gps_rx_buffer[ubx_state->rx_count]) { + + ubx_gps->satellites_visible = packet->numVis; + + ubx_gps->timestamp = hrt_absolute_time(); + ubx_gps->counter++; + + + pthread_mutex_lock(ubx_mutex); + ubx_state->last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); + pthread_mutex_unlock(ubx_mutex); + ret = 1; + + } else { + if (gps_verbose) printf("[gps] RXM_SVSI: 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(); + + break; + } + } + + (ubx_state->rx_count)++; + + + + } + + + return 0; // no valid packet found + +} + +void calculate_ubx_checksum(uint8_t *message, uint8_t length) +{ + uint8_t ck_a = 0; + uint8_t ck_b = 0; + + int i; + + for (i = 2; i < length - 2; i++) { + ck_a = ck_a + message[i]; + ck_b = ck_b + ck_a; + } + + 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); + + //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); + + usleep(100000); + + //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); + + //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); + + //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); + + //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); + + + //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; +} + + + +int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size) +{ + + uint8_t ret = 0; + uint8_t c; + int rx_count = 0; + int gpsRxOverflow = 0; + + struct pollfd fds; + fds.fd = fd; + 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); + 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. + gpsRxOverflow++; + rx_count = 0; + ubx_decode_init(); + + if (gps_verbose) printf("[gps] Buffer full\n"); + + } else { + //gps_rx_buffer[rx_count] = c; + rx_count++; + + } + + int msg_read = ubx_parse(c, gps_rx_buffer); + + if (msg_read > 0) { + // printf("Found sequence\n"); + break; + } + + } else { + break; + } + + } else { + break; + } + + } + + return ret; +} + +int write_config_message_ubx(uint8_t *message, size_t length, int fd) +{ + //calculate and write checksum to the end + uint8_t ck_a = 0; + uint8_t ck_b = 0; + + int i; + + for (i = 2; i < length; i++) { + ck_a = ck_a + message[i]; + ck_b = ck_b + ck_a; + } + +// printf("[%x,%x]\n", ck_a, ck_b); + + int result_write = write(fd, message, length); + result_write += write(fd, &ck_a, 1); + result_write += write(fd, &ck_b, 1); + + return (result_write != length + 2); //return 0 as success + +} + +void *ubx_watchdog_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "gps ubx watchdog", getpid()); + + + /* Retrieve file descriptor */ + int fd = *((int *)arg); + + /* GPS watchdog error message skip counter */ + + bool ubx_healthy = false; + + uint8_t ubx_fail_count = 0; + uint8_t ubx_success_count = 0; + bool once_ok = false; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + while (1) { + /* if some values are to old reconfigure gps */ + int i; + pthread_mutex_lock(ubx_mutex); + bool all_okay = true; + uint64_t timestamp_now = hrt_absolute_time(); + + for (i = 0; i < UBX_NO_OF_MESSAGES; i++) { +// printf("timestamp_now=%llu\n", timestamp_now); +// printf("last_message_timestamps=%llu\n", ubx_state->last_message_timestamps[i]); + if (timestamp_now - ubx_state->last_message_timestamps[i] > UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS) { +// printf("Warning: GPS ubx message %d not received for a long time\n", i); + all_okay = false; + } + } + + pthread_mutex_unlock(ubx_mutex); + + if (!all_okay) { + /* gps error */ + ubx_fail_count++; +// if (err_skip_counter == 0) +// { +// printf("GPS Watchdog detected gps not running or having problems\n"); +// err_skip_counter = 20; +// } +// err_skip_counter--; +// printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok); + + + /* If we have too many failures and another mode or baud should be tried, exit... */ + if ((gps_mode_try_all == true || gps_baud_try_all == true) && (ubx_fail_count >= UBX_HEALTH_FAIL_COUNTER_LIMIT) && (ubx_healthy == false) && once_ok == false) { + if (gps_verbose) printf("[gps] Connection attempt failed, no UBX module found\r\n"); + + gps_mode_success = false; + break; + } + + if (ubx_healthy && ubx_fail_count == UBX_HEALTH_FAIL_COUNTER_LIMIT) { + printf("[gps] ERROR: UBX GPS module stopped responding\r\n"); + // global_data_send_subsystem_info(&ubx_present_enabled); + mavlink_log_critical(mavlink_fd, "[gps] UBX module stopped responding\n"); + ubx_healthy = false; + ubx_success_count = 0; + } + + /* trying to reconfigure the gps configuration */ + configure_gps_ubx(fd); + fflush(stdout); + sleep(1); + + } else { + /* gps healthy */ + ubx_success_count++; + + if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) { + printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed); + // global_data_send_subsystem_info(&ubx_present_enabled_healthy); + mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n"); + ubx_healthy = true; + ubx_fail_count = 0; + once_ok = true; + } + + } + + usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS); + } + + close(mavlink_fd); + + return NULL; +} + +void *ubx_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "gps ubx read", getpid()); + + /* Retrieve file descriptor */ + int fd = *((int *)arg); + + /* Initialize gps stuff */ + char gps_rx_buffer[UBX_BUFFER_SIZE]; + + + if (gps_verbose) printf("[gps] UBX protocol driver starting..\r\n"); + + //set parameters for ubx + + +// //ubx state +// gps_bin_ubx_state_t * ubx_state = malloc(sizeof(gps_bin_ubx_state_t)); +// printf("gps: ubx_state created\n"); +// ubx_decode_init(); +// ubx_state->print_errors = false; + + + /* set parameters for ubx */ + + if (configure_gps_ubx(fd) != 0) { + printf("[gps] Configuration of gps module to ubx failed\r\n"); + + /* Write shared variable sys_status */ + + //global_data_send_subsystem_info(&ubx_present); + + } else { + if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\r\n"); + + // XXX Shouldn't the system status only change if the module is known to work ok? + + /* Write shared variable sys_status */ + + //global_data_send_subsystem_info(&ubx_present_enabled); + } + + struct vehicle_gps_position_s ubx_gps_d = {0}; + + ubx_gps = &ubx_gps_d; + + int gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); + + while (1) { + /* Parse a message from the gps receiver */ + if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) { + /* publish new GPS position */ + orb_publish(ORB_ID(vehicle_gps_position), gps_pub, ubx_gps); + + } else { + /* de-advertise */ + close(gps_pub); + break; + } + } + + + return NULL; + +} diff --git a/apps/gps/ubx.h b/apps/gps/ubx.h new file mode 100644 index 000000000..5def1ed33 --- /dev/null +++ b/apps/gps/ubx.h @@ -0,0 +1,314 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol definitions */ + +#ifndef UBX_H_ +#define UBX_H_ + +#include <stdint.h> +#include <stdlib.h> +#include <stdio.h> +#include <time.h> +#include <math.h> +#include <stdbool.h> +#include <unistd.h> +#include <pthread.h> + + +//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 APPNAME "gps: ubx" + +//UBX Protocoll definitions (this is the subset of the messages that are parsed) +#define UBX_CLASS_NAV 0x01 +#define UBX_CLASS_RXM 0x02 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 +#define UBX_MESSAGE_NAV_POSLLH 0x02 +#define UBX_MESSAGE_NAV_SOL 0x06 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +#define UBX_MESSAGE_NAV_DOP 0x04 +#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_VELNED 0x12 +#define UBX_MESSAGE_RXM_SVSI 0x20 +#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_ACK_NAK 0x00 + + +// ************ +/** the structures of the binary packets */ +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t lon; // Longitude * 1e-7, deg + int32_t lat; // Latitude * 1e-7, deg + int32_t height; // Height above Ellipsoid, mm + int32_t height_msl; // Height above mean sea level, mm + uint32_t hAcc; // Horizontal Accuracy Estimate, mm + uint32_t vAcc; // Vertical Accuracy Estimate, mm + + uint8_t ck_a; + uint8_t ck_b; +} __attribute__((__packed__)) type_gps_bin_nav_posllh_packet; + +typedef type_gps_bin_nav_posllh_packet gps_bin_nav_posllh_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 + int16_t week; // GPS week (GPS time) + uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix + uint8_t flags; + int32_t ecefX; + int32_t ecefY; + int32_t ecefZ; + uint32_t pAcc; + int32_t ecefVX; + int32_t ecefVY; + int32_t ecefVZ; + uint32_t sAcc; + uint16_t pDOP; + uint8_t reserved1; + uint8_t numSV; + uint32_t reserved2; + + uint8_t ck_a; + uint8_t ck_b; +} __attribute__((__packed__)) type_gps_bin_nav_sol_packet; + +typedef type_gps_bin_nav_sol_packet gps_bin_nav_sol_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint32_t time_accuracy; //Time Accuracy Estimate, ns + int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC) + uint16_t year; //Year, range 1999..2099 (UTC) + uint8_t month; //Month, range 1..12 (UTC) + uint8_t day; //Day of Month, range 1..31 (UTC) + uint8_t hour; //Hour of Day, range 0..23 (UTC) + uint8_t min; //Minute of Hour, range 0..59 (UTC) + uint8_t sec; //Seconds of Minute, range 0..59 (UTC) + uint8_t valid_flag; //Validity Flags (see ubx documentation) + + + uint8_t ck_a; + uint8_t ck_b; +} __attribute__((__packed__)) type_gps_bin_nav_timeutc_packet; + +typedef type_gps_bin_nav_timeutc_packet gps_bin_nav_timeutc_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint16_t gDOP; //Geometric DOP (scaling 0.01) + uint16_t pDOP; //Position DOP (scaling 0.01) + uint16_t tDOP; //Time DOP (scaling 0.01) + uint16_t vDOP; //Vertical DOP (scaling 0.01) + uint16_t hDOP; //Horizontal DOP (scaling 0.01) + uint16_t nDOP; //Northing DOP (scaling 0.01) + uint16_t eDOP; //Easting DOP (scaling 0.01) + + + uint8_t ck_a; + uint8_t ck_b; +} __attribute__((__packed__)) type_gps_bin_nav_dop_packet; + +typedef type_gps_bin_nav_dop_packet gps_bin_nav_dop_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint8_t numCh; //Number of channels + uint8_t globalFlags; + uint16_t reserved2; + +} __attribute__((__packed__)) type_gps_bin_nav_svinfo_part1_packet; + +typedef type_gps_bin_nav_svinfo_part1_packet gps_bin_nav_svinfo_part1_packet_t; + +typedef struct { + uint8_t chn; //Channel number, 255 for SVs not assigned to a channel + uint8_t svid; //Satellite ID + uint8_t flags; + uint8_t quality; + uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz + int8_t elev; //Elevation in integer degrees + int16_t azim; //Azimuth in integer degrees + int32_t prRes; //Pseudo range residual in centimetres + +} __attribute__((__packed__)) type_gps_bin_nav_svinfo_part2_packet; + +typedef type_gps_bin_nav_svinfo_part2_packet gps_bin_nav_svinfo_part2_packet_t; + +typedef struct { + uint8_t ck_a; + uint8_t ck_b; + +} __attribute__((__packed__)) type_gps_bin_nav_svinfo_part3_packet; + +typedef type_gps_bin_nav_svinfo_part3_packet gps_bin_nav_svinfo_part3_packet_t; + + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t velN; //NED north velocity, cm/s + int32_t velE; //NED east velocity, cm/s + int32_t velD; //NED down velocity, cm/s + uint32_t speed; //Speed (3-D), cm/s + uint32_t gSpeed; //Ground Speed (2-D), cm/s + int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 + uint32_t sAcc; //Speed Accuracy Estimate, cm/s + uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 + + uint8_t ck_a; + uint8_t ck_b; +} __attribute__((__packed__)) type_gps_bin_nav_velned_packet; + +typedef type_gps_bin_nav_velned_packet gps_bin_nav_velned_packet_t; + +typedef struct { + int32_t time_milliseconds; // Measurement integer millisecond GPS time of week + int16_t week; //Measurement GPS week number + uint8_t numVis; //Number of visible satellites + + //... rest of package is not used in this implementation + +} __attribute__((__packed__)) type_gps_bin_rxm_svsi_packet; + +typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgId; + + uint8_t ck_a; + uint8_t ck_b; +} __attribute__((__packed__)) type_gps_bin_ack_ack_packet; + +typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgId; + + uint8_t ck_a; + uint8_t ck_b; +} __attribute__((__packed__)) type_gps_bin_ack_nak_packet; + +typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t; + + +// END the structures of the binary packets +// ************ + +enum UBX_MESSAGE_CLASSES { + CLASS_UNKNOWN = 0, + NAV = 1, + RXM = 2, + ACK = 3 +}; + +enum UBX_MESSAGE_IDS { + //these numbers do NOT correspond to the message id numbers of the ubx protocol + ID_UNKNOWN = 0, + NAV_POSLLH = 1, + NAV_SOL = 2, + NAV_TIMEUTC = 3, + NAV_DOP = 4, + NAV_SVINFO = 5, + NAV_VELNED = 6, + RXM_SVSI = 7 +}; + +enum UBX_DECODE_STATES { + UBX_DECODE_UNINIT = 0, + UBX_DECODE_GOT_SYNC1 = 1, + UBX_DECODE_GOT_SYNC2 = 2, + UBX_DECODE_GOT_CLASS = 3, + UBX_DECODE_GOT_MESSAGEID = 4, + UBX_DECODE_GOT_LENGTH1 = 5, + UBX_DECODE_GOT_LENGTH2 = 6 +}; + +typedef struct { + union { + uint16_t ck; + struct { + uint8_t ck_a; + uint8_t ck_b; + }; + }; + enum UBX_DECODE_STATES decode_state; + bool print_errors; + int16_t rx_count; + uint16_t payload_size; + + enum UBX_MESSAGE_CLASSES message_class; + enum UBX_MESSAGE_IDS message_id; + uint64_t last_message_timestamps[UBX_NO_OF_MESSAGES]; + +} __attribute__((__packed__)) type_gps_bin_ubx_state; + +typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; + +extern pthread_mutex_t *ubx_mutex; +extern gps_bin_ubx_state_t *ubx_state; + +void ubx_decode_init(void); + +void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b); + + + +int ubx_parse(uint8_t b, char *gps_rx_buffer); + +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); + +void calculate_ubx_checksum(uint8_t *message, uint8_t length); + +void *ubx_watchdog_loop(void *arg); + +void *ubx_loop(void *arg); + + +#endif /* UBX_H_ */ diff --git a/apps/include/apps.h b/apps/include/apps.h new file mode 100644 index 000000000..520128203 --- /dev/null +++ b/apps/include/apps.h @@ -0,0 +1,149 @@ +/**************************************************************************** + * apps/include/apps.h + * + * Copyright(C) 2011 Uros Platise. All rights reserved. + * Author: Uros Platise <uros.platise@isotel.eu> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_INCLUDE_APPS_H +#define __APPS_INCLUDE_APPS_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <sys/types.h> +#include <stdint.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct namedapp_s +{ + const char *name; /* Invocation name and as seen under /sbin/ */ + int priority; /* Use: SCHED_PRIORITY_DEFAULT */ + int stacksize; /* Desired stack size */ + main_t main; /* Entry point: main(int argc, char *argv[]) */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* The "bindir" is file system that supports access to the named applications. + * It is typically mounted under /bin. + */ + +#ifdef CONFIG_APPS_BINDIR +struct mountpt_operations; +extern const struct mountpt_operations binfs_operations; +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Name: namedapp_isavail + * + * Description: + * Checks for availabiliy of application registerred during compile time. + * + * Input Parameter: + * filename - Name of the linked-in binary to be started. + * + * Returned Value: + * This is an end-user function, so it follows the normal convention: + * Returns index of builtin application. If it is not found then it + * returns -1 (ERROR) and sets errno appropriately. + * + ****************************************************************************/ + +EXTERN int namedapp_isavail(FAR const char *appname); + +/**************************************************************************** + * Name: namedapp_getname + * + * Description: + * Returns pointer to a name of built-in application pointed by the + * index. + * + * Input Parameter: + * index, from 0 and on ... + * + * Returned Value: + * Returns valid pointer pointing to the app name if index is valid. + * Otherwise NULL is returned. + * + ****************************************************************************/ + +EXTERN const char *namedapp_getname(int index); + +/**************************************************************************** + * Name: exec_namedapp + * + * Description: + * Executes builtin named application registered during compile time. + * New application is run in a separate task context (and thread). + * + * Input Parameter: + * filename - Name of the linked-in binary to be started. + * argv - Argument list + * + * Returned Value: + * This is an end-user function, so it follows the normal convention: + * Returns the PID of the exec'ed module. On failure, it.returns + * -1 (ERROR) and sets errno appropriately. + * + ****************************************************************************/ + +EXTERN int exec_namedapp(FAR const char *appname, FAR const char **argv); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __APPS_INCLUDE_APPS_H */ diff --git a/apps/include/ftpc.h b/apps/include/ftpc.h new file mode 100644 index 000000000..f9a73676a --- /dev/null +++ b/apps/include/ftpc.h @@ -0,0 +1,225 @@ +/**************************************************************************** + * apps/include/ftpc.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_INCLUDE_FTPC_H +#define __APPS_INCLUDE_FTPC_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdbool.h> +#include <signal.h> +#include <time.h> + +#include <netinet/in.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ + +#ifndef CONFIG_FTP_DEFTIMEO +# define CONFIG_FTP_DEFTIMEO 30 +#endif + +#ifndef CONFIG_FTP_ANONPWD +# define CONFIG_FTP_ANONPWD "" +#endif + +#ifndef CONFIG_FTP_DEFPORT +# define CONFIG_FTP_DEFPORT 21 +#endif + +#ifndef CONFIG_FTP_MAXREPLY +# define CONFIG_FTP_MAXREPLY 256 +#endif + +#ifndef CONFIG_FTP_TMPDIR +# define CONFIG_FTP_TMPDIR "/tmp" +#endif + +#ifndef CONFIG_FTP_BUFSIZE +# define CONFIG_FTP_BUFSIZE 1024 +#endif + +#ifndef CONFIG_FTP_MAXPATH +# define CONFIG_FTP_MAXPATH 256 +#endif + +#ifndef CONFIG_FTP_SIGNAL +# define CONFIG_FTP_SIGNAL SIGUSR1 +#endif + +/* Interface arguments ******************************************************/ +/* These definitions describe how a put operation should be performed */ + +#define FTPC_PUT_NORMAL 0 /* Just PUT the file on the server */ +#define FTPC_PUT_APPEND 1 /* Append file to an existing file on the server */ +#define FTPC_PUT_UNIQUE 2 /* Create a uniquely named file on the server */ +#define FTPC_PUT_RESUME 3 /* Resume a previously started PUT transfer */ + +/* These definitions describe how a get operation should be performed */ + +#define FTPC_GET_NORMAL 0 /* Just GET the file from the server */ +#define FTPC_GET_APPEND 1 /* Append new file to an existing file */ +#define FTPC_GET_RESUME 3 /* Resume a previously started GET transfer */ + +/* Transfer mode encoding */ + +#define FTPC_XFRMODE_UNKNOWN 0 /* Nothing has been transferred yet */ +#define FTPC_XFRMODE_ASCII 1 /* Last transfer was ASCII mode */ +#define FTPC_XFRMODE_BINARY 2 /* Last transfer was binary mode */ + +/**************************************************************************** + * Public Types + ****************************************************************************/ +/* This "handle" describes the FTP session */ + +typedef FAR void *SESSION; + +/* This structure provides information to connect to a host FTP server. + * + * addr - The IPv4 address of the FTP server (or the proxy) for the FTP + * server. + * port - The port number on the FTP server to connect to (in host byte + * order). This is usually port 21 for FTP. You may set this + * value to zero to let FTPC select the default port number for + * you (it will use CONFIG_FTP_DEFPORT). + */ + +struct ftpc_connect_s +{ + struct in_addr addr; /* Server/proxy IP address */ + uint16_t port; /* Server/proxy port number (usually 21) in network order */ +}; + +/* This structure provides FTP login information */ + +struct ftpc_login_s +{ + FAR const char *uname; /* Login uname */ + FAR const char *pwd; /* Login pwd */ + FAR const char *rdir; /* Initial remote directory */ + bool pasv; /* true: passive connection mode */ +}; + +/* This structure describes one simple directory listing. The directory + * list container as well the individual filename strings are allocated. + * The number of names in tha actual allocated array is variable, given + * by the nnames field. + * + * Since the structure and file names are allocated, they must be freed + * by calling ftpc_dirfree() when they are no longer needed. Allocated + * name strings maby be "stolen" from the array but the pointer int the + * array should be nullified so that the string is not freed by + * ftpc_dirfree(). + */ + +struct ftpc_dirlist_s +{ + unsigned int nnames; /* Number of entries in name[] array */ + FAR char *name[1]; /* Filename with absolute path */ +}; + +#define SIZEOF_FTPC_DIRLIST(n) \ + (sizeof(struct ftpc_dirlist_s) + ((n)-1)*sizeof(FAR char *)) + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/* Connection management ****************************************************/ + +EXTERN SESSION ftpc_connect(FAR struct ftpc_connect_s *server); +EXTERN void ftpc_disconnect(SESSION handle); + +/* FTP commands *************************************************************/ + +EXTERN int ftpc_login(SESSION handle, FAR struct ftpc_login_s *login); +EXTERN int ftpc_quit(SESSION handle); + +EXTERN int ftpc_chdir(SESSION handle, FAR const char *path); +EXTERN FAR char *ftpc_rpwd(SESSION handle); +EXTERN int ftpc_cdup(SESSION handle); +EXTERN int ftpc_mkdir(SESSION handle, FAR const char *path); +EXTERN int ftpc_rmdir(SESSION handle, FAR const char *path); + +EXTERN int ftpc_unlink(SESSION handle, FAR const char *path); +EXTERN int ftpc_chmod(SESSION handle, FAR const char *path, FAR const char *mode); +EXTERN int ftpc_rename(SESSION handle, FAR const char *oldname, FAR const char *newname); +EXTERN off_t ftpc_filesize(SESSION handle, FAR const char *path); +EXTERN time_t ftpc_filetime(SESSION handle, FAR const char *filename); + +EXTERN int ftpc_idle(SESSION handle, unsigned int idletime); +EXTERN int ftpc_noop(SESSION handle); +EXTERN int ftpc_help(SESSION handle, FAR const char *arg); + +/* Directory listings *******************************************************/ + +EXTERN FAR struct ftpc_dirlist_s *ftpc_listdir(SESSION handle, + FAR const char *dirpath); +EXTERN void ftpc_dirfree(FAR struct ftpc_dirlist_s *dirlist); + +/* File transfers ***********************************************************/ + +EXTERN int ftpc_getfile(SESSION handle, FAR const char *rname, + FAR const char *lname, uint8_t how, uint8_t xfrmode); +EXTERN int ftp_putfile(SESSION handle, FAR const char *lname, + FAR const char *rname, uint8_t how, uint8_t xfrmode); + +/* FTP response *************************************************************/ + +EXTERN FAR char *ftpc_response(SESSION handle); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __APPS_INCLUDE_FTPC_H */ diff --git a/apps/include/nsh.h b/apps/include/nsh.h new file mode 100644 index 000000000..4b5a3cd31 --- /dev/null +++ b/apps/include/nsh.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * apps/include/nsh.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_INCLUDE_NSH_H +#define __APPS_INCLUDE_NSH_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +#if CONFIG_RR_INTERVAL > 0 +# define SCHED_NSH SCHED_RR +#else +# define SCHED_NSH SCHED_FIFO +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_initialize + * + * Description: + * This nterfaces is used to initialize the NuttShell (NSH). + * nsh_initialize() should be called one during application start-up prior + * to executing either nsh_consolemain() or nsh_telnetstart(). + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ + +EXTERN void nsh_initialize(void); + +/**************************************************************************** + * Name: nsh_consolemain + * + * Description: + * This interfaces maybe to called or started with task_start to start a + * single an NSH instance that operates on stdin and stdout (/dev/console). + * This function does not return. + * + * Input Parameters: + * Standard task start-up arguements. These are not used. argc may be + * zero and argv may be NULL. + * + * Returned Values: + * This function does not normally return. exit() is usually called to + * terminate the NSH session. This function will return in the event of + * an error. In that case, a nonzero value is returned (1). + * + ****************************************************************************/ + +EXTERN int nsh_consolemain(int argc, char *argv[]); + +/* nsh_telnetstart() starts a telnet daemon that will allow multiple + * NSH connections via telnet. This function returns immediately after + * the daemon has been started. + */ + +EXTERN int nsh_telnetstart(void); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __APPS_INCLUDE_NSH_H */ diff --git a/apps/include/readline.h b/apps/include/readline.h new file mode 100644 index 000000000..647778210 --- /dev/null +++ b/apps/include/readline.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * apps/include/readline.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_INCLUDE_READLINE_H +#define __APPS_INCLUDE_READLINE_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: readline + * + * readline() reads in at most one less than 'buflen' characters from + * 'instream' and stores them into the buffer pointed to by 'buf'. + * Characters are echoed on 'outstream'. Reading stops after an EOF or a + * newline. If a newline is read, it is stored into the buffer. A null + * terminator is stored after the last character in the buffer. + * + * This version of realine assumes that we are reading and writing to + * a VT100 console. This will not work well if 'instream' or 'outstream' + * corresponds to a raw byte steam. + * + * This function is inspired by the GNU readline but is an entirely + * different creature. + * + * Input Parameters: + * buf - The user allocated buffer to be filled. + * buflen - the size of the buffer. + * instream - The stream to read characters from + * outstream - The stream to each characters to. + * + * Returned values: + * On success, the (positive) number of bytes transferred is returned. + * A length of zero would indicated an end of file condition. An failure, + * a negated errno value is returned. + * + **************************************************************************/ + +EXTERN ssize_t readline(FAR char *buf, int buflen, FILE *instream, FILE *outstream); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __APPS_INCLUDE_READLINE_H */ diff --git a/apps/include/tiff.h b/apps/include/tiff.h new file mode 100644 index 000000000..378af9fe0 --- /dev/null +++ b/apps/include/tiff.h @@ -0,0 +1,465 @@ +/************************************************************************************ + * apps/include/tiff.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * Reference: + * "TIFF, Revision 6.0, Final," June 3, 1992, Adobe Developers Association. + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __APPS_INCLUDE_TIFF_H +#define __APPS_INCLUDE_TIFF_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> +#include <sys/types.h> +#include <nuttx/nx/nxglib.h> + +/************************************************************************************ + * Pre-Processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +/* TIFF File Format Definitions *****************************************************/ +/* Values for the IFD field type */ + +#define IFD_FIELD_BYTE 1 /* 8-bit unsigned integer */ +#define IFD_FIELD_ASCII 2 /* 8-bit byte that contains a 7-bit ASCII code. + * The last byte must be NUL */ +#define IFD_FIELD_SHORT 3 /* 16-bit (2-byte) unsigned integer */ +#define IFD_FIELD_LONG 4 /* 32-bit (4-byte) unsigned integer */ +#define IFD_FIELD_RATIONAL 5 /* Two LONGs: the first represents the + * numerator of a fraction, the second the + * denominator */ +#define IFD_FIELD_SBYTE 6 /* An 8-bit signed (twos-complement) integer */ +#define IFD_FIELD_UNDEFINED 7 /* An 8-bit byte that may contain anything, + * depending on the definition of the field */ +#define IFD_FIELD_SSHORT 8 /* A 16-bit (2-byte) signed (twos-complement) + * integer */ +#define IFD_FIELD_SLONG 9 /* A 32-bit (4-byte) signed (twos-complement) + * integer */ +#define IFD_FIELD_SRATIONAL 10 /* Two SLONG’s: the first represents the + * numerator of a fraction, the second the + * denominator */ +#define IFD_FIELD_FLOAT 11 /* Single precision (4-byte) IEEE format */ +#define IFD_FIELD_DOUBLE 12 /* Double precision (8-byte) IEEE format */ + +/* Values for the IFD tag type */ + +#define IFD_TAG_NEWSUBFILETYPE 254 /* NewSubfileType, LONG */ +# define TAG_NEWSUBFILETYPE_REDUCED (1 << 0) /* Bit 0: Reduced resolution verson of image */ +# define TAG_NEWSUBFILETYPE_SINGLE (1 << 1) /* Bit 1: Single page of a multi-page image */ +# define TAG_NEWSUBFILETYPE_TRANSP (1 << 2) /* Bit 2: Defines a transparency mask for image */ +#define IFD_TAG_SUBFILETYPE 255 /* SubfileType, SHORT */ +# define TAG_SUBFILETYPE_FULL 1 /* Full-resolution image data */ +# define TAG_SUBFILETYPE_REDUCED 2 /* Reduced-resolution image data */ +# define TAG_SUBFILETYPE_SINGLE 3 /* Single page of a multi-page image */ +#define IFD_TAG_IMAGEWIDTH 256 /* ImageLength, SHORT or LONG (Required) */ +#define IFD_TAG_IMAGELENGTH 257 /* ImageWidth, SHORT or LONG (Required) */ +#define IFD_TAG_BITSPERSAMPLE 258 /* BitsPerSample, SHORT (Required + * in greyscale and pallette-color image files) */ +#define IFD_TAG_COMPRESSION 259 /* Compression, SHORT (Required) */ +# define TAG_COMP_NONE 1 /* No compression */ +# define TAG_COMP_CCITT 2 /* CCITT Group 3 1-Dimensional Modified Huffman + * run length encoding */ +# define TAG_COMP_T4 3 /* CCITT T.4 bi-level encoding */ +# define TAG_COMP_T6 4 /* CCITT T.6 bi-level encoding */ +# define TAG_COMP_LZW 5 /* LZW */ +# define TAG_COMP_JPEG 6 /* LZW */ +# define TAG_COMP_PACKBITS 32773 /* PackBits compression */ +#define IFD_TAG_PMI 262 /* PhotometricInterpretation, SHORT (Required) */ +# define TAG_PMI_WHITE 0 /* WhiteIsZero */ +# define TAG_PMI_BLACK 1 /* BlackIsZero */ +# define TAG_PMI_RGB 2 /* RGB */ +# define TAG_PMI_PALETTE 3 /* Palette color */ +# define TAG_PMI_TRANSP 4 /* Transparency mask */ +# define TAG_PMI_CMYK 5 /* CMYK */ +# define TAG_PMI_YCbCr 6 /* YCbCr */ +# define TAG_PMI_CIELAB 8 /* 1976 CIE L*a*b* */ +#define IFD_TAG_THRESHHOLDING 263 /* Threshholding, SHORT */ +# define TAG_THRESHHOLD_NONE 1 /* No dithering or halftoning has been applied */ +# define TAG_THRESHHOLD_ORDERED 2 /* Ordered dither or halftone technique has been applied */ +# define TAG_THRESHHOLD_RANDOM 3 /* Randomized process has been applied */ +#define IFD_TAG_CELLWIDTH 264 /* CellWidth, SHORT */ +#define IFD_TAG_CELLLENGTH 265 /* CellLength, SHORT */ +#define IFD_TAG_FILLORDER 266 /* FillOrder, SHORT */ +# define TAG_FILLORDER_HIGH 1 /* Lower column values are stored in the + * higher-order bits */ +# define TAG_FILLORDER_LOW 2 /* Lower column values are stored in the + * lower-order bits */ +#define IFD_TAG_DOCUMENTNAME 269 /* DocumentName, ASCII */ +#define IFD_TAG_IMAGEDESCRIPTION 270 /* ImageDescription, ASCII */ +#define IFD_TAG_MAKE 271 /* Make, ASCII */ +#define IFD_TAG_MODEL 272 /* Model, ASCII */ +#define IFD_TAG_STRIPOFFSETS 273 /* StripOffsets, SHORT or LONG (Required) */ +#define IFD_TAG_ORIENTATION 274 /* Orientation, SHORT */ +# define TAG_ORIENTATION_TL 1 /* (0,0)=top left */ +# define TAG_ORIENTATION_TR 2 /* (0,0)=top right */ +# define TAG_ORIENTATION_BR 3 /* (0,0)=bottom right */ +# define TAG_ORIENTATION_BL 4 /* (0,0)=bottom left */ +# define TAG_ORIENTATION_LT 5 /* (0,0)=left top */ +# define TAG_ORIENTATION_RT 6 /* (0,0)=right top */ +# define TAG_ORIENTATION_RB 7 /* (0,0)=right bottom */ +# define TAG_ORIENTATION_LB 8 /* (0,0)=left bottom */ +#define IFD_TAG_SAMPLESPERPIXEL 277 /* SamplesPerPixel, SHORT (Required in + * RGB full color files) */ +#define IFD_TAG_ROWSPERSTRIP 278 /* RowsPerStrip, SHORT or LONG (Required) */ +#define IFD_TAG_STRIPCOUNTS 279 /* StripByteCounts, SHORT or LONG (Required) */ +#define IFD_TAG_MINSAMPLEVALUE 280 /* MinSampleValue, SHORT */ +#define IFD_TAG_MAXSAMPLEVALUE 281 /* MaxSampleValue, SHORT */ +#define IFD_TAG_XRESOLUTION 282 /* XResolution, RATIONAL (Required) */ +#define IFD_TAG_YRESOLUTION 283 /* YResolution, RATIONAL (Required) */ +#define IFD_TAG_PLANARCONFIG 284 /* PlanarConfiguration, SHORT */ +# define TAG_PLCONFIG_CHUNKY 1 /* Chunky format */ +# define TAG_PLCONFIG_PLANAR 2 /* Planar format */ +#define IFD_TAG_PAGENAME 285 /* PageName, ASCII */ +#define IFD_TAG_XPOSITION 286 /* XPosition, RATIONAL */ +#define IFD_TAG_YPOSITION 287 /* YPosition, RATIONAL */ +#define IFD_TAG_FREEOFFSETS 288 /* FreeOffsets, LONG */ +#define IFD_TAG_FREEBYTECOUNTS 289 /* FreeByteCounts, LONG */ +#define IFD_TAG_GRAYRESPONSEUNIT 290 /* GrayResponseUnit, SHORT */ +# define TAG_GRAYRESPUNIT_10THS 1 /* Number represents tenths of a unit */ +# define TAG_GRAYRESPUNIT_100THS 2 /* Number represents hundredths of a unit */ +# define TAG_GRAYRESPUNIT_1KTHS 3 /* Number represents thousandths of a unit */ +# define TAG_GRAYRESPUNIT_10KTHS 4 /* Number represents ten-thousandths of a unit */ +# define TAG_GRAYRESPUNIT_100KTHS 5 /* Number represents hundred-thousandths of a unit */ +#define IFD_TAG_GRAYRESPONSECURVE 291 /* GrayResponseCurve, SHORT */ +#define IFD_TAG_T4OPTIONS 292 /* T4Options, LONG */ +# define TAG_T4OPTIONS_2D (1 << 0) /* 2-dimensional coding */ +# define TAG_T4OPTIONS_NONE (1 << 1) /* Uncompressed mode */ +# define TAG_T4OPTIONS_FILL (1 << 2) /* Fill bits have been added */ +#define IFD_TAG_T6OPTIONS 293 /* T6Options, LONG */ +# define TAG_T6OPTIONS_NONE (1 << 1) /* Uncompressed mode allowed */ +#define IFD_TAG_RESUNIT 296 /* ResolutionUnit, SHORT (Required) */ +# define TAG_RESUNIT_NONE 1 /* No absolute unit of measurement */ +# define TAG_RESUNIT_INCH 2 /* Inch (default) */ +# define TAG_RESUNIT_CENTIMETER 3 /* Centimeter */ +#define IFD_TAG_PAGENUMBER 297 /* PageNumber, SHORT */ +#define IFD_TAG_TRANSFERFUNCTION 301 /* TransferFunction, SHORT */ +#define IFD_TAG_SOFTWARE 305 /* Software, ASCII */ +#define IFD_TAG_DATETIME 306 /* DateTime, ASCII */ +#define IFD_TAG_ARTIST 315 /* Artist, ASCII */ +#define IFD_TAG_HOSTCOMPUTER 316 /* HostComputer, ASCII */ +#define IFD_TAG_PREDICTOR 317 /* Predictor SHORT */ +# define TAG_PREDICTOR_NONE 1 /* No prediction scheme used before coding */ +# define TAG_PREDICTOR_HORIZ 2 /* Horizontal differencing */ +#define IFD_TAG_WHITEPOINT 318 /* WhitePoint, RATIONAL */ +#define IFD_TAG_PRIMARYCHROMA 319 /* PrimaryChromaticities, RATIONAL */ +#define IFD_TAG_COLORMAP 320 /* ColorMap, SHORT (Required in palette + * color image files) */ +#define IFD_TAG_HALFTONEHINTS 321 /* HalftoneHints, SHORT */ +#define IFD_TAG_TILEWIDTH 322 /* TileWidth, SHORT or LONG */ +#define IFD_TAG_TILELENGTH 323 /* TileLength, SHORT or LONG */ +#define IFD_TAG_TILEOFFSETS 324 /* TileOffsets, LONG */ +#define IFD_TAG_TILEBYTECOUNTS 325 /* TileByteCounts, SHORT or LONG */ +#define IFD_TAG_INKSET 332 /* InkSet, SHORT */ +# define TAG_INKSET_CMYK 1 /* CMYK */ +# define TAG_INKSET_OTHER 2 /* Not CMYK */ +#define IFD_TAG_INKNAMES 333 /* InkNames, ASCII */ +#define IFD_TAG_NUMBEROFINKS 334 /* NumberOfInks, SHORT */ +#define IFD_TAG_DOTRANGE 336 /* DotRange, BYTE or SHORT */ +#define IFD_TAG_TARGETPRINTER 337 /* TargetPrinter, ASCII */ +#define IFD_TAG_EXTRASAMPLES 338 /* ExtraSamples, SHORT */ +# define TAG_EXTSAMP_UNSPEC 0 /* Unspecified */ +# define TAG_EXTSAMP_ASSOCALPHA 1 /* Associated alpha data */ +# define TAG_EXTSAMP_UNASSALPHA 2 /* Unassociated alpha data */ +#define IFD_TAG_SAMPLEFORMAT 339 /* SampleFormat, SHORT */ +# define TAG_SAMPLEFMT_UNSIGED 1 /* Unsigned integer data */ +# define TAG_SAMPLEFMT_SIGNED 2 /* Two’s complement signed integer data */ +# define TAG_SAMPLEFMT_FLOAT 3 /* IEEE floating point data */ +# define TAG_SAMPLEFMT_UNDEFINED 4 /* Undefined data format */ +#define IFD_TAG_SMINSAMPLEVALUE 340 /* SMinSampleValue, type matches sample data */ +#define IFD_TAG_SMAXSAMPLEVALUE 341 /* SMaxSampleValue, type matches sample data */ +#define IFD_TAG_TRANSFERRANGE 342 /* TransferRange, SHORT */ +#define IFD_TAG_JPEGPROC 512 /* JPEGProc, SHORT */ +#define IFD_TAG_JPEGFMT 513 /* JPEGInterchangeFormat, LONG */ +#define IFD_TAG_JPEGLENGTH 514 /* JPEGInterchangeFormatLength, LONG */ +#define IFD_TAG_JPEGRESTART 515 /* JPEGRestartInterval, SHORT */ +#define IFD_TAG_JPEGLLPREDICTORS 517 /* JPEGLosslessPredictors, SHORT */ +#define IFD_TAG_JPEGPOINTXFORMS 518 /* JPEGPointTransforms, SHORT */ +#define IFD_TAG_JPEGQTABLES 519 /* JPEGQTables, LONG */ +#define IFD_TAG_JPEGDCTABLES 520 /* JPEGDCTables, LONG */ +#define IFD_TAG_JPEGACTABLES 521 /* JPEGACTables, LONG */ +#define IFD_TAG_YCbCrCOEFFS 529 /* YCbCrCoefficients, RATIONAL */ +#define IFD_TAG_YCbCrSUBSAMPLING 530 /* YCbCrSubSampling, SHORT */ +#define IFD_TAG_YCbCrPOSITIONING 531 /* YCbCrPositioning, SHORT */ +#define IFD_TAG_REFERENCEBW 532 /* ReferenceBlackWhite, RATIONAL */ +#define IFD_TAG_COPYRIGHT 33432 /* Copyright, ASCII */ + +/************************************************************************************ + * Public Types + ************************************************************************************/ +/* TIFF File Format Structure *******************************************************/ +/* "A TIFF file begins with an 8-byte image file header that points to an + * image file directory (IFD). An image file directory contains information + * about the image, as well as pointers to the actual image data." + */ + +struct tiff_header_s +{ + uint8_t order[2]; /* 0-1: Byte order: "II"=little endian, "MM"=big endian */ + uint8_t magic[2]; /* 2-3: 42 in appropriate byte order */ + uint8_t offset[4]; /* 4-7: Offset to the first IFD */ +}; +#define SIZEOF_TIFF_HEADER 8 + +/* "An Image File Directory (IFD) consists of a 2-byte count of the number + * of directory entries (i.e., the number of fields), followed by a sequence + * of 12-byte field entries, followed by a 4-byte offset of the next IFD (or + * 0 if none). + * + * Each 12-byte IFD entry has the following format: + */ + +struct tiff_ifdentry_s +{ + uint8_t tag[2]; /* 0-1: The Tag that identifies the field */ + uint8_t type[2]; /* 2-3 The field Type */ + uint8_t count[4]; /* 4-7: The number of values of the indicated type */ + uint8_t offset[4]; /* 8-11: The Value Offset (or the value itself) */ +}; +#define SIZEOF_IFD_ENTRY 12 + +/************************************************************************************/ +/* Structures needed to interface with the TIFF file creation library )and also + * structures used only internally by the TIFF file creation library). + */ + +/* This structure describes on strip in tmpfile2 */ + +struct tiff_strip_s +{ + uint32_t offset; /* Offset to the strip data in tmpfile1 */ + uint32_t count; /* Count of pixels in the strip */ +}; + +/* This structure is used only internally by the TIFF file creation library to + * manage file offsets. + */ + +struct tiff_filefmt_s +{ + uint16_t nifdentries; /* Number of IFD entries */ + uint16_t soifdoffset; /* Offset to StripOffset IFD entry */ + uint16_t sbcifdoffset; /* Offset to StripByteCount IFD entry */ + uint16_t valoffset; /* Offset to first values */ + uint16_t xresoffset; /* Offset to XResolution values */ + uint16_t yresoffset; /* Offset to yResolution values */ + uint16_t swoffset; /* Offset to Software string */ + uint16_t dateoffset; /* Offset to DateTime string */ + uint16_t sbcoffset; /* Offset to StripByteCount values */ +}; + +/* These type is used to hold information about the TIFF file under + * construction + */ + +struct tiff_info_s +{ + /* The first fields are used to pass information to the TIFF file creation + * logic via tiff_initialize(). + * + * Filenames. Three file names are required. (1) path to the final + * output file and (2) two paths to temporary files. One temporary file + * (tmpfile1) will be used to hold the strip image data and the other + * (tmpfile2) will be used to hold strip offset and count information. + * + * colorfmt - Specifies the form of the color data that will be provided + * in the strip data. These are the FB_FMT_* definitions + * provided in include/nuttx/fb.h. Only the following values + * are supported: + * + * FB_FMT_Y1 BPP=1, monochrome, 0=black + * FB_FMT_Y4 BPP=4, 4-bit greyscale, 0=black + * FB_FMT_Y8 BPP=8, 8-bit greyscale, 0=black + * FB_FMT_RGB16_565 BPP=16 R=6, G=6, B=5 + * FB_FMT_RGB24 BPP=24 R=8, G=8, B=8 + * + * rps - TIFF RowsPerStrip + * imgwidth - TIFF ImageWidth, Number of columns in the image + * imgheight - TIFF ImageLength, Number of rows in the image + */ + + FAR const char *outfile; /* Full path to the final output file name */ + FAR const char *tmpfile1; /* Full path to first temporary file */ + FAR const char *tmpfile2; /* Full path to second temporary file */ + + uint8_t colorfmt; /* See FB_FMT_* definitions in include/nuttx/fb.h */ + nxgl_coord_t rps; /* TIFF RowsPerStrip */ + nxgl_coord_t imgwidth; /* TIFF ImageWidth, Number of columns in the image */ + nxgl_coord_t imgheight; /* TIFF ImageLength, Number of rows in the image */ + + /* The caller must provide an I/O buffer as well. This I/O buffer will + * used for color conversions and as the intermediate buffer for copying + * files. The larger the buffer, the better the performance. + */ + + FAR uint8_t *iobuffer; /* IO buffer allocated by the caller */ + unsigned int iosize; /* The size of the I/O buffer in bytes */ + + /* The second set of fields are used only internally by the TIFF file + * creation logic. These fields must be set to zero initially by the + * caller of tiff_initialize(). User logic should not depend upon any + * definitions in the following -- they are subject to change without + * notice. They are only exposed here so that the caller can allocate + * memory for their storage. + */ + + uint8_t imgflags; /* Bit-encoded image flags */ + nxgl_coord_t nstrips; /* Number of strips in tmpfile3 */ + size_t pps; /* Pixels per strip */ + size_t bps; /* Bytes per strip */ + int outfd; /* outfile file descriptor */ + int tmp1fd; /* tmpfile1 file descriptor */ + int tmp2fd; /* tmpfile2 file descriptor */ + off_t outsize; /* Current size of outfile */ + off_t tmp1size; /* Current size of tmpfile1 */ + off_t tmp2size; /* Current size of tmpfile2 */ + + /* Points to an internal constant structure of file offsets */ + + FAR const struct tiff_filefmt_s *filefmt; +}; + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Name: tiff_initialize + * + * Description: + * Setup to create a new TIFF file. The overall steps to creating a TIFF file are + * as follows: + * + * 1) Create an initialize a struct tiff_info_s instance + * 2) Call tiff_initialize() to setup the file creation + * 3) Call tiff_addstrip() repeatedly to add strips to the graphic image + * 4) Call tiff_finalize() to complete the file creation. + * + * Input Parameters: + * info - A pointer to the caller allocated parameter passing/TIFF state instance. + * + * Returned Value: + * Zero (OK) on success. A negated errno value on failure. + * + ************************************************************************************/ + +EXTERN int tiff_initialize(FAR struct tiff_info_s *info); + +/************************************************************************************ + * Name: tiff_addstrip + * + * Description: + * Add an image data strip. The size of the strip in pixels must be equal to + * the RowsPerStrip x ImageWidth values that were provided to tiff_initialize(). + * + * Input Parameters: + * info - A pointer to the caller allocated parameter passing/TIFF state instance. + * buffer - A buffer containing a single row of data. + * + * Returned Value: + * Zero (OK) on success. A negated errno value on failure. + * + ************************************************************************************/ + +EXTERN int tiff_addstrip(FAR struct tiff_info_s *info, FAR const uint8_t *strip); + +/************************************************************************************ + * Name: tiff_finalize + * + * Description: + * Finalize the TIFF output file, completing the TIFF file creation steps. + * + * Input Parameters: + * info - A pointer to the caller allocated parameter passing/TIFF state instance. + * + * Returned Value: + * Zero (OK) on success. A negated errno value on failure. + * + ************************************************************************************/ + +EXTERN int tiff_finalize(FAR struct tiff_info_s *info); + +/************************************************************************************ + * Name: tiff_abort + * + * Description: + * Abort the TIFF file creation and create-up resources. + * + * Input Parameters: + * info - A pointer to the caller allocated parameter passing/TIFF state instance. + * + * Returned Value: + * None + * + ************************************************************************************/ + +EXTERN void tiff_abort(FAR struct tiff_info_s *info); + +/************************************************************************************ + * Name: tiff_put/get16/32 + * + * Description: + * Put and get 16 and 32 values in the correct byte order at the specified position. + * + * Input Parameters: + * dest - The location to store the multi-byte data (put only) + * src - The location to get the multi-byte data (get only) + * + * Returned Value: + * None (put) + * The extracted value (get) + * + ************************************************************************************/ + +EXTERN void tiff_put16(FAR uint8_t *dest, uint16_t value); +EXTERN void tiff_put32(FAR uint8_t *dest, uint32_t value); +EXTERN uint16_t tiff_get16(FAR uint8_t *dest); +EXTERN uint32_t tiff_get32(FAR uint8_t *dest); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __APPS_INCLUDE_TIFF_H */ diff --git a/apps/interpreters/Kconfig b/apps/interpreters/Kconfig new file mode 100644 index 000000000..34cbf2eee --- /dev/null +++ b/apps/interpreters/Kconfig @@ -0,0 +1,21 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +comment "Interpreters" + +source "$APPSDIR/interpreters/ficl/Kconfig" + +config PCODE + bool "Pascal p-code interpreter" + default n + ---help--- + Enable support for the Pascal p-code interpreter. See README file at misc/pascal + and also the README.txt file in the apps/interpreter directory. Use of this + configuration implies that you have performed the required installation of the + Pascal run-time code. + +if PCODE +endif + diff --git a/apps/interpreters/Make.defs b/apps/interpreters/Make.defs new file mode 100644 index 000000000..36ee7004d --- /dev/null +++ b/apps/interpreters/Make.defs @@ -0,0 +1,43 @@ +############################################################################ +# apps/interpreters/Make.defs +# Adds selected applications to apps/ build +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +if ($(CONFIG_PCODE),y) +CONFIGURED_APPS += interpreters/pcode +endif + +if ($(CONFIG_FICL),y) +CONFIGURED_APPS += interpreters/ficl +endif diff --git a/apps/interpreters/Makefile b/apps/interpreters/Makefile new file mode 100644 index 000000000..1ef9c7439 --- /dev/null +++ b/apps/interpreters/Makefile @@ -0,0 +1,70 @@ +############################################################################ +# apps/interpreters/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config # Current configuration + +# Sub-directories containing interpreter runtime + +SUBDIRS = pcode ficl + +# Create the list of installed runtime modules (INSTALLED_DIRS) + +define ADD_DIRECTORY +INSTALLED_DIRS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi} +endef + +$(foreach DIR, $(SUBDIRS), $(eval $(call ADD_DIRECTORY,$(DIR)))) + +all: nothing +.PHONY: nothing context depend clean distclean + +nothing: + +context: + +depend: + @for dir in $(INSTALLED_DIRS) ; do \ + $(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + +clean: + @for dir in $(INSTALLED_DIRS) ; do \ + $(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + +distclean: clean + @for dir in $(INSTALLED_DIRS) ; do \ + $(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done diff --git a/apps/interpreters/README.txt b/apps/interpreters/README.txt new file mode 100644 index 000000000..8cf7ce321 --- /dev/null +++ b/apps/interpreters/README.txt @@ -0,0 +1,67 @@ +apps/interpreters README file +============================= + +This apps/ directory is set aside to hold interpreters that may be +incorporated into NuttX. + +ficl +---- + + This is DIY port of Ficl (the "Forth Inspired Command Language"). See + http://ficl.sourceforge.net/. It is a "DIY" port because the Ficl source + is not in that directory, only an environment and instructions that will + let you build Ficl under NuttX. The rest is up to you. + +pcode +----- + + At present, only the NuttX Pascal add-on is supported. This NuttX add-on + must be downloaded separately (or is available in an SVN snapshot in the + misc/pascal directory). + + This Pascal add-on must be installed into the NuttX apps/ directory. After + unpacking the Pascal add-on package, an installation script and README.txt + instructions can be found at pascal/nuttx. + + INSTALL.sh -- The script that performs the operation. Usage: + + ./INSTALL.sh [-16|-32] <install-dir> + + If you are using this standard NuttX apps/ package, the correct + location for the <install-dir> is apps/interpreters. That is + where the examples and build logic will expect to find the pcode + sub-directory. + + Example: + + ./INSTALL.sh -16 $PWD/../../../apps/interpreters + + After installation, the NuttX apps/interpresters directory will contain + the following files + + pcode + |-- Makefile + |-- include + | `-- Common header files + |-- libboff + | `-- Pascal object format (POFF) library + `--insn + |-- include + | `-- model-specific header files + `-- prun + `-- model-specific source files + + pashello + + There is a simple Pascal example at apps/examples/pashello. This is the + standard "Hello, World!" example written in Pascal and interpreted from + Pascal P-Code at runtime. To use this example, place the following in + your appconfig file" + + # Path to example in apps/examples containing the user_start entry point + + CONFIGURED_APPS += examples/pashello + + # Path to the Pascal p-code runtime interpreter module + + CONFIGURED_APPS += interpreters/pcode diff --git a/apps/interpreters/ficl/Kconfig b/apps/interpreters/ficl/Kconfig new file mode 100644 index 000000000..1860a1591 --- /dev/null +++ b/apps/interpreters/ficl/Kconfig @@ -0,0 +1,16 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config FICL + bool "Ficl Forth interpreter" + default n + ---help--- + Enable support for the Ficl interpreter interpreter. See README.txt file in the + apps/interpreters/ficl directory. Use of this configuration assumes + that you have performed the required installation of the Ficl run-time code. + +if FICL +endif + diff --git a/apps/interpreters/ficl/Makefile b/apps/interpreters/ficl/Makefile new file mode 100644 index 000000000..183246b67 --- /dev/null +++ b/apps/interpreters/ficl/Makefile @@ -0,0 +1,118 @@ +############################################################################ +# apps/interpreters/ficl/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +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 + +# Include paths + +CFLAGS += ${shell $(INCDIR) $(INCDIROPT) "$(CC)" $(BUILDDIR)/$(FICL_SUBDIR) $(BUILDDIR)/src} + +# Source Files + +ASRCS = +CXXSRCS = +CSRCS = nuttx.c + +-include Make.srcs + +ASRCS += $(FICL_ASRCS) +CXXSRCS += $(FICL_CXXSRCS) +CSRCS += $(FICL_CSRCS) + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOT_DEPPATH = --dep-path . +SRC_DEPPATH = --dep-path src + +VPATH = src:$(FICL_SUBDIR) + +all: .built +.PHONY: debug context depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +debug: + @#echo "FICL: $(FICL_SUBDIR)" + @#echo "VPATH: $(VPATH)" + @#echo "CFLAGS: $(CFLAGS)" + +.built: debug $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: debug Makefile $(SRCS) + @$(MKDEP) $(ROOT_DEPPATH) $(SRC_DEPPATH) $(FICL_DEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/interpreters/ficl/README.txt b/apps/interpreters/ficl/README.txt new file mode 100755 index 000000000..bac9f3148 --- /dev/null +++ b/apps/interpreters/ficl/README.txt @@ -0,0 +1,42 @@ +apps/interpreter/README.txt +=========================== + +Ficl is a programming language interpreter designed to be embedded into +other systems as a command, macro, and development prototyping language. +Ficl is an acronym for "Forth Inspired Command Language". See +http://ficl.sourceforge.net/ + +Build Instructions +------------------ + +Disclaimer: This installation steps have only been exercised using Ficl +4.1.0. With new versions you will likely have to make some adjustments +to this instructtions or to the files within this directory. Think of this +information as "recommendations" -- not necessarily proven instructions. + +1. CD to apps/interpreters/ficl + +2. Download Ficl: http://sourceforge.net/projects/ficl/files/ + +3. Uznip the Ficl compressed file. + + For example, 'unzip ficl-4.1.0.zip' will leave the file + apps/interpreters/ficl/ficl-4.1.0 + +4. Configure to build Ficl in the apps/interpreters/ficl directory using + the configure.sh script. + + For example, './configure.sh ficl-4.1.0' will leave the Makefile + fragment 'Make.srcs' in the ficl build directory. + +5. Create your NuttX configuration. The appconfig file should include + (1) the path to your application code, and (2) the path to the Ficl + build directory. That latter would appear as the following line in + your appconfig file: + + CONFIGURED_APPS += interpreters/ficl + + 6. Configure and build NuttX. On successful completion, the Ficl objects + will be available in apps/libapps.a and that NuttX binary will be + linked against that file. Of course, Ficl will do nothing unless + you have written some application code that uses it! diff --git a/apps/interpreters/ficl/configure.sh b/apps/interpreters/ficl/configure.sh new file mode 100755 index 000000000..cadc1d48a --- /dev/null +++ b/apps/interpreters/ficl/configure.sh @@ -0,0 +1,47 @@ +#!/bin/bash + +USAGE="$0 <Ficl-dir>" + +FICLDIR=$1 +if [ -z "${FICLDIR}" ]; then + echo "Missing command line argument" + echo $USAGE + exit 1 +fi + +if [ ! -d "${FICLDIR}" ]; then + echo "Sub-directory ${FICLDIR} does not exist" + echo $USAGE + exit 1 +fi + +if [ ! -r "${FICLDIR}/Makefile" ]; then + echo "Readable ${FICLDIR}/Makefile does not exist" + echo $USAGE + exit 1 +fi + +OBJECTS=`grep "^OBJECTS" ${FICLDIR}/Makefile` +if [ -z "${OBJECTS}" ]; then + echo "No OBJECTS found in ${FICLDIR}/Makefile" + echo $USAGE + exit 1 +fi + +OBJLIST=`echo ${OBJECTS} | cut -d'=' -f2 | sed -e "s/unix\.o//g"` + +rm -f Make.srcs +echo "# apps/interpreters/ficl/Make.obs" >> Make.srcs +echo "# Auto-generated file.. Do not edit" >> Make.srcs +echo "" >> Make.srcs +echo "FICL_SUBDIR = ${1}" >> Make.srcs +echo "FICL_DEPPATH = --dep-path ${1}" >> Make.srcs + +unset CSRCS +for OBJ in ${OBJLIST}; do + SRC=`echo ${OBJ} | sed -e "s/\.o/\.c/g"` + CSRCS=${CSRCS}" ${SRC}" +done +echo "FICL_ASRCS = " >> Make.srcs +echo "FICL_CXXSRCS = " >> Make.srcs +echo "FICL_CSRCS = ${CSRCS}" >> Make.srcs diff --git a/apps/interpreters/ficl/src/nuttx.c b/apps/interpreters/ficl/src/nuttx.c new file mode 100644 index 000000000..16b3fa1db --- /dev/null +++ b/apps/interpreters/ficl/src/nuttx.c @@ -0,0 +1,65 @@ +#include <sys/stat.h> +#include <sys/statfs.h> + +#include <stdio.h> +#include <stdlib.h> +#include <errno.h> + +#include "ficl.h" + +void *ficlMalloc(size_t size) +{ + return malloc(size); +} + +void *ficlRealloc(void *p, size_t size) +{ + return realloc(p, size); +} + +void ficlFree(void *p) +{ + free(p); +} + +void ficlCallbackDefaultTextOut(ficlCallback *callback, char *message) +{ + FICL_IGNORE(callback); + if (message != NULL) + fputs(message, stdout); + else + fflush(stdout); + return; +} + +int ficlFileStatus(char *filename, int *status) +{ + struct stat statbuf; + if (stat(filename, &statbuf) == 0) + { + *status = statbuf.st_mode; + return 0; + } + *status = ENOENT; + return -1; +} + +long ficlFileSize(ficlFile *ff) +{ + struct stat statbuf; + if (ff == NULL) + return -1; + + statbuf.st_size = -1; + if (fstat(fileno(ff->f), &statbuf) != 0) + return -1; + + return statbuf.st_size; +} + +void ficlSystemCompilePlatform(ficlSystem *system) +{ + return; +} + + diff --git a/apps/interpreters/ficl/src/nuttx.h b/apps/interpreters/ficl/src/nuttx.h new file mode 100644 index 000000000..e44031066 --- /dev/null +++ b/apps/interpreters/ficl/src/nuttx.h @@ -0,0 +1,19 @@ +#include <stdint.h> + +typedef int8_t ficlInteger8; +typedef uint8_t ficlUnsigned8; +typedef int16_t ficlInteger16; +typedef uint16_t ficlUnsigned16; +typedef int32_t ficlInteger32; +typedef uint32_t ficlUnsigned32; + +typedef intptr_t ficlInteger; +typedef uintptr_t ficlUnsigned; +typedef float ficlFloat; + +#define FICL_PLATFORM_BASIC_TYPES (1) +#define FICL_PLATFORM_HAS_2INTEGER (0) +#define FICL_PLATFORM_HAS_FTRUNCATE (0) + +#define FICL_PLATFORM_OS "ansi" +#define FICL_PLATFORM_ARCHITECTURE "unknown" diff --git a/apps/mavlink/.context b/apps/mavlink/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/mavlink/.context diff --git a/apps/mavlink/.gitignore b/apps/mavlink/.gitignore new file mode 100644 index 000000000..a02827195 --- /dev/null +++ b/apps/mavlink/.gitignore @@ -0,0 +1,3 @@ +include +mavlink-* +pymavlink-* diff --git a/apps/mavlink/Makefile b/apps/mavlink/Makefile new file mode 100644 index 000000000..d4e9a5762 --- /dev/null +++ b/apps/mavlink/Makefile @@ -0,0 +1,44 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Mavlink Application +# + +APPNAME = mavlink +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c new file mode 100644 index 000000000..466ea7565 --- /dev/null +++ b/apps/mavlink/mavlink.c @@ -0,0 +1,1193 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink.c + * MAVLink 1.0 protocol implementation. + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <mqueue.h> +#include "mavlink_bridge_header.h" +#include <v1.0/common/mavlink.h> +#include <arch/board/up_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <stdlib.h> +#include <poll.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/ardrone_control.h> +#include <uORB/topics/fixedwing_control.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/ardrone_motors_setpoint.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include "waypoints.h" +#include "mavlink_log.h" + + +__EXPORT int mavlink_main(int argc, char *argv[]); + +/* terminate MAVLink on user request - disabled by default */ +static bool mavlink_link_termination_allowed = false; +static bool mavlink_exit_requested = false; + +static int system_type = MAV_TYPE_FIXED_WING; +mavlink_system_t mavlink_system = {100, 50}; // System ID, 1-255, Component/Subsystem ID, 1-255 +static uint8_t chan = MAVLINK_COMM_0; +static mavlink_status_t status; + +/* pthreads */ +static pthread_t receive_thread; +static pthread_t uorb_receive_thread; + +static uint16_t mavlink_message_intervals[256]; /**< intervals at which to send MAVLink packets */ +/* Allocate storage space for waypoints */ +mavlink_wpm_storage wpm_s; +/* Global position */ +static struct vehicle_global_position_s global_pos = {0}; +/* Local position */ +static struct vehicle_local_position_s local_pos = {0}; +/* Vehicle status */ +static struct vehicle_status_s v_status = {0}; +/* RC channels */ +static struct rc_channels_s rc = {0}; + +/* HIL publishers */ +static int pub_hil_attitude = -1; +static struct vehicle_attitude_s hil_attitude = {0}; +static struct vehicle_global_position_s hil_global_pos = {0}; +static struct fixedwing_control_s fw_control = {0}; +static struct ardrone_motors_setpoint_s ardrone_motors = {0}; +static struct vehicle_command_s vcmd = {0}; +static int pub_hil_global_pos = -1; +static int ardrone_motors_pub = -1; +static int cmd_pub = -1; +static int global_pos_sub = -1; +static int local_pos_sub = -1; +static int global_position_setpoint_pub = -1; +static int local_position_setpoint_pub = -1; +static bool mavlink_hil_enabled = false; + +static char mavlink_message_string[51] = {0}; + + +/* 3: Define waypoint helper functions */ +void mavlink_wpm_send_message(mavlink_message_t *msg); +void mavlink_wpm_send_gcs_string(const char *string); +uint64_t mavlink_wpm_get_system_timestamp(void); +void mavlink_missionlib_send_message(mavlink_message_t *msg); +void mavlink_missionlib_send_gcs_string(const char *string); +uint64_t mavlink_missionlib_get_system_timestamp(void); + +/* 4: Include waypoint protocol */ +#include "waypoints.h" +mavlink_wpm_storage *wpm; + + +#include "mavlink_parameters.h" + +static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + +void mavlink_missionlib_send_message(mavlink_message_t *msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + write(uart, missionlib_msg_buf, len); +} + +void mavlink_missionlib_send_gcs_string(const char *string) +{ + const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; + mavlink_statustext_t statustext; + int i = 0; + + while (i < len - 1) { + statustext.text[i] = string[i]; + + if (string[i] == '\0') + break; + + i++; + } + + if (i > 1) { + /* Enforce null termination */ + statustext.text[i] = '\0'; + mavlink_message_t msg; + + mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + mavlink_missionlib_send_message(&msg); + } +} + +/* + * Get system time since boot in microseconds + * + * @return the system time since boot in microseconds + */ +uint64_t mavlink_missionlib_get_system_timestamp() +{ + return hrt_absolute_time(); +} + +extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) +{ + /* Update controller setpoints */ + if (frame == (int)MAV_FRAME_GLOBAL) { + /* global, absolute waypoint */ + struct vehicle_global_position_setpoint_s sp; + sp.lat = param5_lat_x * 1e7; + sp.lon = param6_lon_y * 1e7; + sp.altitude = param7_alt_z; + sp.altitude_is_relative = false; + sp.yaw = (param4 / 180.0f) * ((float)M_PI) - ((float)M_PI); + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + + } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { + /* global, relative alt (in relation to HOME) waypoint */ + struct vehicle_global_position_setpoint_s sp; + sp.lat = param5_lat_x * 1e7; + sp.lon = param6_lon_y * 1e7; + sp.altitude = param7_alt_z; + sp.altitude_is_relative = true; + sp.yaw = (param4 / 180.0f) * ((float)M_PI) - ((float)M_PI); + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + + } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { + /* local, absolute waypoint */ + struct vehicle_local_position_setpoint_s sp; + sp.x = param5_lat_x; + sp.y = param6_lon_y; + sp.z = param7_alt_z; + sp.yaw = (param4 / 180.0f) * ((float)M_PI) - ((float)M_PI); + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); + } + + printf("[mavlink mp] new setpoint: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x, param6_lon_y, param7_alt_z, param4); +} + + +void handleMessage(mavlink_message_t *msg); + +/** + * Enable / disable Hardware in the Loop simulation mode. + */ +int set_hil_on_off(uint8_t vehicle_mode) +{ + int ret = OK; + + /* Enable HIL */ + if ((vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && !mavlink_hil_enabled) { + + //printf("\n HIL ON \n"); + + (void)close(pub_hil_attitude); + (void)close(pub_hil_global_pos); + + /* Advertise topics */ + pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + + printf("\n pub_hil_attitude :%i\n", pub_hil_attitude); + printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos); + + if (pub_hil_attitude > 0 && pub_hil_global_pos > 0) { + mavlink_hil_enabled = true; + + } else { + ret = ERROR; + } + } + + if (!(vehicle_mode & MAV_MODE_FLAG_HIL_ENABLED) && mavlink_hil_enabled) { + mavlink_hil_enabled = false; + (void)close(pub_hil_attitude); + (void)close(pub_hil_global_pos); + + } else { + ret = ERROR; + } + + return ret; +} + +/** + * Translate the custom state into standard mavlink modes and state. + */ +void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode) +{ + //TODO: Make this correct + switch (c_status->state_machine) { + case SYSTEM_STATE_PREFLIGHT: + *mavlink_state = MAV_STATE_UNINIT; + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + break; + + case SYSTEM_STATE_STANDBY: + *mavlink_state = MAV_STATE_STANDBY; + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + break; + + case SYSTEM_STATE_GROUND_READY: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + break; + + case SYSTEM_STATE_MANUAL: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + break; + + case SYSTEM_STATE_STABILIZED: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + break; + + case SYSTEM_STATE_AUTO: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + break; + + case SYSTEM_STATE_MISSION_ABORT: + *mavlink_state = MAV_STATE_EMERGENCY; + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + break; + + case SYSTEM_STATE_EMCY_LANDING: + *mavlink_state = MAV_STATE_EMERGENCY; + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + *mavlink_state = MAV_STATE_EMERGENCY; + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + break; + + case SYSTEM_STATE_GROUND_ERROR: + *mavlink_state = MAV_STATE_EMERGENCY; + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + break; + + case SYSTEM_STATE_REBOOT: + *mavlink_state = MAV_STATE_POWEROFF; + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + break; + } + +} + +/** + * Receive data from UART. + */ +static void *receiveloop(void *arg) +{ + uint8_t ch; + + mavlink_message_t msg; + + prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + + while (1) { + + if (mavlink_exit_requested) break; + + /* blocking read on next byte */ + int nread = read(uart, &ch, 1); + + + if (nread > 0 && mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* handle generic messages and commands */ + handleMessage(&msg); + + /* Handle packet with waypoint component */ + mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + + /* Handle packet with parameter component */ + mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + msg.msgid = -1; + } + + } + + return NULL; +} + +/** + * Listen for uORB topics and send via MAVLink. + * + * This pthread performs a blocking wait on selected + * uORB topics and sends them via MAVLink to other + * vehicles or a ground control station. + */ +static void *uorb_receiveloop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "mavlink uORB async", getpid()); + + + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 10; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + + + union { + struct sensor_combined_s raw; + struct vehicle_attitude_s att; + struct vehicle_gps_position_s gps; + struct ardrone_control_s ar_control; + } buf; + + /* --- SENSORS RAW VALUE --- */ + /* subscribe to ORB for sensors raw */ + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + orb_set_interval(sensor_sub, 100); /* 10Hz updates */ + fds[fdsc_count].fd = sensor_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ATTITUDE VALUE --- */ + /* subscribe to ORB for attitude */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_set_interval(att_sub, 100); /* 10Hz updates */ + fds[fdsc_count].fd = att_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GPS VALUE --- */ + /* subscribe to ORB for attitude */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(gps_sub, 1000); /* 1Hz updates */ + fds[fdsc_count].fd = gps_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ARDRONE CONTROL --- */ + /* subscribe to ORB for AR.Drone controller outputs */ + int ar_sub = orb_subscribe(ORB_ID(ardrone_control)); + orb_set_interval(ar_sub, 200); /* 5Hz updates */ + fds[fdsc_count].fd = ar_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- SYSTEM STATE --- */ + /* struct already globally allocated */ + /* subscribe to topic */ + int status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + fds[fdsc_count].fd = status_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- RC CHANNELS VALUE --- */ + /* struct already globally allocated */ + /* subscribe to ORB for global position */ + int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(rc_sub, 100); /* 10Hz updates */ + fds[fdsc_count].fd = rc_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- FIXED WING CONTROL VALUE --- */ + /* struct already globally allocated */ + /* subscribe to ORB for fixed wing control */ + int fw_sub = orb_subscribe(ORB_ID(fixedwing_control)); + orb_set_interval(fw_sub, 200); /* 5Hz updates */ + fds[fdsc_count].fd = fw_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GLOBAL POS VALUE --- */ + /* struct already globally allocated and topic already subscribed */ + fds[fdsc_count].fd = global_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- LOCAL POS VALUE --- */ + /* struct and topic already globally subscribed */ + fds[fdsc_count].fd = local_pos_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + unsigned int sensors_raw_counter = 0; + unsigned int attitude_counter = 0; + unsigned int gps_counter = 0; + + /* WARNING: If you get the error message below, + * then the number of registered messages (fdsc) + * differs from the number of messages in the above list. + */ + if (fdsc_count > fdsc) { + fprintf(stderr, "[mavlink] WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); + fdsc_count = fdsc; + } + + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms (1 second) + */ + const int timeout = 5000; + + while (1) { + if (mavlink_exit_requested) break; + + int poll_ret = poll(fds, fdsc_count, timeout); + + /* handle the poll result */ + if (poll_ret == 0) { + /* XXX this means none of our providers is giving us data - might be an error? */ + } else if (poll_ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else { + + /* --- SENSORS RAW VALUE --- */ + if (fds[0].revents & POLLIN) { + + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw); + + /* send raw imu data */ + mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); + /* send scaled imu data */ + mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 9810, buf.raw.accelerometer_m_s2[1] * 9810, buf.raw.accelerometer_m_s2[2] * 9810, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000); + /* send pressure */ + mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100); + + sensors_raw_counter++; + } + + /* --- ATTITUDE VALUE --- */ + if (fds[1].revents & POLLIN) { + + /* copy attitude data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att); + + /* send sensor values */ + mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); + + attitude_counter++; + } + + /* --- GPS VALUE --- */ + if (fds[2].revents & POLLIN) { + /* copy gps data into local buffer */ + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps); + /* GPS position */ + mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible); + + if (buf.gps.satellite_info_available && (gps_counter % 4 == 0)) { + mavlink_msg_gps_status_send(MAVLINK_COMM_0, buf.gps.satellites_visible, buf.gps.satellite_prn, buf.gps.satellite_used, buf.gps.satellite_elevation, buf.gps.satellite_azimuth, buf.gps.satellite_snr); + } + + gps_counter++; + } + + /* --- ARDRONE CONTROL OUTPUTS --- */ + if (fds[3].revents & POLLIN) { + /* copy ardrone control data into local buffer */ + orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control); + uint64_t timestamp = buf.ar_control.timestamp; + float setpoint_roll = buf.ar_control.setpoint_attitude[0]; + float setpoint_pitch = buf.ar_control.setpoint_attitude[1]; + float setpoint_yaw = buf.ar_control.setpoint_attitude[2]; + float setpoint_thrust = buf.ar_control.setpoint_thrust_cast; + + float control_roll = buf.ar_control.attitude_control_output[0]; + float control_pitch = buf.ar_control.attitude_control_output[1]; + float control_yaw = buf.ar_control.attitude_control_output[2]; + + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, timestamp / 1000, setpoint_roll, setpoint_pitch, setpoint_yaw, setpoint_thrust); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.roll", control_roll); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.pitch", control_pitch); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.yaw", control_yaw); + } + + /* --- SYSTEM STATUS --- */ + if (fds[4].revents & POLLIN) { + /* immediately communicate state changes back to user */ + orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); + /* enable or disable HIL */ + set_hil_on_off(v_status.mode); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = v_status.mode; + get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_GENERIC, mavlink_mode, v_status.state_machine, mavlink_state); + } + + /* --- RC CHANNELS --- */ + if (fds[5].revents & POLLIN) { + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + /* Channels are sent in MAVLink main loop at a fixed interval */ + // TODO decide where to send channels + } + + /* --- FIXED WING CONTROL CHANNELS --- */ + if (fds[6].revents & POLLIN) { + /* copy fixed wing control into local buffer */ + orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control); + // XXX Output fixed wing control + } + + /* --- VEHICLE GLOBAL POSITION --- */ + if (fds[7].revents & POLLIN) { + /* copy global position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + uint64_t timestamp = global_pos.timestamp; + int32_t lat = (int32_t)(global_pos.lat * 1e7); + int32_t lon = (int32_t)(global_pos.lon * 1e7); + int32_t alt = (int32_t)(global_pos.alt * 1e3); + int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1e3); + int16_t vx = (int16_t)(global_pos.vx * 1e2); + int16_t vy = (int16_t)(global_pos.vy * 1e2); + int16_t vz = (int16_t)(global_pos.vz * 1e2); + /* heading in degrees * 10, from 0 to 36.000) */ + uint16_t hdg = (global_pos.hdg / M_PI_F) * (180 * 10) + (180 * 10); + + mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, relative_alt, vx, vy, vz, hdg); + } + + /* --- VEHICLE LOCAL POSITION --- */ + if (fds[8].revents & POLLIN) { + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, local_pos.timestamp / 1000, local_pos.x, local_pos.y, local_pos.z, local_pos.vx, local_pos.vy, local_pos.vz); + } + + } + } + + return NULL; +} + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + +static const struct file_operations mavlink_fops = { + .ioctl = mavlink_dev_ioctl +}; + +static int +mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + static unsigned int total_counter = 0; + + switch (cmd) { + case (int)MAVLINK_IOC_SEND_TEXT_INFO: + case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: + case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { + const char *txt = (const char *)arg; + strncpy(mavlink_message_string, txt, 51); + total_counter++; + return OK; + } + + default: + return ENOTTY; + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +void handleMessage(mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && cmd_mavlink.param1 == 3.0f) { + /* This is the link shutdown command, terminate mavlink */ //TODO: check what happens with global_data buffers that are read by the mavlink app + printf("[mavlink] Terminating .. \n"); + fflush(stdout); + usleep(50000); + + /* terminate other threads */ + mavlink_exit_requested = true; + pthread_cancel(receive_thread); + pthread_cancel(uorb_receive_thread); + + pthread_exit(NULL); + + } else { + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + + /* Publish */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + } + + if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { + /* Set mode on request */ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = new_mode.custom_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + + /* Handle quadrotor motor setpoints */ + + if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT) { + mavlink_set_quad_motors_setpoint_t quad_motors_setpoint; + mavlink_msg_set_quad_motors_setpoint_decode(msg, &quad_motors_setpoint); +// printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid); + + if (quad_motors_setpoint.target_system == mavlink_system.sysid) { + ardrone_motors.motor_front_nw = quad_motors_setpoint.motor_front_nw; + ardrone_motors.motor_right_ne = quad_motors_setpoint.motor_right_ne; + ardrone_motors.motor_back_se = quad_motors_setpoint.motor_back_se; + ardrone_motors.motor_left_sw = quad_motors_setpoint.motor_left_sw; + + ardrone_motors.counter++; + ardrone_motors.timestamp = hrt_absolute_time(); + + /* Publish */ + orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors); + } + } + + /* + * Only decode hil messages in HIL mode. + * + * The HIL mode is enabled by the HIL bit flag + * in the system mode. Either send a set mode + * COMMAND_LONG message or a SET_MODE message + */ + + // printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false"); +#define DEG2RAD ((1.0/180.0)*M_PI) + + if (mavlink_hil_enabled) { + + if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { + + mavlink_hil_state_t hil_state; + mavlink_msg_hil_state_decode(msg, &hil_state); + + // printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n " + // "ROLL %i \n PITCH %i \n YAW %i \n" + // "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n", + // hil_state.lat/1000000, // 1e7 + // hil_state.lon/1000000, // 1e7 + // hil_state.alt/1000, // mm + // hil_state.roll, // float rad + // hil_state.pitch, // float rad + // hil_state.yaw, // float rad + // hil_state.rollspeed, // float rad/s + // hil_state.pitchspeed, // float rad/s + // hil_state.yawspeed); // float rad/s + + + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt/1000; + hil_global_pos.vx = hil_state.vx; + hil_global_pos.vy = hil_state.vy; + hil_global_pos.vz = hil_state.vz; + + /* set timestamp and notify processes (broadcast) */ + hil_global_pos.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + + hil_attitude.roll = hil_state.roll; + hil_attitude.pitch = hil_state.pitch; + hil_attitude.yaw = hil_state.yaw; + hil_attitude.rollspeed = hil_state.rollspeed; + hil_attitude.pitchspeed = hil_state.pitchspeed; + hil_attitude.yawspeed = hil_state.yawspeed; + + /* set timestamp and notify processes (broadcast) */ + hil_attitude.counter++; + hil_attitude.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } + + // if (msg->msgid == MAVLINK_MSG_ID_ATTITUDE) { + // mavlink_attitude_t att; + // mavlink_msg_attitude_decode(msg, &att); + // float RAD2DEG = 57.3f; + + // // printf("\n\n\n ATTITUDE \n\n\n %i \n", (int)(1000*att.rollspeed)); + + // global_data_lock(&global_data_attitude->access_conf); + // global_data_attitude->roll = RAD2DEG * att.roll; + // global_data_attitude->pitch = RAD2DEG * att.pitch; + // global_data_attitude->yaw = RAD2DEG * att.yaw; + // global_data_attitude->rollspeed = att.rollspeed; + // global_data_attitude->pitchspeed = att.pitchspeed; + // global_data_attitude->yawspeed = att.yawspeed; + + // global_data_attitude->counter++; + // global_data_attitude->timestamp = hrt_absolute_time(); + // global_data_unlock(&global_data_attitude->access_conf); + // global_data_broadcast(&global_data_attitude->access_conf); + // } + + // if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { + // mavlink_raw_imu_t imu; + // mavlink_msg_raw_imu_decode(msg, &imu); + + // // printf("\n\n\n RAW_IMU : %i \n %i \n %i \n %i \n %i \n %i \n\n\n", (int)(1000*imu.xgyro), + // // (int)(1000*imu.ygyro), (int)(1000*imu.zgyro)); + + // global_data_lock(&global_data_attitude->access_conf); + // global_data_attitude->rollspeed = 1000 * imu.xgyro; + // global_data_attitude->pitchspeed = 1000 * imu.ygyro; + // global_data_attitude->yawspeed = 1000 * imu.zgyro; + + // global_data_attitude->counter++; + // global_data_attitude->timestamp = hrt_absolute_time(); + // global_data_unlock(&global_data_attitude->access_conf); + // global_data_broadcast(&global_data_attitude->access_conf); + // } + + // if (msg->msgid == MAVLINK_MSG_ID_SCALED_IMU) { + // mavlink_raw_imu_t imu; + // mavlink_msg_raw_imu_decode(msg, &imu); + + // // printf("\n\n\n SCALED_IMU : %i \n %i \n %i \n %i \n %i \n %i \n\n\n", (int)(1000*imu.xgyro), + // // (int)(1000*imu.ygyro), (int)(1000*imu.zgyro)); + + // global_data_lock(&global_data_attitude->access_conf); + // global_data_attitude->rollspeed = 1000 * imu.xgyro; + // global_data_attitude->pitchspeed = 1000 * imu.ygyro; + // global_data_attitude->yawspeed = 1000 * imu.zgyro; + + // global_data_attitude->counter++; + // global_data_attitude->timestamp = hrt_absolute_time(); + // global_data_unlock(&global_data_attitude->access_conf); + // global_data_broadcast(&global_data_attitude->access_conf); + // } + } +} + +int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + /* process baud rate */ + int speed; + + switch (baudrate) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baudrate); + return -EINVAL; + } + + /* open uart */ + printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baudrate); + uart = open(uart_name, O_RDWR | O_NOCTTY); + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + if (strcmp(uart_name, "/dev/ttyACM0") != OK) { + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + } else { + *is_usb = true; + } + + return uart; +} + +/** + * MAVLink Protocol main function. + */ +int mavlink_main(int argc, char *argv[]) +{ + wpm = &wpm_s; + + // print text + printf("[mavlink] MAVLink v1.0 serial interface starting..\n"); + + // create the device node that's used for sending text log messages, etc. + register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + + /* Send attitude at 10 Hz / every 100 ms */ + mavlink_message_intervals[MAVLINK_MSG_ID_ATTITUDE] = 100; + /* Send raw sensor values at 10 Hz / every 100 ms */ + mavlink_message_intervals[MAVLINK_MSG_ID_RAW_IMU] = 100; + + //default values for arguments + char *uart_name = "/dev/ttyS0"; + int baudrate = 115200; + const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n"; + + /* read program arguments */ + int i; + + for (i = 1; i < argc; i++) { /* argv[0] is "mavlink" */ + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + printf(commandline_usage, argv[0]); + return 0; + } + + /* UART device ID */ + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { + if (argc > i + 1) { + uart_name = argv[i + 1]; + + } else { + printf(commandline_usage, argv[0]); + return 0; + } + } + + /* baud rate */ + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--baud") == 0) { + if (argc > i + 1) { + baudrate = atoi(argv[i + 1]); + + } else { + printf(commandline_usage, argv[0]); + return 0; + } + } + + /* terminating MAVLink is allowed - yes/no */ + if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) { + mavlink_link_termination_allowed = true; + } + } + + struct termios uart_config_original; + + bool usb_uart; + + uart = mavlink_open_uart(baudrate, uart_name, &uart_config_original, &usb_uart); + + if (uart < 0) { + printf("[mavlink] FAILED to open %s, terminating.\n", uart_name); + return -1; + } + + /* Flush UART */ + fflush(stdout); + + /* topics to advertise */ + ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors); + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + /* topics to subscribe globally */ + /* subscribe to ORB for global position */ + global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(global_pos_sub, 1000); /* 1Hz active updates */ + /* subscribe to ORB for local position */ + local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(local_pos_sub, 1000); /* 1Hz active updates */ + + + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_create(&receive_thread, &receiveloop_attr, receiveloop, NULL); + + pthread_attr_t uorb_attr; + pthread_attr_init(&uorb_attr); + /* Set stack size, needs more than 2048 bytes */ + pthread_attr_setstacksize(&uorb_attr, 4096); + pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL); + + /* initialize waypoint manager */ + mavlink_wpm_init(wpm); + + uint16_t counter = 0; + int lowspeed_counter = 0; + + /**< Subscribe to system state and RC channels */ + // int status_sub = orb_subscribe(ORB_ID(vehicle_status)); + // int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + + while (1) { + + if (mavlink_exit_requested) break; + + /* get local and global position */ + orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + + /* check if waypoint has been reached against the last positions */ + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + + // sleep + usleep(50000); + + // 1 Hz + if (lowspeed_counter == 10) { + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = v_status.mode; + get_mavlink_mode_and_state(&v_status, &mavlink_state, &mavlink_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_GENERIC, mavlink_mode, v_status.state_machine, mavlink_state); + + /* send status (values already copied in the section above) */ + mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, + v_status.onboard_control_sensors_health, v_status.load, v_status.voltage_battery * 1000.f, v_status.current_battery * 1000.f, + v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, + v_status.errors_count1, v_status.errors_count2, v_status.errors_count3, v_status.errors_count4); + + /* send over MAVLink */ + mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw, + rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi); + + lowspeed_counter = 0; + } + + lowspeed_counter++; + + /* Only send in HIL mode */ + if (v_status.mode & MAV_MODE_FLAG_HIL_ENABLED) { + /* Send the desired attitude from RC or from the autonomous controller */ + // XXX it should not depend on a RC setting, but on a system_state value + + float roll_ail, pitch_elev, throttle, yaw_rudd; + + if (rc.chan[rc.function[OVERRIDE]].scale < 2000) { + + //orb_copy(ORB_ID(fixedwing_control), fixed_wing_control_sub, &fixed_wing_control); + roll_ail = fw_control.attitude_control_output[ROLL]; + pitch_elev = fw_control.attitude_control_output[PITCH]; + throttle = fw_control.attitude_control_output[THROTTLE]; + yaw_rudd = fw_control.attitude_control_output[YAW]; + + } else { + + roll_ail = rc.chan[rc.function[ROLL]].scale; + pitch_elev = rc.chan[rc.function[PITCH]].scale; + throttle = rc.chan[rc.function[THROTTLE]].scale; + yaw_rudd = rc.chan[rc.function[YAW]].scale; + } + + /* hacked HIL implementation in order for the APM Planner to work + * (correct cmd: mavlink_msg_hil_controls_send()) + */ + + mavlink_msg_rc_channels_scaled_send(chan, + hrt_absolute_time(), + 0, // port 0 + roll_ail, + pitch_elev, + throttle, + yaw_rudd, + 0, + 0, + 0, + 0, + 1 /*rssi=1*/); + /* correct command duplicate */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + roll_ail, + pitch_elev, + yaw_rudd, + throttle, + 0, + 0, + 0, + 0, + 32, /* HIL_MODE */ + 0); + } + + /* send parameters at 20 Hz (if queued for sending) */ + mavlink_pm_queued_send(); + usleep(50000); + mavlink_pm_queued_send(); + + /* send one string at 10 Hz */ + mavlink_missionlib_send_gcs_string(mavlink_message_string); + mavlink_message_string[0] = '\0'; + counter++; + } + + /* wait for threads to complete */ + pthread_join(receive_thread, NULL); + pthread_join(uorb_receive_thread, NULL); + + /* Reset the UART flags to original state */ + if (!usb_uart) { + int termios_state; + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config_original)) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", uart_name); + } + + printf("[mavlink] Restored original UART config, exiting..\n"); + } + + /* close uart */ + close(uart); + + fflush(stdout); + fflush(stderr); + + return 0; +} + + diff --git a/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h new file mode 100644 index 000000000..26bc26fdf --- /dev/null +++ b/apps/mavlink/mavlink_bridge_header.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_bridge_header + * MAVLink bridge header for UART access. + */ + +/* MAVLink adapter header */ +#ifndef MAVLINK_BRIDGE_HEADER_H +#define MAVLINK_BRIDGE_HEADER_H + +#define MAVLINK_USE_CONVENIENCE_FUNCTIONS + +//use efficient approach, see mavlink_helpers.h +#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes + +#include "v1.0/mavlink_types.h" +#include <unistd.h> + + +/* Struct that stores the communication settings of this system. + you can also define / alter these settings elsewhere, as long + as they're included BEFORE mavlink.h. + So you can set the + + mavlink_system.sysid = 100; // System ID, 1-255 + mavlink_system.compid = 50; // Component/Subsystem ID, 1-255 + + Lines also in your main.c, e.g. by reading these parameter from EEPROM. + */ +extern mavlink_system_t mavlink_system; + + +mqd_t gps_queue; +int uart; + + +/** + * @brief Send multiple chars (uint8_t) over a comm channel + * + * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 + * @param ch Character to send + */ +static inline void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, uint16_t length) +{ + ssize_t ret; + + if (chan == MAVLINK_COMM_0) { + ret = write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + + if (ret != length) { + printf("[mavlink] Error: Written %u instead of %u\n", ret, length); + } + } +} + +#endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h new file mode 100644 index 000000000..1ad1364be --- /dev/null +++ b/apps/mavlink/mavlink_log.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_log.h + * MAVLink text logging. + */ + +#ifndef MAVLINK_LOG +#define MAVLINK_LOG + +/* + * IOCTL interface for sending log messages. + */ +#include <sys/ioctl.h> + +/* + * The mavlink log device node; must be opened before messages + * can be logged. + */ +#define MAVLINK_LOG_DEVICE "/dev/mavlink" + +#define MAVLINK_IOC_SEND_TEXT_INFO _IOC(0x1100, 1) +#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2) +#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3) + +/** + * Send a mavlink emergency message. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); + +/** + * Send a mavlink critical message. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); + +/** + * Send a mavlink info message. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); + +#endif + diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c new file mode 100644 index 000000000..a064fc167 --- /dev/null +++ b/apps/mavlink/mavlink_parameters.c @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_parameters.c + * MAVLink parameter protocol implementation (BSD-relicensed). + */ + +#include "mavlink_parameters.h" +#include <uORB/uORB.h> +#include "math.h" /* isinf / isnan checks */ +#include <assert.h> +#include <stdio.h> +#include <fcntl.h> +#include <stdbool.h> +#include <string.h> + +extern mavlink_system_t mavlink_system; + +extern void mavlink_missionlib_send_message(mavlink_message_t *msg); +extern void mavlink_missionlib_send_gcs_string(const char *string); + +/* send one parameter, assume lock on global_data_parameter_storage */ +void mavlink_pm_send_one_parameter(uint16_t next_param) +{ + if (next_param < global_data_parameter_storage->pm.size) { + static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; + mavlink_message_t tx_msg; + + strncpy((char *)name_buf, global_data_parameter_storage->pm.param_names[next_param], MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + mavlink_msg_param_value_pack_chan(mavlink_system.sysid, + mavlink_system.compid, + MAVLINK_COMM_0, + &tx_msg, + name_buf, + global_data_parameter_storage->pm.param_values[next_param], + MAVLINK_TYPE_FLOAT, + global_data_parameter_storage->pm.size, + next_param); + mavlink_missionlib_send_message(&tx_msg); + + + // mavlink_msg_param_value_send(MAVLINK_COMM_0, + // name_buf, + // global_data_parameter_storage->pm.param_values[next_param], + // MAVLINK_TYPE_FLOAT, + // global_data_parameter_storage->pm.size, + // next_param); + } +} + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* Start sending parameters */ + global_data_parameter_storage->pm.next_param = 0; + mavlink_missionlib_send_gcs_string("[pm] sending list"); + } break; + + case MAVLINK_MSG_ID_PARAM_SET: { + + /* Handle parameter setting */ + + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t mavlink_param_set; + mavlink_msg_param_set_decode(msg, &mavlink_param_set); + + if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + + uint16_t i; //parameters + uint16_t j; //chars + bool match; + + for (i = 0; i < PARAM_MAX_COUNT; i++) { + match = true; + + for (j = 0; j < MAX_PARAM_NAME_LEN; j++) { + /* Compare char by char */ + if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_set.param_id[j]) { + match = false; + } + + /* End matching if null termination is reached */ + if (global_data_parameter_storage->pm.param_names[i][j] == '\0') { + break; + } + } + + /* Check if matched */ + if (match) { + // XXX handle param type as well, assuming float here + global_data_parameter_storage->pm.param_values[i] = mavlink_param_set.param_value; + mavlink_pm_send_one_parameter(i); + } + } + } + } + } break; + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + mavlink_param_request_read_t mavlink_param_request_read; + mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); + + if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + /* when no index is given, loop through string ids and compare them */ + if (mavlink_param_request_read.param_index == -1) { + + uint16_t i; //parameters + uint16_t j; //chars + bool match; + + for (i = 0; i < PARAM_MAX_COUNT; i++) { + match = true; + + for (j = 0; j < MAX_PARAM_NAME_LEN; j++) { + /* Compare char by char */ + if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_request_read.param_id[j]) { + match = false; + } + + /* End matching if null termination is reached */ + if (global_data_parameter_storage->pm.param_names[i][j] == '\0') { + break; + } + } + + /* Check if matched */ + if (match) { + mavlink_pm_send_one_parameter(i); + } + } + + } else { + /* when index is >= 0, send this parameter again */ + mavlink_pm_send_one_parameter(mavlink_param_request_read.param_index); + } + } + + } break; + } +} + + +/** + * Send low-priority messages at a maximum rate of xx Hertz. + * + * This function sends messages at a lower rate to not exceed the wireless + * bandwidth. It sends one message each time it is called until the buffer is empty. + * Call this function with xx Hertz to increase/decrease the bandwidth. + */ +void mavlink_pm_queued_send(void) +{ + //send parameters one by one: + mavlink_pm_send_one_parameter(global_data_parameter_storage->pm.next_param); + global_data_parameter_storage->pm.next_param++; +} diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h new file mode 100644 index 000000000..e32732f58 --- /dev/null +++ b/apps/mavlink/mavlink_parameters.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_parameters.h + * MAVLink parameter protocol definitions (BSD-relicensed). + */ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + + +#include "v1.0/common/mavlink.h" +#include <stdbool.h> + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); +void mavlink_pm_queued_send(void); diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c new file mode 100644 index 000000000..977f7d47e --- /dev/null +++ b/apps/mavlink/waypoints.c @@ -0,0 +1,1137 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file waypoints.c + * MAVLink waypoint protocol implementation (BSD-relicensed). + */ + +#include "waypoints.h" +#include <math.h> +#include <sys/prctl.h> +#include <unistd.h> +#include <stdio.h> + +#ifndef FM_PI +#define FM_PI 3.1415926535897932384626433832795f +#endif + +bool debug = false; +bool verbose = false; + +extern mavlink_wpm_storage *wpm; + +extern void mavlink_missionlib_send_message(mavlink_message_t *msg); +extern void mavlink_missionlib_send_gcs_string(const char *string); +extern uint64_t mavlink_missionlib_get_system_timestamp(void); +extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); + + +#define MAVLINK_WPM_NO_PRINTF + +uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + +void mavlink_wpm_init(mavlink_wpm_storage *state) +{ + // Set all waypoints to zero + + // Set count to zero + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + state->idle = false; ///< indicates if the system is following the waypoints or is waiting + state->current_active_wp_id = -1; ///< id of current waypoint + state->yaw_reached = false; ///< boolean for yaw attitude reached + state->pos_reached = false; ///< boolean for position reached + state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = wpm->current_partner_sysid; + wpa.target_component = wpm->current_partner_compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); + mavlink_missionlib_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + +#endif + mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); + } +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, + cur->param2, cur->param3, cur->param4, cur->x, + cur->y, cur->z, cur->frame, cur->command); + + wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = wpm->current_partner_sysid; + wpc.target_component = wpm->current_partner_compid; + wpc.count = count; + + mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_message_t msg; + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); + wp->target_system = wpm->current_partner_sysid; + wp->target_component = wpm->current_partner_compid; + mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm->max_size) { + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = wpm->current_partner_sysid; + wpr.target_component = wpm->current_partner_compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +//{ +// if (seq < wpm->size) +// { +// mavlink_mission_item_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// // seq not the second last waypoint +// if ((uint16_t)(seq+1) < wpm->size) +// { +// mavlink_mission_item_t *next = waypoints->at(seq+1); +// const PxVector3 B(next->x, next->y, next->z); +// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); +// if (r >= 0 && r <= 1) +// { +// const PxVector3 P(A + r*(B-A)); +// return (P-C).length(); +// } +// else if (r < 0.f) +// { +// return (C-A).length(); +// } +// else +// { +// return (C-B).length(); +// } +// } +// else +// { +// return (C-A).length(); +// } +// } +// else +// { +// // if (verbose) // printf("ERROR: index out of bounds\n"); +// } +// return -1.f; +//} + +/* + * Calculate distance in global frame. + * + * The distance calculation is based on the WGS84 geoid (GPS) + */ +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +{ + //TODO: implement for z once altidude contoller is implemented + + static uint16_t counter; + +// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + double current_x_rad = cur->x / 180.0 * M_PI; + double current_y_rad = cur->y / 180.0 * M_PI; + double x_rad = lat / 180.0 * M_PI; + double y_rad = lon / 180.0 * M_PI; + + double d_lat = x_rad - current_x_rad; + double d_lon = y_rad - current_y_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + const double radius_earth = 6371000.0; + + return radius_earth * c; + + } else { + return -1.0f; + } + + counter++; + +} + +/* + * Calculate distance in local frame (NED) + */ +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + float dx = (cur->x - x); + float dy = (cur->y - y); + float dz = (cur->z - z); + + return sqrt(dx * dx + dy * dy + dz * dz); + + } else { + return -1.0f; + } +} + +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +{ + static uint16_t counter; + + if (wpm->size > 0 && counter % 10 == 0) { + printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id); + } + + + if (wpm->current_active_wp_id < wpm->size) { + + float orbit = wpm->waypoints[wpm->current_active_wp_id].param2; + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; + float dist = -1.0f; + + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->alt); + + } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); + + } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); + + } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { + /* Check if conditions of mission item are satisfied */ + // XXX TODO + } + + if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw + wpm->pos_reached = true; + + if (counter % 10 == 0) + printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); + } + +// else +// { +// if(counter % 100 == 0) +// printf("Setpoint not reached yet: %0.4f, orbit: %.4f\n",dist,orbit); +// } + } + + //check if the current waypoint was reached + if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->current_active_wp_id < wpm->size) { + mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); + + if (wpm->timestamp_firstinside_orbit == 0) { + // Announce that last waypoint was reached + printf("Reached waypoint %u for the first time \n", cur_wp->seq); + mavlink_wpm_send_waypoint_reached(cur_wp->seq); + wpm->timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) { + printf("Reached waypoint %u long enough \n", cur_wp->seq); + + if (cur_wp->autocontinue) { + cur_wp->current = 0; + + if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + wpm->current_active_wp_id = 1; + + } else { + if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) + wpm->current_active_wp_id++; + } + + // Fly to next waypoint + wpm->timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->waypoints[wpm->current_active_wp_id].current = true; + wpm->pos_reached = false; + wpm->yaw_reached = false; + printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); + } + } + } + + } else { + wpm->timestamp_lastoutside_orbit = now; + } + + counter++; +} + + +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +{ + /* check for timed-out operations */ + if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state); + +#endif + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_count = 0; + wpm->current_partner_sysid = 0; + wpm->current_partner_compid = 0; + wpm->current_wp_id = -1; + + if (wpm->size == 0) { + wpm->current_active_wp_id = -1; + } + } + + if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + } + + check_waypoints_reached(now, global_position , local_position); + + return OK; +} + + +void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) +{ + uint64_t now = mavlink_missionlib_get_system_timestamp(); + + switch (msg->msgid) { +// case MAVLINK_MSG_ID_ATTITUDE: +// { +// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) +// { +// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); +// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) +// { +// mavlink_attitude_t att; +// mavlink_msg_attitude_decode(msg, &att); +// float yaw_tolerance = wpm->accept_range_yaw; +// //compare current yaw +// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) +// { +// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) +// wpm->yaw_reached = true; +// } +// else if(att.yaw - yaw_tolerance < 0.0f) +// { +// float lowerBound = 360.0f + att.yaw - yaw_tolerance; +// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) +// wpm->yaw_reached = true; +// } +// else +// { +// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; +// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) +// wpm->yaw_reached = true; +// } +// } +// } +// break; +// } +// +// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: +// { +// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) +// { +// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); +// +// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) +// { +// mavlink_local_position_ned_t pos; +// mavlink_msg_local_position_ned_decode(msg, &pos); +// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); +// +// wpm->pos_reached = false; +// +// // compare current position (given in message) with current waypoint +// float orbit = wp->param1; +// +// float dist; +//// if (wp->param2 == 0) +//// { +//// // FIXME segment distance +//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); +//// } +//// else +//// { +// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); +//// } +// +// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) +// { +// wpm->pos_reached = true; +// } +// } +// } +// break; +// } + +// case MAVLINK_MSG_ID_CMD: // special action from ground station +// { +// mavlink_cmd_t action; +// mavlink_msg_cmd_decode(msg, &action); +// if(action.target == mavlink_system.sysid) +// { +// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; +// switch (action.action) +// { +// // case MAV_ACTION_LAUNCH: +// // // if (verbose) std::cerr << "Launch received" << std::endl; +// // current_active_wp_id = 0; +// // if (wpm->size>0) +// // { +// // setActive(waypoints[current_active_wp_id]); +// // } +// // else +// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; +// // break; +// +// // case MAV_ACTION_CONTINUE: +// // // if (verbose) std::c +// // err << "Continue received" << std::endl; +// // idle = false; +// // setActive(waypoints[current_active_wp_id]); +// // break; +// +// // case MAV_ACTION_HALT: +// // // if (verbose) std::cerr << "Halt received" << std::endl; +// // idle = true; +// // break; +// +// // default: +// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; +// // break; +// } +// } +// break; +// } + + case MAVLINK_MSG_ID_MISSION_ACK: { + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpm->current_wp_id == wpm->size - 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + +#endif + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_wp_id = 0; + } + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + if (wpc.seq < wpm->size) { + // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); + wpm->current_active_wp_id = wpc.seq; + uint32_t i; + + for (i = 0; i < wpm->size; i++) { + if (i == wpm->current_active_wp_id) { + wpm->waypoints[i].current = true; + + } else { + wpm->waypoints[i].current = false; + } + } + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("NEW WP SET"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); + +#endif + wpm->yaw_reached = false; + wpm->pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->timestamp_firstinside_orbit = 0; + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpm->size > 0) { + //if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + + } else { + // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + + wpm->current_count = wpm->size; + mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); + + } else { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state); + } + } else { + // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST: { + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) { + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + wpm->current_wp_id = wpr.seq; + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); + + } else { + // if (verbose) + { + if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state); + +#endif + break; + + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpr.seq != 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + +#endif + } + + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + +#endif + + } else if (wpr.seq >= wpm->size) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); + +#endif + } + } + } + + } else { + //we we're target but already communicating with someone else + if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid); + +#endif + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_COUNT: { + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) { +// printf("wpc count in: %d\n",wpc.count); +// printf("Comp id: %d\n",msg->compid); +// printf("Current partner sysid: %d\n",wpm->current_partner_sysid); + + if (wpc.count > 0) { + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + +#endif + } + + wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + wpm->current_count = wpc.count; + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); + +#endif + wpm->rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints_receive_buffer->back(); +// waypoints_receive_buffer->pop_back(); +// } + + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + + } else if (wpc.count == 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("COUNT 0"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); + +#endif + wpm->rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints->back(); +// waypoints->pop_back(); +// } + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + break; + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CMD"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + +#endif + } + + } else { + if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state); + +#endif + + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id); + +#endif + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); + +#endif + } + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + } + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: { + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + mavlink_missionlib_send_gcs_string("GOT WP"); +// printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); +// printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); + +// if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) + if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { + + wpm->timestamp_lastaction = now; + +// printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id); + +// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) { + //mavlink_missionlib_send_gcs_string("DEBUG 2"); + +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); +// + wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); + memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); +// printf("WP seq: %d\n",wp.seq); + wpm->current_wp_id = wp.seq + 1; + + // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); +// printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + +// printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); + if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + mavlink_missionlib_send_gcs_string("GOT ALL WPS"); + // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); + + if (wpm->current_active_wp_id > wpm->rcv_size - 1) { + wpm->current_active_wp_id = wpm->rcv_size - 1; + } + + // switch the waypoints list + // FIXME CHECK!!! + uint32_t i; + + for (i = 0; i < wpm->current_count; ++i) { + wpm->waypoints[i] = wpm->rcv_waypoints[i]; + } + + wpm->size = wpm->current_count; + + //get the new current waypoint + + for (i = 0; i < wpm->size; i++) { + if (wpm->waypoints[i].current == 1) { + wpm->current_active_wp_id = i; + //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); + wpm->yaw_reached = false; + wpm->pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == wpm->size) { + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + wpm->timestamp_firstinside_orbit = 0; + } + + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + + } else { + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + } + + } else { + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + //we're done receiving waypoints, answer with ack. + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + } + + // if (verbose) + { + if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); + break; + + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { + if (!(wp.seq == 0)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + if (!(wp.seq == wpm->current_wp_id)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + + } else if (!(wp.seq < wpm->current_count)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } + } + } else { + //we we're target but already communicating with someone else + if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid); + } else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + wpm->timestamp_lastaction = now; + + // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // Delete all waypoints + wpm->size = 0; + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); + } + + break; + } + + default: { + // if (debug) // printf("Waypoint: received message of unknown type"); + break; + } + } + + check_waypoints_reached(now, global_pos, local_pos); +} diff --git a/apps/mavlink/waypoints.h b/apps/mavlink/waypoints.h new file mode 100644 index 000000000..6011b16b2 --- /dev/null +++ b/apps/mavlink/waypoints.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file waypoints.h + * MAVLink waypoint protocol definition (BSD-relicensed). + */ + +#ifndef WAYPOINTS_H_ +#define WAYPOINTS_H_ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + +#include <v1.0/mavlink_types.h> +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *buffer, uint16_t len); + +#ifndef MAVLINK_SEND_UART_BYTES +#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) +#endif +extern mavlink_system_t mavlink_system; +#include <v1.0/common/mavlink.h> +#include <stdbool.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 15 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#ifndef MAVLINK_WPM_TEXT_FEEDBACK +#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text +#endif +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 + + +struct mavlink_wpm_storage { + mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + uint16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; +}; + +typedef struct mavlink_wpm_storage mavlink_wpm_storage; + +void mavlink_wpm_init(mavlink_wpm_storage *state); +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, + struct vehicle_local_position_s *local_pos); +void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , + struct vehicle_local_position_s *local_pos); + +#endif /* WAYPOINTS_H_ */ diff --git a/apps/mix_and_link/Makefile b/apps/mix_and_link/Makefile new file mode 100644 index 000000000..a907277c7 --- /dev/null +++ b/apps/mix_and_link/Makefile @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the mix-and-link functions +# + +include $(APPDIR)/mk/app.mk diff --git a/apps/mix_and_link/mix_and_link.c b/apps/mix_and_link/mix_and_link.c new file mode 100644 index 000000000..495f336b1 --- /dev/null +++ b/apps/mix_and_link/mix_and_link.c @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Nils Wenzler <wenzlern@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Mixing / linking of RC channels + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdint.h> + +#include "mix_and_link.h" + +/* + * Library function that mixes the abstract functions like ROLL, PITCH, THROTTLE + * to one or several actuators. + * @param *mixers is a pointer to the configuration struct, declared in mix_and_link.h + * @param nr_mixers is a integer that denotes the number of mixers that should be processed + * @param nr_actuators is a integer that denotes the max number of actuators that get linked to one source + * @param *data is a pointer to the mixer data struct, declared in mix_and_link.h + */ +void mix_and_link(const mixer_conf_t *mixers, uint8_t nr_mixers, uint8_t nr_actuators, mixer_data_t *data) +{ + + /* Reset the Output Array */ + uint8_t i, j; + + for (i = 0; i < nr_actuators; i++) { + data->output[i] = 0; + } + + /* Loop throug the given mixers */ + for (i = 0; i < nr_mixers; i++) { + + /* The actual mixing */ + for (j = 0; j < mixers[i].nr_actuators; j++) { + data->output[mixers[i].dest[j]] += (int16_t)(data->input[mixers[i].source] + * mixers[i].dual_rate[j]); + + /* Saturate to +-10000 */ + if (data->output[mixers[i].dest[j]] > 10000) + data->output[mixers[i].dest[j]] = 10000; + else if (data->output[mixers[i].dest[j]] < -10000) + data->output[mixers[i].dest[j]] = -10000; + + } + } +} + + diff --git a/apps/mix_and_link/mix_and_link.h b/apps/mix_and_link/mix_and_link.h new file mode 100644 index 000000000..0a374fd85 --- /dev/null +++ b/apps/mix_and_link/mix_and_link.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Nils Wenzler <wenzlern@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#ifndef _MIX_AND_LINK_H_ +#define _MIX_AND_LINK_H_ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdint.h> + +/**< The maximum number of actuators that get linked to one abstract function*/ +#define NR_MAX_ACTUATORS 2 + +/** + * The struct mixer_conf_t contains all the necessary information for one mixer. + * Make an array of type mixer_conf_t for several mix functions. + */ +typedef struct { + uint8_t source; /**< source RC channel index 0..n */ + uint8_t nr_actuators; /**< number of actuators to output to */ + uint8_t dest[NR_MAX_ACTUATORS]; /**< Which actuators to output to */ + float dual_rate[NR_MAX_ACTUATORS]; /**< Direction and rate of mixing. Range [-1,1] */ +} mixer_conf_t; /**< Setup for one mixer */ + +/* + * The struct mixer_data contains two int16_t arrays with the abstract source functions + * (Throttle, Roll,...) in the input, and an empty array output of the desired + * size for the results. + */ +typedef struct { + int16_t *input; + int16_t *output; +} mixer_data_t; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * Mixes the functions to the actuators. + * e.g. Throttle, Roll,... + * + * Each mixer_conf struct in the mixers array contains the source function (ex. THROTTLE), the number + * of actuators, an array with the desired actuators, Dual Rate determines which + * percentage of the source gets mixed to the destination and sets the direction. + * It's range is between -1 and 1. + * + * EXAMPLE: Deltamix for a wing plane: + * + * mixer_data_t mixer_buffer; + * mixer_buffer.input = buffer_rc; + * mixer_buffer.output = buffer_servo; + * + * + * mixer_conf_t mixers[3]; + * + * mixers[0].source = PITCH; + * mixers[0].nr_actuators = 2; + * mixers[0].dest[0] = AIL_1; + * mixers[0].dest[1] = AIL_2; + * mixers[0].dual_rate[0] = 1; + * mixers[0].dual_rate[1] = 1; + * + * mixers[1].source = ROLL; + * mixers[1].nr_actuators = 2; + * mixers[1].dest[0] = AIL_1; + * mixers[1].dest[1] = AIL_2; + * mixers[1].dual_rate[0] = 1; + * mixers[1].dual_rate[0] = -1; + * + * mixers[2].source = THROTTLE; + * mixers[2].nr_actuators = 1; + * mixers[2].dest[0] = MOT; + * mixers[2].dual_rate[0] = 1; + * + * @param mixers pointer to the mixer struct array + * @param nr_mixers number of mixers in struct array + * @param nr_actuators number of actuators + * @param data pointer to the mixer_data struct. + */ +void mix_and_link(const mixer_conf_t *mixers, uint8_t nr_mixers, uint8_t nr_actuators, mixer_data_t *data); + +#endif diff --git a/apps/mk/app.mk b/apps/mk/app.mk new file mode 100644 index 000000000..3948ef354 --- /dev/null +++ b/apps/mk/app.mk @@ -0,0 +1,223 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Common Makefile for nsh command modules and utility libraries; should be +# included by the module-specific Makefile. +# +# To build an app that appears as an nsh external command, the caller +# must define: +# +# APPNAME - the name of the application, defaults to the name +# of the parent directory. +# +# If APPNAME is not defined, a utility library is built instead. The library +# name is normally automatically determined, but it can be overridden by +# setting: +# +# LIBNAME - the name of the library, defaults to the name of the +# directory +# +# The calling makefile may also set: +# +# ASRCS - list of assembly source files, defaults to all .S +# files in the directory +# +# CSRCS - list of C source files, defaults to all .c files +# in the directory +# +# CXXSRCS - list of C++ source files, defaults to all .cpp +# files in the directory +# +# INCLUDES - list of directories to be added to the include +# search path +# +# PRIORITY - thread priority for the command (defaults to +# SCHED_PRIORITY_DEFAULT) +# +# STACKSIZE - stack size for the command (defaults to +# CONFIG_PTHREAD_STACK_DEFAULT) +# +# Symbols in the module are private to the module unless deliberately exported +# using the __EXPORT tag. +# + +############################################################################ +# No user-serviceable parts below +############################################################################ + + +############################################################################ +# Work out who included us so we can report decent errors +# +THIS_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +PARENT_MAKEFILE := $(lastword $(filter-out $(THIS_MAKEFILE),$(MAKEFILE_LIST))) + +############################################################################ +# Get configuration +# +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +############################################################################ +# Sanity-check the information we've been given and set any defaults +# +SRCDIR ?= $(dir $(PARENT_MAKEFILE)) +PRIORITY ?= SCHED_PRIORITY_DEFAULT +STACKSIZE ?= CONFIG_PTHREAD_STACK_DEFAULT + +INCLUDES += $(APPDIR) + +ASRCS ?= $(wildcard $(SRCDIR)/*.S) +CSRCS ?= $(wildcard $(SRCDIR)/*.c) +CXXSRCS ?= $(wildcard $(SRCDIR)/*.cpp) + +# if APPNAME is not set, this is a library +ifeq ($(APPNAME),) +LIBNAME ?= $(lastword $(subst /, ,$(realpath $(SRCDIR)))) +endif + +# there has to be a source file +ifeq ($(ASRCS)$(CSRCS)$(CXXSRCS),) +$(error $(realpath $(PARENT_MAKEFILE)): at least one of ASRCS, CSRCS or CXXSRCS must be set) +endif + +# check that C++ is configured if we have C++ source files and we are building +ifneq ($(CXXSRCS),) +ifneq ($(CONFIG_HAVE_CXX),y) +ifeq ($(MAKECMDGOALS),build) +$(error $(realpath $(PARENT_MAKEFILE)): cannot set CXXSRCS if CONFIG_HAVE_CXX not set in configuration) +endif +endif +endif + +############################################################################ +# Adjust compilation flags to implement EXPORT +# +CFLAGS += -fvisibility=hidden -include $(APPDIR)/systemlib/visibility.h +CXXFLAGS += -fvisibility=hidden -include $(APPDIR)/systemlib/visibility.h + +############################################################################ +# Add extra include directories +# +CFLAGS += $(addprefix -I,$(INCLUDES)) +CXXFLAGS += $(addprefix -I,$(INCLUDES)) + +############################################################################ +# Things we are going to build +# + +SRCS = $(ASRCS) $(CSRCS) $(CXXSRCS) +AOBJS = $(patsubst %.S,%.o,$(ASRCS)) +COBJS = $(patsubst %.c,%.o,$(CSRCS)) +CXXOBJS = $(patsubst %.cpp,%.o,$(CXXSRCS)) +OBJS = $(AOBJS) $(COBJS) $(CXXOBJS) + +# The prelinked object that we are ultimately going to build +ifneq ($(APPNAME),) +PRELINKOBJ = $(APPNAME).pre.o +else +PRELINKOBJ = $(LIBNAME).pre.o +endif + +# The archive that the object file will be placed in +# XXX does WINTOOL ever get set? +ifeq ($(WINTOOL),y) + INCDIROPT = -w + BIN = "$(shell cygpath -w $(APPDIR)/libapps$(LIBEXT))" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +############################################################################ +# Rules for building things +# + +all: .built +.PHONY: clean depend distclean + +# +# Top-level build; add prelinked object to the apps archive +# +.built: $(PRELINKOBJ) + @$(call ARCHIVE, $(BIN), $(PRELINKOBJ)) + @touch $@ + +# +# Source dependencies +# +depend: .depend +.depend: $(MAKEFILE_LIST) $(SRCS) + @$(MKDEP) --dep-path . $(CC) -- $(CFLAGS) -- $(CSRCS) >Make.dep + @$(MKDEP) --dep-path . $(CXX) -- $(CXXFLAGS) -- $(CXXSRCS) >>Make.dep + @touch $@ + +ifneq ($(APPNAME),) +# +# App registration +# +context: .context +.context: $(MAKEFILE_LIST) + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ +else +context: +endif + +# +# Object files +# +$(PRELINKOBJ): $(OBJS) + $(call PRELINK, $@, $(OBJS)) + +$(AOBJS): %.o : %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %.o : %.c + $(call COMPILE, $<, $@) + +$(CXXOBJS): %.o : %.cpp + $(call COMPILEXX, $<, $@) + +# +# Tidying up +# +clean: + @rm -f $(OBJS) $(PRELINKOBJ) Make.dep .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep diff --git a/apps/namedapp/Kconfig b/apps/namedapp/Kconfig new file mode 100644 index 000000000..8d8f03421 --- /dev/null +++ b/apps/namedapp/Kconfig @@ -0,0 +1,15 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config NAMEDAPP + bool "Support named applications" + default n + ---help--- + Enable support for named applications. This features assigns a string + name to an application. This feature is also the underlying requirement + to support built-in applications in the NuttShell (NSH). + +if NAMEDAPP +endif diff --git a/apps/namedapp/Make.defs b/apps/namedapp/Make.defs new file mode 100644 index 000000000..a07b3b3c2 --- /dev/null +++ b/apps/namedapp/Make.defs @@ -0,0 +1,40 @@ +############################################################################ +# apps/namedapps/Make.defs +# Adds selected applications to apps/ build +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +if ($(CONFIG_NAMEDAPP),y) +CONFIGURED_APPS += namedapp +endif + diff --git a/apps/namedapp/Makefile b/apps/namedapp/Makefile new file mode 100644 index 000000000..fba8fb1ff --- /dev/null +++ b/apps/namedapp/Makefile @@ -0,0 +1,106 @@ +############################################################################ +# apps/nshlib/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# NSH Library + +# Source and object files + +ASRCS = +CSRCS = namedapp.c exec_namedapp.c + +ifeq ($(CONFIG_APPS_BINDIR),y) +CSRCS += binfs.c +endif + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . +VPATH = + +# Build Targets + +all: .built +.PHONY: .context context depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: + @echo "/* List of application requirements, generated during make context. */" > namedapp_list.h + @echo "/* List of application entry points, generated during make context. */" > namedapp_proto.h + @touch $@ + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f .context Make.dep .depend + @rm -f namedapp_list.h + @rm -f namedapp_proto.h + +-include Make.dep + diff --git a/apps/namedapp/binfs.c b/apps/namedapp/binfs.c new file mode 100644 index 000000000..36e3ace92 --- /dev/null +++ b/apps/namedapp/binfs.c @@ -0,0 +1,596 @@ +/**************************************************************************** + * apps/namedapps/binfs.c + * + * 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 + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/statfs.h> +#include <sys/stat.h> + +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <assert.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/fs/fs.h> +#include <nuttx/fs/dirent.h> + +#include "namedapp.h" + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && defined(CONFIG_APPS_BINDIR) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* This structure represents the overall mountpoint state. An instance of this + * structure is retained as inode private data on each mountpoint that is + * mounted with a fat32 filesystem. + */ + +struct binfs_state_s +{ + sem_t bm_sem; /* Used to assume thread-safe access */ +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static void binfs_semtake(struct binfs_state_s *bm); +static inline void binfs_semgive(struct binfs_state_s *bm); +static int binfs_open(FAR struct file *filep, const char *relpath, + int oflags, mode_t mode); +static int binfs_close(FAR struct file *filep); +static ssize_t binfs_read(FAR struct file *filep, char *buffer, size_t buflen); +static int binfs_ioctl(FAR struct file *filep, int cmd, unsigned long arg); + +static int binfs_opendir(struct inode *mountpt, const char *relpath, + struct fs_dirent_s *dir); +static int binfs_readdir(struct inode *mountpt, struct fs_dirent_s *dir); +static int binfs_rewinddir(struct inode *mountpt, struct fs_dirent_s *dir); + +static int binfs_bind(FAR struct inode *blkdriver, const void *data, + void **handle); +static int binfs_unbind(void *handle, FAR struct inode **blkdriver); +static int binfs_statfs(struct inode *mountpt, struct statfs *buf); + +static int binfs_stat(struct inode *mountpt, const char *relpath, struct stat *buf); + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/* See fs_mount.c -- this structure is explicitly externed there. + * We use the old-fashioned kind of initializers so that this will compile + * with any compiler. + */ + +const struct mountpt_operations binfs_operations = +{ + binfs_open, /* open */ + binfs_close, /* close */ + binfs_read, /* read */ + NULL, /* write */ + NULL, /* seek */ + binfs_ioctl, /* ioctl */ + NULL, /* sync */ + + binfs_opendir, /* opendir */ + NULL, /* closedir */ + binfs_readdir, /* readdir */ + binfs_rewinddir, /* rewinddir */ + + binfs_bind, /* bind */ + binfs_unbind, /* unbind */ + binfs_statfs, /* statfs */ + + NULL, /* unlink */ + NULL, /* mkdir */ + NULL, /* rmdir */ + NULL, /* rename */ + binfs_stat /* stat */ +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: binfs_semtake + ****************************************************************************/ + +static void binfs_semtake(struct binfs_state_s *bm) +{ + /* Take the semaphore (perhaps waiting) */ + + while (sem_wait(&bm->bm_sem) != 0) + { + /* The only case that an error should occur here is if + * the wait was awakened by a signal. + */ + + ASSERT(errno == EINTR); + } +} + +/**************************************************************************** + * Name: binfs_semgive + ****************************************************************************/ + +static inline void binfs_semgive(struct binfs_state_s *bm) +{ + sem_post(&bm->bm_sem); +} + +/**************************************************************************** + * Name: binfs_open + ****************************************************************************/ + +static int binfs_open(FAR struct file *filep, const char *relpath, + int oflags, mode_t mode) +{ + struct binfs_state_s *bm; + int ret = -ENOSYS; + + fvdbg("Open '%s'\n", relpath); + + /* Sanity checks */ + + DEBUGASSERT(filep->f_priv == NULL && filep->f_inode != NULL); + + /* mountpoint private data from the inode reference from the file + * structure + */ + + bm = (struct binfs_state_s*)filep->f_inode->i_private; + DEBUGASSERT(bm != NULL); + + /* BINFS is read-only. Any attempt to open with any kind of write + * access is not permitted. + */ + + if ((oflags & O_WRONLY) != 0 || (oflags & O_RDONLY) == 0) + { + fdbg("Only O_RDONLY supported\n"); + ret = -EACCES; + } + + /* Save open-specific state in filep->f_priv */ + + /* Opening of elements within the pseudo-file system is not yet supported */ + + return ret; +} + +/**************************************************************************** + * Name: binfs_close + ****************************************************************************/ + +static int binfs_close(FAR struct file *filep) +{ + struct binfs_state_s *bm; + int ret = -ENOSYS; + + fvdbg("Closing\n"); + + /* Sanity checks */ + + DEBUGASSERT(filep->f_priv != NULL && filep->f_inode != NULL); + + /* Recover the open file state from the struct file instance */ + /* bf = filep->f_priv; */ + + /* Recover the file system state from the inode */ + + bm = filep->f_inode->i_private; + DEBUGASSERT(bm != NULL); + + /* Free the open file state */ + /* free(bf); */ + + filep->f_priv = NULL; + + /* Since open() is not yet supported, neither is close(). */ + + return ret; +} + +/**************************************************************************** + * Name: binfs_read + ****************************************************************************/ + +static ssize_t binfs_read(FAR struct file *filep, char *buffer, size_t buflen) +{ + struct binfs_state_s *bm; + + fvdbg("Read %d bytes from offset %d\n", buflen, filep->f_pos); + + /* Sanity checks */ + + DEBUGASSERT(filep->f_priv != NULL && filep->f_inode != NULL); + + /* Recover the open file state data from the struct file instance */ + /* bf = filep->f_priv; */ + + /* Recover the file system state from the inode */ + + bm = filep->f_inode->i_private; + DEBUGASSERT(bm != NULL); + + /* Since open is not yet supported, neither is reading */ + + return -ENOSYS; +} + +/**************************************************************************** + * Name: binfs_ioctl + ****************************************************************************/ + +static int binfs_ioctl(FAR struct file *filep, int cmd, unsigned long arg) +{ + struct binfs_state_s *bm; + + fvdbg("cmd: %d arg: %08lx\n", cmd, arg); + + /* Sanity checks */ + + DEBUGASSERT(filep->f_priv != NULL && filep->f_inode != NULL); + + /* Recover the open file state from the struct file instance */ + /* bf = filep->f_priv; */ + + /* Recover the file system state from the inode */ + + bm = filep->f_inode->i_private; + DEBUGASSERT(bm != NULL); + + /* No ioctl commands yet supported */ + + return -ENOTTY; +} + +/**************************************************************************** + * Name: binfs_opendir + * + * Description: + * Open a directory for read access + * + ****************************************************************************/ + +static int binfs_opendir(struct inode *mountpt, const char *relpath, + struct fs_dirent_s *dir) +{ + struct binfs_state_s *bm; + int ret; + + fvdbg("relpath: \"%s\"\n", relpath ? relpath : "NULL"); + + /* Sanity checks */ + + DEBUGASSERT(mountpt != NULL && mountpt->i_private != NULL); + + /* Recover the file system state from the inode instance */ + + bm = mountpt->i_private; + binfs_semtake(bm); + + /* The requested directory must be the volume-relative "root" directory */ + + if (relpath && relpath[0] != '\0') + { + ret = -ENOENT; + goto errout_with_semaphore; + } + + /* Set the index to the first entry */ + + dir->u.binfs.fb_index = 0; + ret = OK; + +errout_with_semaphore: + binfs_semgive(bm); + return ret; +} + +/**************************************************************************** + * Name: binfs_readdir + * + * Description: Read the next directory entry + * + ****************************************************************************/ + +static int binfs_readdir(struct inode *mountpt, struct fs_dirent_s *dir) +{ + struct binfs_state_s *bm; + unsigned int index; + int ret; + + /* Sanity checks */ + + DEBUGASSERT(mountpt != NULL && mountpt->i_private != NULL); + + /* Recover the file system state from the inode instance */ + + bm = mountpt->i_private; + binfs_semtake(bm); + + /* Have we reached the end of the directory */ + + index = dir->u.binfs.fb_index; + if (namedapps[index].name == NULL) + { + /* We signal the end of the directory by returning the + * special error -ENOENT + */ + + fvdbg("Entry %d: End of directory\n", index); + ret = -ENOENT; + } + else + { + /* Save the filename and file type */ + + fvdbg("Entry %d: \"%s\"\n", index, namedapps[index].name); + dir->fd_dir.d_type = DTYPE_FILE; + strncpy(dir->fd_dir.d_name, namedapps[index].name, NAME_MAX+1); + + /* The application list is terminated by an entry with a NULL name. + * Therefore, there is at least one more entry in the list. + */ + + index++; + + /* Set up the next directory entry offset. NOTE that we could use the + * standard f_pos instead of our own private fb_index. + */ + + dir->u.binfs.fb_index = index; + ret = OK; + } + + binfs_semgive(bm); + return ret; +} + +/**************************************************************************** + * Name: binfs_rewindir + * + * Description: Reset directory read to the first entry + * + ****************************************************************************/ + +static int binfs_rewinddir(struct inode *mountpt, struct fs_dirent_s *dir) +{ + struct binfs_state_s *bm; + + fvdbg("Entry\n"); + + /* Sanity checks */ + + DEBUGASSERT(mountpt != NULL && mountpt->i_private != NULL); + + /* Recover the file system state from the inode instance */ + + bm = mountpt->i_private; + binfs_semtake(bm); + + dir->u.binfs.fb_index = 0; + + binfs_semgive(bm); + return OK; +} + +/**************************************************************************** + * Name: binfs_bind + * + * Description: This implements a portion of the mount operation. This + * function allocates and initializes the mountpoint private data and + * binds the blockdriver inode to the filesystem private data. The final + * binding of the private data (containing the blockdriver) to the + * mountpoint is performed by mount(). + * + ****************************************************************************/ + +static int binfs_bind(FAR struct inode *blkdriver, const void *data, + void **handle) +{ + struct binfs_state_s *bm; + + fvdbg("Entry\n"); + + /* Create an instance of the mountpt state structure */ + + bm = (struct binfs_state_s *)zalloc(sizeof(struct binfs_state_s)); + if (!bm) + { + fdbg("Failed to allocate mountpoint structure\n"); + return -ENOMEM; + } + + /* Initialize the allocated mountpt state structure. The filesystem is + * responsible for one reference ont the blkdriver inode and does not + * have to addref() here (but does have to release in ubind(). + */ + + sem_init(&bm->bm_sem, 0, 1); /* Initialize the semaphore that controls access */ + + /* Mounted! */ + + *handle = (void*)bm; + return OK; +} + +/**************************************************************************** + * Name: binfs_unbind + * + * Description: This implements the filesystem portion of the umount + * operation. + * + ****************************************************************************/ + +static int binfs_unbind(void *handle, FAR struct inode **blkdriver) +{ + struct binfs_state_s *bm = (struct binfs_state_s*)handle; + + fvdbg("Entry\n"); + +#ifdef CONFIG_DEBUG + if (!bm) + { + return -EINVAL; + } +#endif + + /* Check if there are sill any files opened on the filesystem. */ + + /* Release the mountpoint private data */ + + sem_destroy(&bm->bm_sem); + return OK; +} + +/**************************************************************************** + * Name: binfs_statfs + * + * Description: Return filesystem statistics + * + ****************************************************************************/ + +static int binfs_statfs(struct inode *mountpt, struct statfs *buf) +{ + struct binfs_state_s *bm; + + fvdbg("Entry\n"); + + /* Sanity checks */ + + DEBUGASSERT(mountpt && mountpt->i_private); + + /* Get the mountpoint private data from the inode structure */ + + bm = mountpt->i_private; + binfs_semtake(bm); + + /* Fill in the statfs info */ + + memset(buf, 0, sizeof(struct statfs)); + buf->f_type = BINFS_MAGIC; + buf->f_bsize = 0; + buf->f_blocks = 0; + buf->f_bfree = 0; + buf->f_bavail = 0; + buf->f_namelen = NAME_MAX; + + binfs_semgive(bm); + return OK; +} + +/**************************************************************************** + * Name: binfs_stat + * + * Description: Return information about a file or directory + * + ****************************************************************************/ + +static int binfs_stat(struct inode *mountpt, const char *relpath, struct stat *buf) +{ + struct binfs_state_s *bm; + int ret; + + fvdbg("Entry\n"); + + /* Sanity checks */ + + DEBUGASSERT(mountpt && mountpt->i_private); + + /* Get the mountpoint private data from the inode structure */ + + bm = mountpt->i_private; + binfs_semtake(bm); + + /* The requested directory must be the volume-relative "root" directory */ + + if (relpath && relpath[0] != '\0') + { + /* Check if there is a file with this name. */ + + if (namedapp_isavail(relpath) < 0) + { + ret = -ENOENT; + goto errout_with_semaphore; + } + + /* It's a execute-only file name */ + + buf->st_mode = S_IFREG|S_IXOTH|S_IXGRP|S_IXUSR; + } + else + { + /* It's a read/execute-only directory name */ + + buf->st_mode = S_IFDIR|S_IROTH|S_IRGRP|S_IRUSR|S_IXOTH|S_IXGRP|S_IXUSR; + } + + /* File/directory size, access block size */ + + buf->st_size = 0; + buf->st_blksize = 0; + buf->st_blocks = 0; + ret = OK; + +errout_with_semaphore: + binfs_semgive(bm); + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +#endif /* !CONFIG_DISABLE_MOUNTPOINT && CONFIG_APPS_BINDIR */ + diff --git a/apps/namedapp/exec_namedapp.c b/apps/namedapp/exec_namedapp.c new file mode 100644 index 000000000..264fca7b9 --- /dev/null +++ b/apps/namedapp/exec_namedapp.c @@ -0,0 +1,187 @@ +/**************************************************************************** + * apps/namedaps/exec_namedapp.c + * + * Copyright (C) 2011 Uros Platise. All rights reserved. + * Author: Uros Platise <uros.platise@isotel.eu> + * + * With updates, modifications, and general maintenance by: + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Auther: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <apps/apps.h> +#include <sched.h> + +#include <string.h> +#include <errno.h> + +#include "namedapp.h" + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: namedapp_getname + * + * Description: + * Return the name of the application at index in the table of named + * applications. + * + ****************************************************************************/ + +const char *namedapp_getname(int index) +{ + if (index < 0 || index >= number_namedapps()) + { + return NULL; + } + + return namedapps[index].name; +} + +/**************************************************************************** + * Name: namedapp_isavail + * + * Description: + * Return the index into the table of applications for the applicaiton with + * the name 'appname'. + * + ****************************************************************************/ + +int namedapp_isavail(FAR const char *appname) +{ + int i; + + for (i = 0; namedapps[i].name; i++) + { + if (!strcmp(namedapps[i].name, appname)) + { + return i; + } + } + + set_errno(ENOENT); + return ERROR; +} + +/**************************************************************************** + * Name: namedapp_isavail + * + * Description: + * Execute the application with name 'appname', providing the arguments + * in the argv[] array. + * + * Returned Value: + * On success, the task ID of the named application is returned. On + * failure, -1 (ERROR) is returned an the errno value is set appropriately. + * + ****************************************************************************/ + +int exec_namedapp(FAR const char *appname, FAR const char **argv) +{ + pid_t pid; + int index; + + /* Verify that an application with this name exists */ + + index = namedapp_isavail(appname); + if (index >= 0) + { + /* Disable pre-emption. This means that although we start the named + * application here, it will not actually run until pre-emption is + * re-enabled below. + */ + + sched_lock(); + + /* Start the named application task */ + + pid = TASK_CREATE(namedapps[index].name, namedapps[index].priority, + namedapps[index].stacksize, namedapps[index].main, + (argv) ? &argv[1] : (const char **)NULL); + + /* If robin robin scheduling is enabled, then set the scheduling policy + * of the new task to SCHED_RR before it has a chance to run. + */ + +#if CONFIG_RR_INTERVAL > 0 + if (pid > 0) + { + struct sched_param param; + + /* Pre-emption is disabled so the task creation and the + * following operation will be atomic. The priority of the + * new task cannot yet have changed from its initial value. + */ + + param.sched_priority = namedapps[index].priority; + sched_setscheduler(pid, SCHED_RR, ¶m); + } +#endif + /* Now let the named application run */ + + sched_unlock(); + + /* Return the task ID of the new task if the task was sucessfully + * started. Otherwise, pid will be ERROR (and the errno value will + * be set appropriately). + */ + + return pid; + } + + /* Return ERROR with errno set appropriately */ + + return ERROR; +} diff --git a/apps/namedapp/namedapp.c b/apps/namedapp/namedapp.c new file mode 100644 index 000000000..a5c2bee0e --- /dev/null +++ b/apps/namedapp/namedapp.c @@ -0,0 +1,96 @@ +/**************************************************************************** + * apps/namedaps/namedapp.c + * + * Copyright (C) 2011 Uros Platise. All rights reserved. + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Authors: Uros Platise <uros.platise@isotel.eu> + * Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <apps/apps.h> + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +#include "namedapp_proto.h" + +const struct namedapp_s namedapps[] = +{ +# include "namedapp_list.h" + { NULL, 0, 0, 0 } +}; + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int number_namedapps(void) +{ + return sizeof(namedapps)/sizeof(struct namedapp_s) - 1; +} + + diff --git a/apps/namedapp/namedapp.h b/apps/namedapp/namedapp.h new file mode 100644 index 000000000..7fcdf42dc --- /dev/null +++ b/apps/namedapp/namedapp.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * apps/namedaps/namedapp.h + * + * Copyright (C) 2011 Uros Platise. All rights reserved. + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. + * Authors: Uros Platise <uros.platise@isotel.eu> + * Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_NAMEDAPP_NAMEDAPP_H +#define __APPS_NAMEDAPP_NAMEDAPP_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <apps/apps.h> + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN const struct namedapp_s namedapps[]; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +EXTERN int number_namedapps(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __APPS_NAMEDAPP_NAMEDAPP_H */ + diff --git a/apps/nshlib/Kconfig b/apps/nshlib/Kconfig new file mode 100644 index 000000000..7e419bdde --- /dev/null +++ b/apps/nshlib/Kconfig @@ -0,0 +1,484 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config NSH_LIBRARY + bool "NSH Library" + default n + ---help--- + Build the NSH support library. This is used, for example, by examples/nsh + in order to implement the full NuttShell (NSH). + +if NSH_LIBRARY +config NSH_BUILTIN_APPS + bool "Enable built-in applications" + default y + depends on NAMEDAPP + ---help--- + Support external registered, "named" applications that can be + executed from the NSH command line (see apps/README.txt for + more information). This options requires support for named applications + (NAMEDAPP). + +menu "Disable Individual commands" +config NSH_DISABLE_CAT + bool "Disable cat" + default n +config NSH_DISABLE_CD + bool "Disable cd" + default n +config NSH_DISABLE_CP + bool "Disable cp" + default n +config NSH_DISABLE_DD + bool "Disable dd" + default n +config NSH_DISABLE_ECHO + bool "Disable echo" + default n +config NSH_DISABLE_EXEC + bool "Disable exec" + default n +config NSH_DISABLE_EXIT + bool "Disable exit" + default n +config NSH_DISABLE_FREE + bool "Disable free" + default n +config NSH_DISABLE_GET + bool "Disable get" + default n +config NSH_DISABLE_HELP + bool "Disable help" + default n +config NSH_DISABLE_IFCONFIG + bool "Disable ifconfig" + default n +config NSH_DISABLE_KILL + bool "Disable kill" + default n +config NSH_DISABLE_LOSETUP + bool "Disable losetup" + default n +config NSH_DISABLE_LS + bool "Disable ls" + default n +config NSH_DISABLE_MB + bool "Disable mb" + default n +config NSH_DISABLE_MKDIR + bool "Disable mkdir" + default n +config NSH_DISABLE_MKFATFS + bool "Disable mkfatfs" + default n +config NSH_DISABLE_MKFIFO + bool "Disable mkfifo" + default n +config NSH_DISABLE_MKRD + bool "Disable mkrd" + default n +config NSH_DISABLE_MH + bool "Disable mh" + default n +config NSH_DISABLE_MOUNT + bool "Disable mount" + default n +config NSH_DISABLE_MW + bool "Disable mw" + default n +config NSH_DISABLE_NSFMOUNT + bool "Disable nfsmount" + default n +config NSH_DISABLE_PS + bool "Disable ps" + default n +config NSH_DISABLE_PING + bool "Disable ping" + default n +config NSH_DISABLE_PUT + bool "Disable put" + default n +config NSH_DISABLE_PWD + bool "Disable pwd" + default n +config NSH_DISABLE_RM + bool "Disable rm" + default n +config NSH_DISABLE_RMDIR + bool "Disable rmdir" + default n +config NSH_DISABLE_SET + bool "Disable set" + default n +config NSH_DISABLE_SH + bool "Disable sh" + default n +config NSH_DISABLE_SLEEP + bool "Disable sleep" + default n +config NSH_DISABLE_TEST + bool "Disable test" + default n +config NSH_DISABLE_UMOUNT + bool "Disable umount" + default n +config NSH_DISABLE_UNSET + bool "Disable unset" + default n +config NSH_DISABLE_USLEEP + bool "Disable usleep" + default n +config NSH_DISABLE_WGET + bool "Disable wget" + default n +config NSH_DISABLE_XD + bool "Disable xd" + default n +endmenu + +config NSH_FILEIOSIZE + int "NSH I/O buffer size" + default 1024 + ---help--- + Size of a static I/O buffer used for file access (ignored if + there is no filesystem). Default is 1024. + +config NSH_STRERROR + bool "Use strerror()" + default n + ---help--- + strerror(errno) makes more readable output but strerror() is + very large and will not be used unless this setting is 'y' + +config NSH_LINELEN + int "Max command line length" + default 80 + ---help--- + The maximum length of one command line and of one output line. + Default: 80 + +config NSH_NESTDEPTH + int "Maximum command nesting" + default 3 + ---help--- + The maximum number of nested if-then[-else]-fi sequences that + are permissable. Default: 3 + +config NSH_DISABLESCRIPT + bool "Disable script support" + default n + ---help--- + This can be set to 'y' to suppress support for scripting. This + setting disables the 'sh', 'test', and '[' commands and the + if-then[-else]-fi construct. This would only be set on systems + where a minimal footprint is a necessity and scripting is not. + +config NSH_DISABLEBG + bool "Disable background commands" + default n + ---help--- + This can be set to 'y' to suppress support for background + commands. This setting disables the 'nice' command prefix and + the '&' command suffix. This would only be set on systems + where a minimal footprint is a necessity and background command + execution is not. + +config NSH_MMCSDMINOR + int "MMC/SD minor device number" + default 0 + ---help--- + If the architecture supports an MMC/SD slot and if the NSH + architecture specific logic is present, this option will provide + the MMC/SD minor number, i.e., the MMC/SD block driver will + be registered as /dev/mmcsdN where N is the minor number. + Default is zero. + +config NSH_ROMFSETC + bool "Support ROMFS start-up script" + default n + ---help--- + Mount a ROMFS filesystem at /etc and provide a startup script + at /etc/init.d/rcS. The default startup script will mount + a FAT FS RAMDISK at /tmp but the logic is easily extensible. + +endif + +if NSH_ROMFSETC +config NSH_ROMFSMOUNTPT + string "ROMFS mount point" + default "/etc" + ---help--- + The default mountpoint for the ROMFS volume is /etc, but that + can be changed with this setting. This must be a absolute path + beginning with '/'. + +config NSH_INITSCRIPT + string "Relative path to startup script" + default "init.d/rcS" + ---help--- + This is the relative path to the startup script within the mountpoint. + The default is init.d/rcS. This is a relative path and must not + start with '/'. + +config NSH_ROMFSDEVNO + int "ROMFS block device minor number" + default 0 + ---help--- + This is the minor number of the ROMFS block device. The default is + '0' corresponding to /dev/ram0. + +config NSH_ROMFSSECTSIZE + int "ROMFS sector size" + default 64 + ---help--- + This is the sector size to use with the ROMFS volume. Since the + default volume is very small, this defaults to 64 but should be + increased if the ROMFS volume were to be become large. Any value + selected must be a power of 2. + +config NSH_FATDEVNO + int "FAT block device minor number" + default 0 + ---help--- + When the default rcS file used when NSH_ROMFSETC is selected, it + will mount a FAT FS under /tmp. This is the minor number of the FAT + FS block device. The default is '1' corresponding to /dev/ram1. + +config NSH_FATSECTSIZE + int "FAT sector size" + default 512 + ---help--- + When the default rcS file used when NSH_ROMFSETC is selected, it + will mount a FAT FS under /tmp. This is the sector size use with the + FAT FS. Default is 512. + +config NSH_FATNSECTORS + int "FAT number of sectors" + default 1024 + ---help--- + When the default rcS file used when NSH_ROMFSETC is selected, it + will mount a FAT FS under /tmp. This is the number of sectors to use + with the FAT FS. Defualt is 1024. The amount of memory used by the + FAT FS will be NSH_FATSECTSIZE * NSH_FATNSECTORS bytes. + +config NSH_FATMOUNTPT + string "FAT mount point" + default 512 + ---help--- + When the default rcS file used when NSH_ROMFSETC is selected, it + will mount a FAT FS under /tmp. This is the location where the FAT + FS will be mounted. Default is /tmp. +endif + +if NSH_LIBRARY +config NSH_CONSOLE + bool "Use console" + default y + ---help--- + If NSH_CONSOLE is set to 'y', then a character driver + console front-end is selected (/dev/console). + + Normally, the serial console device is a UART and RS-232 + interface. However, if CONFIG_USBDEV is defined, then a USB + serial device may, instead, be used if the one of + the following are defined: + + CONFIG_PL2303 and CONFIG_PL2303_CONSOLE - Sets up the + Prolifics PL2303 emulation as a console device at /dev/console. + + CONFIG_CDCACM and CONFIG_CDCACM_CONSOLE - Sets up the + CDC/ACM serial device as a console device at dev/console. + + CONFIG_NSH_USBCONSOLE and CONFIG_NSH_USBCONDEV - Sets up the + some other USB serial device as the NSH console (not necessarily + dev/console). + +config NSH_USBCONSOLE + bool "Use a USB console" + default n + depends on NSH_CONSOLE && USBDEV + ---help--- + If defined, then the an arbitrary USB device may be used + to as the NSH console. In this case, CONFIG_NSH_USBCONDEV + must be defined to indicate which USB device to use as + the console. + +config NSH_USBCONDEV + string "USB console device" + default "/dev/ttyACM0" + depends on NSH_USBCONSOLE + ---help--- + If CONFIG_NSH_USBCONSOLE is set to 'y', then CONFIG_NSH_USBCONDEV + must also be set to select the USB device used to support + the NSH console. This should be set to the quoted name of a + readable/write-able USB driver such as: + CONFIG_NSH_USBCONDEV="/dev/ttyACM0". + +config UBSDEV_MINOR + int "USB console device minor number" + default 0 + depends on NSH_USBCONSOLE + ---help--- + If there are more than one USB devices, then a USB device + minor number may also need to be provided. Default: 0 + +menu "USB Trace Support" + depends on USBDEV && (DEBUG || USBDEV_TRACE) + +config NSH_USBDEV_TRACEINIT + bool "Show initialization events" + default n + ---help--- + Show initialization events + +config NSH_USBDEV_TRACECLASS + bool "Show class driver events" + default n + ---help--- + Show class driver events + +config NSH_USBDEV_TRACETRANSFERS + bool "Show data transfer events" + default n + ---help--- + Show data transfer events + +config NSH_USBDEV_TRACECONTROLLER + bool "Show controller events" + default n + ---help--- + Show controller events + +config NSH_USBDEV_TRACEINTERRUPTS + bool "Show interrupt-related events" + default n + ---help--- + Show interrupt-related events + +endmenu + +config NSH_CONDEV + bool "Default console device" + default "/dev/console" + depends on NSH_CONSOLE && !NSH_USBCONSOLE + ---help--- + If NSH_CONSOLE is set to 'y', then NSH_CONDEV + may also be set to select the serial device used to support + the NSH console. This should be set to the quoted name of a + readable/write-able character driver such as: + NSH_CONDEV="/dev/ttyS1". This is useful, for example, + to separate the NSH command line from the system console when + the system console is used to provide debug output. Default: + stdin and stdout (probably "/dev/console") + + NOTE: When any other device other than /dev/console is used + for a user interface, (1) linefeeds (\n) will not be expanded to + carriage return / linefeeds (\r\n). You will need to set + your terminal program to account for this. And (2) input is + not automatically echoed so you will have to turn local echo on. + +config NSH_ARCHINIT + bool "Have architecture-specific initialization" + default n + ---help--- + Set if your board provides architecture specific initialization + via the board-specific function nsh_archinitialize(). This + function will be called early in NSH initialization to allow + board logic to do such things as configure MMC/SD slots. + +config NSH_TELNET + bool "Use Telnet console" + default n + depends on NET && NET_TCP + ---help--- + If NSH_TELNET is set to 'y', then a TELENET + server front-end is selected. When this option is provided, + you may log into NuttX remotely using telnet in order to + access NSH. + +endif + +if NSH_TELNET +config NSH_TELNETD_PORT + int "Telnet port number" + default 23 + ---help--- + The telnet daemon will listen on this TCP port number for connections. + Default: 23 + +config NSH_TELNETD_DAEMONPRIO + int "Telnet daemon priority" + default 100 + ---help--- + Priority of the Telnet daemon. Default: 100 + +config NSH_TELNETD_DAEMONSTACKSIZE + int "Telnet daemon stack size" + default 2048 + ---help--- + Stack size allocated for the Telnet daemon. Default: 2048 + +config NSH_TELNETD_CLIENTPRIO + int "Telnet client priority" + default 100 + ---help--- + Priority of the Telnet client. Default: 100 + +config NSH_TELNETD_CLIENTSTACKSIZE + int "Telnet client stack size" + default 2048 + ---help--- + Stack size allocated for the Telnet client. Default: 2048 + +config NSH_IOBUFFER_SIZE + int "Telnet I/O buffer size" + default 512 + ---help--- + Determines the size of the I/O buffer to use for sending/ + receiving TELNET commands/reponses. Default: 512 +endif + +config NSH_DHCPC + bool "Use DHCP to get IP address" + default n + depends on NSH_LIBRARY && NET && NET_UDP && NET_BROADCAST + ---help--- + Obtain the IP address via DHCP. + + Per RFC2131 (p. 9), the DHCP client must be prepared to receive DHCP + messages of up to 576 bytes (excluding Ethernet, IP, or UDP headers and FCS). + +config NSH_IPADDR + hex "Target IP address" + default 0x10000002 + depends on NSH_LIBRARY && NET && !NSH_DHCPC + ---help--- + If NSH_DHCPC is NOT set, then the static IP address must be provided. + This is a 32-bit integer value in host order. So, as an example, + 0x10000002 would be 10.0.0.2. + +config NSH_DRIPADDR + hex "Router IP address" + default 0x10000001 + depends on NSH_LIBRARY && NET && !NSH_DHCPC + ---help--- + Default router IP address (aka, Gateway). This is a 32-bit integer + value in host order. So, as an example, 0x10000001 would be 10.0.0.1. + +config NSH_NETMASK + hex "Network mask" + default 0xffffff00 + depends on NSH_LIBRARY && NET && !NSH_DHCPC + ---help--- + Network mask. This is a 32-bit integer value in host order. So, as + an example, 0xffffff00 would be 255.255.255.0. + +config NSH_NOMAC + bool "Hardware has no MAC address" + default n + depends on NSH_LIBRARY && NET + ---help--- + Set if your ethernet hardware has no built-in MAC address. + If set, a bogus MAC will be assigned. diff --git a/apps/nshlib/Make.defs b/apps/nshlib/Make.defs new file mode 100644 index 000000000..c72c09bce --- /dev/null +++ b/apps/nshlib/Make.defs @@ -0,0 +1,40 @@ +############################################################################ +# apps/nshlib/Make.defs +# Adds selected applications to apps/ build +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +if ($(CONFIG_NSH_LIBRARY),y) +CONFIGURED_APPS += nshlib +endif + diff --git a/apps/nshlib/Makefile b/apps/nshlib/Makefile new file mode 100644 index 000000000..f616374bf --- /dev/null +++ b/apps/nshlib/Makefile @@ -0,0 +1,131 @@ +############################################################################ +# apps/nshlib/Makefile +# +# 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 +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# NSH Library + +ASRCS = +CSRCS = nsh_init.c nsh_parse.c nsh_console.c nsh_fscmds.c nsh_ddcmd.c \ + nsh_proccmds.c nsh_mmcmds.c nsh_envcmds.c nsh_dbgcmds.c + +ifeq ($(CONFIG_NSH_BUILTIN_APPS),y) +CSRCS += nsh_apps.c +endif + +ifeq ($(CONFIG_NSH_ROMFSETC),y) +CSRCS += nsh_romfsetc.c +endif + +ifeq ($(CONFIG_NET),y) +CSRCS += nsh_netinit.c nsh_netcmds.c +endif + +ifeq ($(CONFIG_RTC),y) +CSRCS += nsh_timcmds.c +endif + +ifneq ($(CONFIG_DISABLE_MOUNTPOINT),y) +CSRCS += nsh_mntcmds.c +endif + +ifeq ($(CONFIG_NSH_CONSOLE),y) +CSRCS += nsh_consolemain.c +endif + +ifeq ($(CONFIG_NSH_TELNET),y) +CSRCS += nsh_telnetd.c +endif + +ifneq ($(CONFIG_NSH_DISABLESCRIPT),y) +CSRCS += nsh_test.c +endif + +ifeq ($(CONFIG_USBDEV),y) +CSRCS += nsh_usbdev.c +endif + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . +VPATH = + +# Build targets + +all: .built +.PHONY: context .depend depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +context: + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) \ + $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep + diff --git a/apps/nshlib/README.txt b/apps/nshlib/README.txt new file mode 100644 index 000000000..543b9a448 --- /dev/null +++ b/apps/nshlib/README.txt @@ -0,0 +1,1159 @@ +apps/nshlib +^^^^^^^^^^^ + + This directory contains the NuttShell (NSH) library. This library can be + linked with other logic to provide a simple shell application for NuttX. + + - Console/NSH Front End + - Command Overview + - Conditional Command Execution + - Built-In Variables + - Current Working Directory + Environment Variables + - NSH Start-Up Script + - Simple Commands + - NSH Configuration Settings + Command Dependencies on Configuration Settings + NSH-Specific Configuration Settings + - Common Problems + +Console/NSH Front End +^^^^^^^^^^^^^^^^^^^^^ + + Using settings in the configuration file, NSH may be configured to + use either the serial stdin/out or a telnet connection as the console + or BOTH. When NSH is started, you will see the following welcome on + either console: + + NuttShell (NSH) + nsh> + + 'nsh>' is the NSH prompt and indicates that you may enter a command + from the console. + +Command Overview +^^^^^^^^^^^^^^^^ + + This directory contains the NuttShell (NSH). This is a simple + shell-like application. At present, NSH supports the following commands + forms: + + Simple command: <cmd> + Command with re-directed output: <cmd> > <file> + <cmd> >> <file> + Background command: <cmd> & + Re-directed background command: <cmd> > <file> & + <cmd> >> <file> & + + Where: + + <cmd> is any one of the simple commands listed later. + <file> is the full or relative path to any writable object + in the filesystem name space (file or character driver). + Such objects will be referred to simply as files throughout + this README. + + NSH executes at the mid-priority (128). Backgrounded commands can + be made to execute at higher or lower priorities using nice: + + [nice [-d <niceness>>]] <cmd> [> <file>|>> <file>] [&] + + Where <niceness> is any value between -20 and 19 where lower + (more negative values) correspond to higher priorities. The + default niceness is 10. + +Conditional Command Execution +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + An if-then[-else]-fi construct is also supported in order to + support conditional execution of commands. This works from the + command line but is primarily intended for use within NSH scripts + (see the sh commnd). The syntax is as follows: + + if <cmd> + then + [sequence of <cmd>] + else + [sequence of <cmd>] + fi + +Built-In Variables +^^^^^^^^^^^^^^^^^^ + + $? - The result of the last simple command execution + +Current Working Directory +^^^^^^^^^^^^^^^^^^^^^^^^^ + + All path arguments to commands may be either an absolute path or a + path relative to the current working directory. The current working + directory is set using the 'cd' command and can be queried either + by using the 'pwd' command or by using the 'echo $PWD' command. + + Environment Variables: + ---------------------- + + PWD - The current working directory + OLDPWD - The previous working directory + +NSH Start-Up Script +^^^^^^^^^^^^^^^^^^^ + +NSH supports options to provide a start up script for NSH. In general +this capability is enabled with CONFIG_NSH_ROMFSETC, but has +several other related configuration options as described in the final +section of this README. This capability also depends on: + + - CONFIG_DISABLE_MOUNTPOINT not set + - CONFIG_NFILE_DESCRIPTORS > 4 + - CONFIG_FS_ROMFS + +Default Start-Up Behavior +------------------------- + +The implementation that is provided is intended to provide great flexibility +for the use of Start-Up files. This paragraph will discuss the general +behavior when all of the configuration options are set to the default +values. + +In this default case, enabling CONFIG_NSH_ROMFSETC will cause +NSH to behave as follows at NSH startup time: + +- NSH will create a read-only RAM disk (a ROM disk), containing a tiny + ROMFS filesystem containing the following: + + |--init.d/ + `-- rcS + + Where rcS is the NSH start-up script + +- NSH will then mount the ROMFS filesystem at /etc, resulting in: + + |--dev/ + | `-- ram0 + `--etc/ + `--init.d/ + `-- rcS + +- By default, the contents of rcS script are: + + # Create a RAMDISK and mount it at XXXRDMOUNTPOUNTXXX + + mkrd -m 1 -s 512 1024 + mkfatfs /dev/ram1 + mount -t vfat /dev/ram1 /tmp + +- NSH will execute the script at /etc/init.d/rcS at start-up (before the + first NSH prompt. After execution of the script, the root FS will look + like: + + |--dev/ + | |-- ram0 + | `-- ram1 + |--etc/ + | `--init.d/ + | `-- rcS + `--tmp/ + +Modifying the ROMFS Image +------------------------- + +The contents of the /etc directory are retained in the file +apps/nshlib/nsh_romfsimg.h (OR, if CONFIG_NSH_ARCHROMFS +is defined, include/arch/board/rcs.template). In order to modify +the start-up behavior, there are three things to study: + +1. Configuration Options. + The additional CONFIG_NSH_ROMFSETC configuration options + discussed in the final section of this README. + +2. tools/mkromfsimg.sh Script. + The script tools/mkromfsimg.sh creates nsh_romfsimg.h. + It is not automatically executed. If you want to change the + configuration settings associated with creating and mounting + the /tmp directory, then it will be necessary to re-generate + this header file using the mkromfsimg.sh script. + + The behavior of this script depends upon three things: + + - The configuration settings of the installed NuttX configuration. + - The genromfs tool (available from http://romfs.sourceforge.net). + - The file apps/nshlib/rcS.template (OR, if + CONFIG_NSH_ARCHROMFS is defined, include/arch/board/rcs.template) + +3. rcS.template. + The file apps/nshlib/rcS.template contains the general form + of the rcS file; configured values are plugged into this + template file to produce the final rcS file. + +NOTE: + + apps/nshlib/rcS.template generates the standard, default + nsh_romfsimg.h file. If CONFIG_NSH_ARCHROMFS is defined + in the NuttX configuration file, then a custom, board-specific + nsh_romfsimg.h file residing in configs/<board>/include will be + used. NOTE when the OS is configured, include/arch/board will + be linked to configs/<board>/include. + +All of the startup-behavior is contained in rcS.template. The +role of mkromfsimg.sh is to (1) apply the specific configuration +settings to rcS.template to create the final rcS, and (2) to +generate the header file nsh_romfsimg.h containg the ROMFS +file system image. + +Simple Commands +^^^^^^^^^^^^^^^ + +o [ <expression> ] +o test <expression> + + These are two alternative forms of the same command. They support + evaluation of a boolean expression which sets $?. This command + is used most frequently as the conditional command following the + 'if' in the if-then[-else]-fi construct. + + Expression Syntax: + ------------------ + + expression = simple-expression | !expression | + expression -o expression | expression -a expression + + simple-expression = unary-expression | binary-expression + + unary-expression = string-unary | file-unary + + string-unary = -n string | -z string + + file-unary = -b file | -c file | -d file | -e file | -f file | + -r file | -s file | -w file + + binary-expression = string-binary | numeric-binary + + string-binary = string = string | string == string | string != string + + numeric-binary = integer -eq integer | integer -ge integer | + integer -gt integer | integer -le integer | + integer -lt integer | integer -ne integer + +o cat <path> [<path> [<path> ...]] + + This command copies and concatentates all of the files at <path> + to the console (or to another file if the output is redirected). + +o cd [<dir-path>|-|~|..] + + Changes the current working directory (PWD). Also sets the + previous working directory environment variable (OLDPWD). + + FORMS: + ------ + + 'cd <dir-path>' sets the current working directory to <dir-path>. + 'cd -' sets the current working directory to the previous + working directory ($OLDPWD). Equivalent to 'cd $OLDPWD'. + 'cd' or 'cd ~' set the current working directory to the 'home' + directory. The 'home' directory can be configured by setting + CONFIG_LIB_HOMEDIR in the configuration file. The default + 'home' directory is '/'. + 'cd ..' sets the current working directory to the parent directory. + +o cp <source-path> <dest-path> + + Copy of the contents of the file at <source-path> to the location + in the filesystem indicated by <path-path> + +o date [-s "MMM DD HH:MM:SS YYYY"] + + Show or set the current date and time. This command is only supported + if the platform supported RTC hardware (CONFIG_RTC=y). + + Only one format is used both on display and when setting the date/time: + MMM DD HH:MM:SS YYYY. For example, + + data -s "Sep 1 11:30:00 2011" + + 24-hour time format is assumed. + +o dd if=<infile> of=<outfile> [bs=<sectsize>] [count=<sectors>] [skip=<sectors>] + + Copy blocks from <infile> to <outfile>. <nfile> or <outfile> may + be the path to a standard file, a character device, or a block device. + + Examples: + + 1. Read from character device, write to regular file. This will + create a new file of the specified size filled with zero. + + nsh> dd if=/dev/zero of=/tmp/zeros bs=64 count=16 + nsh> ls -l /tmp + /tmp: + -rw-rw-rw- 1024 ZEROS + + 2. Read from character device, write to block device. This will + fill the entire block device with zeros. + + nsh> ls -l /dev + /dev: + brw-rw-rw- 0 ram0 + crw-rw-rw- 0 zero + nsh> dd if=/dev/zero of=/dev/ram0 + + 3. Read from a block devic, write to a character device. This + will read the entire block device and dump the contents in + the bit bucket. + + nsh> ls -l /dev + /dev: + crw-rw-rw- 0 null + brw-rw-rw- 0 ram0 + nsh> dd if=/dev/ram0 of=/dev/null + +o df + + Show the state of each mounted volume + + Example: + + nsh> mount + /etc type romfs + /tmp type vfat + nsh> df + Block Number + Size Blocks Used Available Mounted on + 64 6 6 0 /etc + 512 985 2 983 /tmp + nsh> + +o echo [<string|$name> [<string|$name>...]] + + Copy the sequence of strings and expanded environment variables to + console out (or to a file if the output is re-directed). + +o exec <hex-address> + + Execute the user logic at address <hex-address>. NSH will pause + until the execution unless the user logic is executed in background + via 'exec <hex-address> &' + +o exit + + Exit NSH. Only useful if you have started some other tasks (perhaps + using the 'exec' command') and you would like to have NSH out of the + way. + +o free + + Show the current state of the memory allocator. For example, + + nsh> free + free + total used free largest + Mem: 4194288 1591552 2602736 2601584 + + Where: + total - This is the total size of memory allocated for use + by malloc in bytes. + used - This is the total size of memory occupied by + chunks handed out by malloc. + free - This is the total size of memory occupied by + free (not in use) chunks. + largest - Size of the largest free (not in use) chunk + +o get [-b|-n] [-f <local-path>] -h <ip-address> <remote-path> + + Use TFTP to copy the file at <remote-address> from the host whose IP + address is identified by <ip-address>. Other options: + + -f <local-path> + The file will be saved relative to the current working directory + unless <local-path> is provided. + -b|-n + Selects either binary ("octect") or test ("netascii") transfer + mode. Default: text. + +o help + + Presents summary information about each command to console. + +o ifconfig + + Show the current configuration of the network, for example: + + nsh> ifconfig + eth0 HWaddr 00:18:11:80:10:06 + IPaddr:10.0.0.2 DRaddr:10.0.0.1 Mask:255.255.255.0 + + if uIP statistics are enabled (CONFIG_NET_STATISTICS), then + this command will also show the detailed state of uIP. + +o kill -<signal> <pid> + + Send the <signal> to the task identified by <pid>. + +o losetup [-d <dev-path>] | [[-o <offset>] [-r] <ldev-path> <file-path>] + + Setup or teardown the loop device: + + 1. Teardown the setup for the loop device at <dev-path>: + + losetup d <dev-path> + + 2. Setup the loop device at <dev-path> to access the file at <file-path> + as a block device: + + losetup [-o <offset>] [-r] <dev-path> <file-path> + + Example: + + nsh> dd if=/dev/zero of=/tmp/image bs=512 count=512 + nsh> ls -l /tmp + /tmp: + -rw-rw-rw- 262144 IMAGE + nsh> losetup /dev/loop0 /tmp/image + nsh> ls -l /dev + /dev: + brw-rw-rw- 0 loop0 + nsh> mkfatfs /dev/loop0 + nsh> mount -t vfat /dev/loop0 /mnt/example + nsh> ls -l /mnt + ls -l /mnt + /mnt: + drw-rw-rw- 0 example/ + nsh> echo "This is a test" >/mnt/example/atest.txt + nsh> ls -l /mnt/example + /mnt/example: + -rw-rw-rw- 16 ATEST.TXT + nsh> cat /mnt/example/atest.txt + This is a test + nsh> + +o ls [-lRs] <dir-path> + + Show the contents of the directory at <dir-path>. NOTE: + <dir-path> must refer to a directory and no other filesystem + object. + + Options: + -------- + + -R Show the constents of specified directory and all of its + sub-directories. + -s Show the size of the files along with the filenames in the + listing + -l Show size and mode information along with the filenames + in the listing. + +o mb <hex-address>[=<hex-value>][ <hex-byte-count>] +o mh <hex-address>[=<hex-value>][ <hex-byte-count>] +o mw <hex-address>[=<hex-value>][ <hex-byte-count>] + + Access memory using byte size access (mb), 16-bit accesses (mh), + or 32-bit access (mw). In each case, + + <hex-address>. Specifies the address to be accessed. The current + value at that address will always be read and displayed. + <hex-address>=<hex-value>. Read the value, then write <hex-value> + to the location. + <hex-byte-count>. Perform the mb, mh, or mw operation on a total + of <hex-byte-count> bytes, increment the <hex-address> appropriately + after each access + + Example + + nsh> mh 0 16 + 0 = 0x0c1e + 2 = 0x0100 + 4 = 0x0c1e + 6 = 0x0110 + 8 = 0x0c1e + a = 0x0120 + c = 0x0c1e + e = 0x0130 + 10 = 0x0c1e + 12 = 0x0140 + 14 = 0x0c1e + nsh> + +o mkdir <path> + + Create the directory at <path>. All components of of <path> + except the final directory name must exist on a mounted file + system; the final directory must not. + + Recall that NuttX uses a pseudo filesystem for its root file system. + The mkdir command can only be used to create directories in volumes + set up with the mount command; it cannot be used to create directories + in the pseudo filesystem. + + Example: + ^^^^^^^^ + + nsh> mkdir /mnt/fs/tmp + nsh> ls -l /mnt/fs + /mnt/fs: + drw-rw-rw- 0 TESTDIR/ + drw-rw-rw- 0 TMP/ + nsh> + +o mkfatfs <path> + + Format a fat file system on the block device specified by path. + NSH provides this command to access the mkfatfs() NuttX API. + This block device must reside in the NuttX psuedo filesystem and + must have been created by some call to register_blockdriver() (see + include/nuttx/fs/fs.h). + +o mkfifo <path> + + Creates a FIFO character device anywhere in the pseudo file system, + creating whatever psuedo directories that may be needed to complete + the full path. By convention, however, device drivers are place in + the standard /dev directory. After it is created, the FIFO device + may be used as any other device driver. NSH provides this command + to access the mkfifo() NuttX API. + + Example: + ^^^^^^^^ + + nsh> ls -l /dev + /dev: + crw-rw-rw- 0 console + crw-rw-rw- 0 null + brw-rw-rw- 0 ram0 + nsh> mkfifo /dev/fifo + nsh> ls -l /dev + ls -l /dev + /dev: + crw-rw-rw- 0 console + crw-rw-rw- 0 fifo + crw-rw-rw- 0 null + brw-rw-rw- 0 ram0 + nsh> + +o mkrd [-m <minor>] [-s <sector-size>] <nsectors> + + Create a ramdisk consisting of <nsectors>, each of size + <sector-size> (or 512 bytes if <sector-size> is not specified. + The ramdisk will be registered as /dev/ram<n> (if <n> is not + specified, mkrd will attempt to register the ramdisk as + /dev/ram0. + + Example: + ^^^^^^^^ + + nsh> ls /dev + /dev: + console + null + ttyS0 + ttyS1 + nsh> mkrd 1024 + nsh> ls /dev + /dev: + console + null + ram0 + ttyS0 + ttyS1 + nsh> + + Once the ramdisk has been created, it may be formatted using + the mkfatfs command and mounted using the mount command. + + Example: + ^^^^^^^^ + nsh> mkrd 1024 + nsh> mkfatfs /dev/ram0 + nsh> mount -t vfat /dev/ram0 /tmp + nsh> ls /tmp + /tmp: + nsh> + +o mount [-t <fstype> <block-device> <dir-path>] + + The mount command performs one of two different operations. If no + paramters are provided on the command line after the mount command, + then the 'mount' command will enumerate all of the current + mountpoints on the console. + + If the mount parameters are provied on the command after the 'mount' + command, then the 'mount' command will mount a file system in the + NuttX psuedo-file system. 'mount' performs a three way association, + binding: + + File system. The '-t <fstype>' option identifies the type of + file system that has been formatted on the <block-device>. As + of this writing, vfat is the only supported value for <fstype> + + Block Device. The <block-device> argument is the full or relative + path to a block driver inode in the psuedo filesystem. By convention, + this is a name under the /dev sub-directory. This <block-device> + must have been previously formatted with the same file system + type as specified by <fstype> + + Mount Point. The mount point is the location in the psuedo file + system where the mounted volume will appear. This mount point + can only reside in the NuttX psuedo filesystem. By convention, this + mount point is a subdirectory under /mnt. The mount command will + create whatever psuedo directories that may be needed to complete + the full path but the full path must not already exist. + + After the volume has been mounted in the NuttX psuedo file + system, it may be access in the same way as other objects in the + file system. + + Examples: + ^^^^^^^^^ + + nsh> ls -l /dev + /dev: + crw-rw-rw- 0 console + crw-rw-rw- 0 null + brw-rw-rw- 0 ram0 + nsh> ls /mnt + nsh: ls: no such directory: /mnt + nsh> mount -t vfat /dev/ram0 /mnt/fs + nsh> ls -l /mnt/fs/testdir + /mnt/fs/testdir: + -rw-rw-rw- 15 TESTFILE.TXT + nsh> echo "This is a test" >/mnt/fs/testdir/example.txt + nsh> ls -l /mnt/fs/testdir + /mnt/fs/testdir: + -rw-rw-rw- 15 TESTFILE.TXT + -rw-rw-rw- 16 EXAMPLE.TXT + nsh> cat /mnt/fs/testdir/example.txt + This is a test + nsh> + + nsh> mount + /etc type romfs + /tmp type vfat + /mnt/fs type vfat + +o mv <old-path> <new-path> + + Rename the file object at <old-path> to <new-path>. Both paths must + reside in the same mounted filesystem. + +o nfsmount <server-address> <mount-point> <remote-path> + + Mount the remote NFS server directory <remote-path> at <mount-point> on the target machine. + <server-address> is the IP address of the remote server. + +o ps + + Show the currently active threads and tasks. For example, + + nsh> ps + PID PRI SCHD TYPE NP STATE NAME + 0 0 FIFO TASK READY Idle Task() + 1 128 RR TASK RUNNING init() + 2 128 FIFO TASK WAITSEM nsh_telnetmain() + 3 100 RR PTHREAD WAITSEM <pthread>(21) + nsh> + +o ping [-c <count>] [-i <interval>] <ip-address> + + Test the network communication with a remote peer. Example, + + nsh> 10.0.0.1 + PING 10.0.0.1 56 bytes of data + 56 bytes from 10.0.0.1: icmp_seq=1 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=2 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=3 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=4 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=5 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=6 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=7 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=8 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=9 time=0 ms + 56 bytes from 10.0.0.1: icmp_seq=10 time=0 ms + 10 packets transmitted, 10 received, 0% packet loss, time 10190 ms + nsh> + +o put [-b|-n] [-f <remote-path>] -h <ip-address> <local-path> + + Copy the file at <local-address> to the host whose IP address is + identified by <ip-address>. Other options: + + -f <remote-path> + The file will be saved with the same name on the host unless + unless <local-path> is provided. + -b|-n + Selects either binary ("octect") or test ("netascii") transfer + mode. Default: text. + +o pwd + + Show the current working directory. + + nsh> cd /dev + nsh> pwd + /dev + nsh> + + Same as 'echo $PWD' + + nsh> echo $PWD + /dev + nsh> + +o rm <file-path> + + Remove the specified <file-path> name from the mounted file system. + Recall that NuttX uses a pseudo filesystem for its root file system. + The rm command can only be used to remove (unlink) files in volumes + set up with the mount command; it cannot be used to remove names from + the pseudo filesystem. + + Example: + ^^^^^^^^ + + nsh> ls /mnt/fs/testdir + /mnt/fs/testdir: + TESTFILE.TXT + EXAMPLE.TXT + nsh> rm /mnt/fs/testdir/example.txt + nsh> ls /mnt/fs/testdir + /mnt/fs/testdir: + TESTFILE.TXT + nsh> + +o rmdir <dir-path> + + Remove the specified <dir-path> directory from the mounted file system. + Recall that NuttX uses a pseudo filesystem for its root file system. The + rmdir command can only be used to remove directories from volumes set up + with the mount command; it cannot be used to remove directories from the + pseudo filesystem. + + Example: + ^^^^^^^^ + + nsh> mkdir /mnt/fs/tmp + nsh> ls -l /mnt/fs + /mnt/fs: + drw-rw-rw- 0 TESTDIR/ + drw-rw-rw- 0 TMP/ + nsh> rmdir /mnt/fs/tmp + nsh> ls -l /mnt/fs + ls -l /mnt/fs + /mnt/fs: + drw-rw-rw- 0 TESTDIR/ + nsh> + +o set <name> <value> + + Set the environment variable <name> to the sting <value>. + For example, + + nsh> echo $foobar + + nsh> set foobar foovalue + nsh> echo $foobar + foovalue + nsh> + +o sh <script-path> + + Execute the sequence of NSH commands in the file referred + to by <script-path>. + +o sleep <sec> + + Pause execution (sleep) of <sec> seconds. + +o unset <name> + + Remove the value associated with the environment variable + <name>. Example: + + nsh> echo $foobar + foovalue + nsh> unset foobar + nsh> echo $foobar + + nsh> + +o usleep <usec> + + Pause execution (sleep) of <usec> microseconds. + +o wget [-o <local-path>] <url> + + Use HTTP to copy the file at <url> to the current directory. + Options: + + -o <local-path> + The file will be saved relative to the current working directory + and with the same name as on the HTTP server unless <local-path> + is provided. + +o xd <hex-address> <byte-count> + + Dump <byte-count> bytes of data from address <hex-address> + + Example: + ^^^^^^^^ + + nsh> xd 410e0 512 + Hex dump: + 0000: 00 00 00 00 9c 9d 03 00 00 00 00 01 11 01 10 06 ................ + 0010: 12 01 11 01 25 08 13 0b 03 08 1b 08 00 00 02 24 ....%..........$ + ... + 01f0: 08 3a 0b 3b 0b 49 13 00 00 04 13 01 01 13 03 08 .:.;.I.......... + nsh> + +NSH Configuration Settings +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The availability of the above commands depends upon features that +may or may not be enabled in the NuttX configuration file. The +following table indicates the dependency of each command on NuttX +configuration settings. General configuration settings are discussed +in the NuttX Porting Guide. Configuration settings specific to NSH +as discussed at the bottom of this README file. + +Command Dependencies on Configuration Settings +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + Command Depends on Configuration + ---------- -------------------------- + [ !CONFIG_NSH_DISABLESCRIPT + cat CONFIG_NFILE_DESCRIPTORS > 0 + cd !CONFIG_DISABLE_ENVIRON && CONFIG_NFILE_DESCRIPTORS > 0 + cp CONFIG_NFILE_DESCRIPTORS > 0 + dd CONFIG_NFILE_DESCRIPTORS > 0 + df !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_READABLE (see note 3) + echo -- + exec -- + exit -- + free -- + get CONFIG_NET && CONFIG_NET_UDP && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NET_BUFSIZE >= 558 (see note 1) + help -- + ifconfig CONFIG_NET + kill !CONFIG_DISABLE_SIGNALS + losetup !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 + ls CONFIG_NFILE_DESCRIPTORS > 0 + mb,mh,mw --- + mkdir !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_WRITABLE (see note 4) + mkfatfs !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_FAT + mkfifo CONFIG_NFILE_DESCRIPTORS > 0 + mkrd !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_WRITABLE (see note 4) + mount !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_READABLE (see note 3) + mv !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_WRITABLE (see note 4) + nfsmount !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NET && CONFIG_NFS + ping CONFIG_NET && CONFIG_NET_ICMP && CONFIG_NET_ICMP_PING && !CONFIG_DISABLE_CLOCK && !CONFIG_DISABLE_SIGNALS + ps -- + put CONFIG_NET && CONFIG_NET_UDP && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NET_BUFSIZE >= 558 (see note 1,2) + pwd !CONFIG_DISABLE_ENVIRON && CONFIG_NFILE_DESCRIPTORS > 0 + rm !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_WRITABLE (see note 4) + rmdir !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_WRITABLE (see note 4) + set !CONFIG_DISABLE_ENVIRON + sh CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NFILE_STREAMS > 0 && !CONFIG_NSH_DISABLESCRIPT + sleep !CONFIG_DISABLE_SIGNALS + test !CONFIG_NSH_DISABLESCRIPT + umount !CONFIG_DISABLE_MOUNTPOINT && CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_FS_READABLE + unset !CONFIG_DISABLE_ENVIRON + usleep !CONFIG_DISABLE_SIGNALS + get CONFIG_NET && CONFIG_NET_TCP && CONFIG_NFILE_DESCRIPTORS > 0 + xd --- + +* NOTES: + 1. Because of hardware padding, the actual buffersize required for put and get + operations size may be larger. + 2. Special TFTP server start-up optionss will probably be required to permit + creation of file for the correct operation of the put command. + 3. CONFIG_FS_READABLE is not a user configuration but is set automatically + if any readable filesystem is selected. At present, this is either CONFIG_FS_FAT + and CONFIG_FS_ROMFS. + 4. CONFIG_FS_WRITABLE is not a user configuration but is set automatically + if any writable filesystem is selected. At present, this is only CONFIG_FS_FAT. + +In addition, each NSH command can be individually disabled via one of the following +settings. All of these settings make the configuration of NSH potentially complex but +also allow it to squeeze into very small memory footprints. + + CONFIG_NSH_DISABLE_CAT, CONFIG_NSH_DISABLE_CD, CONFIG_NSH_DISABLE_CP, + CONFIG_NSH_DISABLE_DD, CONFIG_NSH_DISABLE_DF, CONFIG_NSH_DISABLE_ECHO, + CONFIG_NSH_DISABLE_EXEC, CONFIG_NSH_DISABLE_EXIT, CONFIG_NSH_DISABLE_FREE, + CONFIG_NSH_DISABLE_GET, CONFIG_NSH_DISABLE_HELP, CONFIG_NSH_DISABLE_IFCONFIG, + CONFIG_NSH_DISABLE_KILL, CONFIG_NSH_DISABLE_LOSETUP, CONFIG_NSH_DISABLE_LS, + CONFIG_NSH_DISABLE_MB, CONFIG_NSH_DISABLE_MKDIR, CONFIG_NSH_DISABLE_MKFATFS, + CONFIG_NSH_DISABLE_MKFIFO, CONFIG_NSH_DISABLE_MKRD, CONFIG_NSH_DISABLE_MH, + CONFIG_NSH_DISABLE_MOUNT, CONFIG_NSH_DISABLE_MW, CONFIG_NSH_DISABLE_MV, + CONFIG_NSH_DISABLE_NFSMOUNT, CONFIG_NSH_DISABLE_PS, CONFIG_NSH_DISABLE_PING, + CONFIG_NSH_DISABLE_PUT, CONFIG_NSH_DISABLE_PWD, CONFIG_NSH_DISABLE_RM, + CONFIG_NSH_DISABLE_RMDIR, CONFIG_NSH_DISABLE_SET, CONFIG_NSH_DISABLE_SH, + CONFIG_NSH_DISABLE_SLEEP, CONFIG_NSH_DISABLE_TEST, CONFIG_NSH_DISABLE_UMOUNT, + CONFIG_NSH_DISABLE_UNSET, CONFIG_NSH_DISABLE_USLEEP, CONFIG_NSH_DISABLE_WGET, + CONFIG_NSH_DISABLE_XD + +NSH-Specific Configuration Settings +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + The behavior of NSH can be modified with the following settings in + the configs/<board-name>/defconfig file: + + * CONFIG_NSH_BUILTIN_APPS + Support external registered, "named" applications that can be + executed from the NSH command line (see apps/README.txt for + more information). + + * CONFIG_NSH_FILEIOSIZE + Size of a static I/O buffer used for file access (ignored if + there is no filesystem). Default is 1024. + + * CONFIG_NSH_STRERROR + strerror(errno) makes more readable output but strerror() is + very large and will not be used unless this setting is 'y' + + * CONFIG_NSH_LINELEN + The maximum length of one command line and of one output line. + Default: 80 + + * CONFIG_NSH_NESTDEPTH + The maximum number of nested if-then[-else]-fi sequences that + are permissable. Default: 3 + + * CONFIG_NSH_DISABLESCRIPT + This can be set to 'y' to suppress support for scripting. This + setting disables the 'sh', 'test', and '[' commands and the + if-then[-else]-fi construct. This would only be set on systems + where a minimal footprint is a necessity and scripting is not. + + * CONFIG_NSH_DISABLEBG + This can be set to 'y' to suppress support for background + commands. This setting disables the 'nice' command prefix and + the '&' command suffix. This would only be set on systems + where a minimal footprint is a necessity and background command + execution is not. + + * CONFIG_NSH_MMCSDMINOR + If the architecture supports an MMC/SD slot and if the NSH + architecture specific logic is present, this option will provide + the MMC/SD minor number, i.e., the MMC/SD block driver will + be registered as /dev/mmcsdN where N is the minor number. + Default is zero. + + * CONFIG_NSH_ROMFSETC + Mount a ROMFS filesystem at /etc and provide a startup script + at /etc/init.d/rcS. The default startup script will mount + a FAT FS RAMDISK at /tmp but the logic is easily extensible. + + * CONFIG_NSH_CONSOLE + If CONFIG_NSH_CONSOLE is set to 'y', then a serial + console front-end is selected. + + Normally, the serial console device is a UART and RS-232 + interface. However, if CONFIG_USBDEV is defined, then a USB + serial device may, instead, be used if the one of + the following are defined: + + CONFIG_PL2303 and CONFIG_PL2303_CONSOLE - Sets up the + Prolifics PL2303 emulation as a console device + at /dev/console. + + CONFIG_CDCACM and CONFIG_CDCACM_CONSOLE - Sets up the + CDC/ACM serial device as a console device at + dev/console. + + CONFIG_NSH_USBCONSOLE + If defined, then the an arbitrary USB device may be used + to as the NSH console. In this case, CONFIG_NSH_USBCONDEV + must be defined to indicate which USB device to use as + the console. + + CONFIG_NSH_USBCONDEV + If CONFIG_NSH_USBCONSOLE is set to 'y', then CONFIG_NSH_USBCONDEV + must also be set to select the USB device used to support + the NSH console. This should be set to the quoted name of a + readable/write-able USB driver such as: + CONFIG_NSH_USBCONDEV="/dev/ttyACM0". + + If there are more than one USB devices, then a USB device + minor number may also need to be provided: + + CONFIG_NSH_UBSDEV_MINOR + The minor device number of the USB device. Default: 0 + + If USB tracing is enabled (CONFIG_USBDEV_TRACE), then NSH will + initialize USB tracing as requested by the following. Default: + Only USB errors are traced. + + CONFIG_NSH_USBDEV_TRACEINIT + Show initialization events + CONFIG_NSH_USBDEV_TRACECLASS + Show class driver events + CONFIG_NSH_USBDEV_TRACETRANSFERS + Show data transfer events + CONFIG_NSH_USBDEV_TRACECONTROLLER + Show controller events + CONFIG_NSH_USBDEV_TRACEINTERRUPTS + Show interrupt-related events. + + * CONFIG_NSH_CONDEV + If CONFIG_NSH_CONSOLE is set to 'y', then CONFIG_NSH_CONDEV + may also be set to select the serial device used to support + the NSH console. This should be set to the quoted name of a + readable/write-able character driver such as: + CONFIG_NSH_CONDEV="/dev/ttyS1". This is useful, for example, + to separate the NSH command line from the system console when + the system console is used to provide debug output. Default: + stdin and stdout (probably "/dev/console") + + NOTE: When any other device other than /dev/console is used + for a user interface, (1) linefeeds (\n) will not be expanded to + carriage return / linefeeds (\r\n). You will need to set + your terminal program to account for this. And (2) input is + not automatically echoed so you will have to turn local echo on. + + * CONFIG_NSH_TELNET + If CONFIG_NSH_TELNET is set to 'y', then a TELENET + server front-end is selected. When this option is provided, + you may log into NuttX remotely using telnet in order to + access NSH. + + * CONFIG_NSH_ARCHINIT + Set if your board provides architecture specific initialization + via the board-specific function nsh_archinitialize(). This + function will be called early in NSH initialization to allow + board logic to do such things as configure MMC/SD slots. + + If Telnet is selected for the NSH console, then we must configure + the resources used by the Telnet daemon and by the Telnet clients. + + * CONFIG_NSH_TELNETD_PORT - The telnet daemon will listen on this + TCP port number for connections. Default: 23 + + * CONFIG_NSH_TELNETD_DAEMONPRIO - Priority of the Telnet daemon. + Default: SCHED_PRIORITY_DEFAULT + + * CONFIG_NSH_TELNETD_DAEMONSTACKSIZE - Stack size allocated for the + Telnet daemon. Default: 2048 + + * CONFIG_NSH_TELNETD_CLIENTPRIO- Priority of the Telnet client. + Default: SCHED_PRIORITY_DEFAULT + + * CONFIG_NSH_TELNETD_CLIENTSTACKSIZE - Stack size allocated for the + Telnet client. Default: 2048 + + One or both of CONFIG_NSH_CONSOLE and CONFIG_NSH_TELNET + must be defined. If CONFIG_NSH_TELNET is selected, then there some + other configuration settings that apply: + + * CONFIG_NET=y + Of course, networking must be enabled + + * CONFIG_NSOCKET_DESCRIPTORS + And, of course, you must allocate some socket descriptors. + + * CONFIG_NET_TCP=y + TCP/IP support is required for telnet (as well as various other TCP-related + configuration settings). + + * CONFIG_NSH_IOBUFFER_SIZE + Determines the size of the I/O buffer to use for sending/ + receiving TELNET commands/reponses + + * CONFIG_NSH_DHCPC + Obtain the IP address via DHCP. + + * CONFIG_NSH_IPADDR + If CONFIG_NSH_DHCPC is NOT set, then the static IP + address must be provided. + + * CONFIG_NSH_DRIPADDR + Default router IP address + + * CONFIG_NSH_NETMASK + Network mask + + * CONFIG_NSH_NOMAC + Set if your ethernet hardware has no built-in MAC address. + If set, a bogus MAC will be assigned. + + If you use DHCPC, then some special configuration network options are + required. These include: + + * CONFIG_NET=y + Of course, networking must be enabled + + * CONFIG_NSOCKET_DESCRIPTORS + And, of course, you must allocate some socket descriptors. + + * CONFIG_NET_UDP=y + UDP support is required for DHCP (as well as various other UDP-related + configuration settings) + + * CONFIG_NET_BROADCAST=y + UDP broadcast support is needed. + + * CONFIG_NET_BUFSIZE=650 (or larger) + Per RFC2131 (p. 9), the DHCP client must be prepared to receive DHCP + messages of up to 576 bytes (excluding Ethernet, IP, or UDP headers and FCS). + + If CONFIG_NSH_ROMFSETC is selected, then the following additional + configuration setting apply: + + * CONFIG_NSH_ROMFSMOUNTPT + The default mountpoint for the ROMFS volume is /etc, but that + can be changed with this setting. This must be a absolute path + beginning with '/'. + + * CONFIG_NSH_INITSCRIPT + This is the relative path to the startup script within the mountpoint. + The default is init.d/rcS. This is a relative path and must not + start with '/'. + + * CONFIG_NSH_ROMFSDEVNO + This is the minor number of the ROMFS block device. The default is + '0' corresponding to /dev/ram0. + + * CONFIG_NSH_ROMFSSECTSIZE + This is the sector size to use with the ROMFS volume. Since the + default volume is very small, this defaults to 64 but should be + increased if the ROMFS volume were to be become large. Any value + selected must be a power of 2. + + When the default rcS file used when CONFIG_NSH_ROMFSETC is + selected, it will mount a FAT FS under /tmp. The following selections + describe that FAT FS. + + * CONFIG_NSH_FATDEVNO + This is the minor number of the FAT FS block device. The default is + '1' corresponding to /dev/ram1. + + * CONFIG_NSH_FATSECTSIZE + This is the sector size use with the FAT FS. Default is 512. + + * CONFIG_NSH_FATNSECTORS + This is the number of sectors to use with the FAT FS. Defalt is + 1024. The amount of memory used by the FAT FS will be + CONFIG_NSH_FATSECTSIZE * CONFIG_NSH_FATNSECTORS + bytes. + + * CONFIG_NSH_FATMOUNTPT + This is the location where the FAT FS will be mounted. Default + is /tmp. + +Common Problems +^^^^^^^^^^^^^^^ + + Problem: + Using NSH over serial, the "nsh>" prompt repeats over and over again + with no serial input. + Usual Cause: + NSH over serial needs to use the interrupt driven serial driver + (drivers/serial/serial.c) not the polled serial driver (drivers/serial/lowconsole.c). + Make sure that the polled console is disabled in the OS configuration + file, .config. That file should have CONFIG_DEV_LOWCONSOLE=n for + NSH over serial. + + Problem: + The function 'readline' is undefined. + Usual Cause: + The following is missing from your appconfig file: + + CONFIGURED_APPS += system/readline + diff --git a/apps/nshlib/nsh.h b/apps/nshlib/nsh.h new file mode 100644 index 000000000..439c2ffd1 --- /dev/null +++ b/apps/nshlib/nsh.h @@ -0,0 +1,606 @@ +/**************************************************************************** + * apps/nshlib/nsh.h + * + * Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_NSHLIB_NSH_H +#define __APPS_NSHLIB_NSH_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdint.h> +#include <stdbool.h> +#include <errno.h> + +#include <nuttx/usb/usbdev_trace.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* The background commands require pthread support */ + +#ifdef CONFIG_DISABLE_PTHREAD +# ifndef CONFIG_NSH_DISABLEBG +# define CONFIG_NSH_DISABLEBG 1 +# endif +#endif + +/* Telnetd requires networking support */ + +#ifndef CONFIG_NET +# undef CONFIG_NSH_TELNET +#endif + +/* One front end must be defined */ + +#if !defined(CONFIG_NSH_CONSOLE) && !defined(CONFIG_NSH_TELNET) +# error "No NSH front end defined" +#endif + +/* If a USB device is selected for the NSH console then we need to handle some + * special start-up conditions. + */ + +#undef HAVE_USB_CONSOLE +#if defined(CONFIG_USBDEV) + +/* Check for a PL2303 serial console. Use console device "/dev/console". */ + +# if defined(CONFIG_PL2303) && defined(CONFIG_PL2303_CONSOLE) +# define HAVE_USB_CONSOLE 1 + +/* Check for a CDC/ACM serial console. Use console device "/dev/console". */ + +# elif defined(CONFIG_CDCACM) && defined(CONFIG_CDCACM_CONSOLE) +# define HAVE_USB_CONSOLE 1 + +/* Check for other USB console. USB console device must be provided in CONFIG_NSH_CONDEV */ + +# elif defined(CONFIG_NSH_USBCONSOLE) +# define HAVE_USB_CONSOLE 1 +# endif +#endif + +/* Defaults for the USB console */ + +#ifdef HAVE_USB_CONSOLE + +/* The default USB console device minor number is 0*/ + +# ifndef CONFIG_NSH_UBSDEV_MINOR +# define CONFIG_NSH_UBSDEV_MINOR 0 +# endif + +/* The default console device is always /dev/console */ + +# ifndef CONFIG_NSH_USBCONDEV +# define CONFIG_NSH_USBCONDEV "/dev/console" +# endif + +/* USB trace settings */ + +#ifdef CONFIG_NSH_USBDEV_TRACEINIT +# define TRACE_INIT_BITS (TRACE_INIT_BIT) +#else +# define TRACE_INIT_BITS (0) +#endif + +#define TRACE_ERROR_BITS (TRACE_DEVERROR_BIT|TRACE_CLSERROR_BIT) + +#ifdef CONFIG_NSH_USBDEV_TRACECLASS +# define TRACE_CLASS_BITS (TRACE_CLASS_BIT|TRACE_CLASSAPI_BIT|TRACE_CLASSSTATE_BIT) +#else +# define TRACE_CLASS_BITS (0) +#endif + +#ifdef CONFIG_NSH_USBDEV_TRACETRANSFERS +# define TRACE_TRANSFER_BITS (TRACE_OUTREQQUEUED_BIT|TRACE_INREQQUEUED_BIT|TRACE_READ_BIT|\ + TRACE_WRITE_BIT|TRACE_COMPLETE_BIT) +#else +# define TRACE_TRANSFER_BITS (0) +#endif + +#ifdef CONFIG_NSH_USBDEV_TRACECONTROLLER +# define TRACE_CONTROLLER_BITS (TRACE_EP_BIT|TRACE_DEV_BIT) +#else +# define TRACE_CONTROLLER_BITS (0) +#endif + +#ifdef CONFIG_NSH_USBDEV_TRACEINTERRUPTS +# define TRACE_INTERRUPT_BITS (TRACE_INTENTRY_BIT|TRACE_INTDECODE_BIT|TRACE_INTEXIT_BIT) +#else +# define TRACE_INTERRUPT_BITS (0) +#endif + +#define TRACE_BITSET (TRACE_INIT_BITS|TRACE_ERROR_BITS|TRACE_CLASS_BITS|\ + TRACE_TRANSFER_BITS|TRACE_CONTROLLER_BITS|TRACE_INTERRUPT_BITS) + +#endif + +/* If Telnet is selected for the NSH console, then we must configure + * the resources used by the Telnet daemon and by the Telnet clients. + * + * CONFIG_NSH_TELNETD_PORT - The telnet daemon will listen on this. + * port. Default: 23 + * CONFIG_NSH_TELNETD_DAEMONPRIO - Priority of the Telnet daemon. + * Default: SCHED_PRIORITY_DEFAULT + * CONFIG_NSH_TELNETD_DAEMONSTACKSIZE - Stack size allocated for the + * Telnet daemon. Default: 2048 + * CONFIG_NSH_TELNETD_CLIENTPRIO- Priority of the Telnet client. + * Default: SCHED_PRIORITY_DEFAULT + * CONFIG_NSH_TELNETD_CLIENTSTACKSIZE - Stack size allocated for the + * Telnet client. Default: 2048 + */ + +#ifndef CONFIG_NSH_TELNETD_PORT +# define CONFIG_NSH_TELNETD_PORT 23 +#endif + +#ifndef CONFIG_NSH_TELNETD_DAEMONPRIO +# define CONFIG_NSH_TELNETD_DAEMONPRIO SCHED_PRIORITY_DEFAULT +#endif + +#ifndef CONFIG_NSH_TELNETD_DAEMONSTACKSIZE +# define CONFIG_NSH_TELNETD_DAEMONSTACKSIZE 2048 +#endif + +#ifndef CONFIG_NSH_TELNETD_CLIENTPRIO +# define CONFIG_NSH_TELNETD_CLIENTPRIO SCHED_PRIORITY_DEFAULT +#endif + +#ifndef CONFIG_NSH_TELNETD_CLIENTSTACKSIZE +# define CONFIG_NSH_TELNETD_CLIENTSTACKSIZE 2048 +#endif + +/* Verify support for ROMFS /etc directory support options */ + +#ifdef CONFIG_NSH_ROMFSETC +# ifdef CONFIG_DISABLE_MOUNTPOINT +# error "Mountpoint support is disabled" +# undef CONFIG_NSH_ROMFSETC +# endif +# if CONFIG_NFILE_DESCRIPTORS < 4 +# error "Not enough file descriptors" +# undef CONFIG_NSH_ROMFSETC +# endif +# ifndef CONFIG_FS_ROMFS +# error "ROMFS support not enabled" +# undef CONFIG_NSH_ROMFSETC +# endif +# ifndef CONFIG_NSH_ROMFSMOUNTPT +# define CONFIG_NSH_ROMFSMOUNTPT "/etc" +# endif +# ifdef CONFIG_NSH_INIT +# ifndef CONFIG_NSH_INITSCRIPT +# define CONFIG_NSH_INITSCRIPT "init.d/rcS" +# endif +# endif +# undef NSH_INITPATH +# define NSH_INITPATH CONFIG_NSH_ROMFSMOUNTPT "/" CONFIG_NSH_INITSCRIPT +# ifndef CONFIG_NSH_ROMFSDEVNO +# define CONFIG_NSH_ROMFSDEVNO 0 +# endif +# ifndef CONFIG_NSH_ROMFSSECTSIZE +# define CONFIG_NSH_ROMFSSECTSIZE 64 +# endif +# define NSECTORS(b) (((b)+CONFIG_NSH_ROMFSSECTSIZE-1)/CONFIG_NSH_ROMFSSECTSIZE) +# define STR_RAMDEVNO(m) #m +# define MKMOUNT_DEVNAME(m) "/dev/ram" STR_RAMDEVNO(m) +# define MOUNT_DEVNAME MKMOUNT_DEVNAME(CONFIG_NSH_ROMFSDEVNO) +#else +# undef CONFIG_NSH_ROMFSMOUNTPT +# undef CONFIG_NSH_INIT +# undef CONFIG_NSH_INITSCRIPT +# undef CONFIG_NSH_ROMFSDEVNO +# undef CONFIG_NSH_ROMFSSECTSIZE +#endif + +/* This is the maximum number of arguments that will be accepted for a command */ + +#define NSH_MAX_ARGUMENTS 6 + +/* strerror() produces much nicer output but is, however, quite large and + * will only be used if CONFIG_NSH_STRERROR is defined. + */ + +#ifdef CONFIG_NSH_STRERROR +# define NSH_ERRNO strerror(errno) +# define NSH_ERRNO_OF(err) strerror(err) +#else +# define NSH_ERRNO (errno) +# define NSH_ERRNO_OF(err) (err) +#endif + +/* Maximum size of one command line (telnet or serial) */ + +#ifndef CONFIG_NSH_LINELEN +# define CONFIG_NSH_LINELEN 80 +#endif + +/* The following two settings are used only in the telnetd interface */ + +#ifndef CONFIG_NSH_IOBUFFER_SIZE +# define CONFIG_NSH_IOBUFFER_SIZE 512 +#endif + +/* The maximum number of nested if-then[-else]-fi sequences that + * are permissable. + */ + +#ifndef CONFIG_NSH_NESTDEPTH +# define CONFIG_NSH_NESTDEPTH 3 +#endif + +/* Define to enable dumping of all input/output buffers */ + +#undef CONFIG_NSH_TELNETD_DUMPBUFFER +#undef CONFIG_NSH_FULLPATH + +/* Make sure that the home directory is defined */ + +#ifndef CONFIG_LIB_HOMEDIR +# define CONFIG_LIB_HOMEDIR "/" +#endif + +/* Stubs used when working directory is not supported */ + +#if CONFIG_NFILE_DESCRIPTORS <= 0 || defined(CONFIG_DISABLE_ENVIRON) +# define nsh_getfullpath(v,p) ((char*)(p)) +# define nsh_freefullpath(p) +#endif + +/* The size of the I/O buffer may be specified in the + * configs/<board-name>defconfig file -- provided that it is at least as + * large as PATH_MAX. + */ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +# ifdef CONFIG_NSH_FILEIOSIZE +# if CONFIG_NSH_FILEIOSIZE > (PATH_MAX + 1) +# define IOBUFFERSIZE CONFIG_NSH_FILEIOSIZE +# else +# define IOBUFFERSIZE (PATH_MAX + 1) +# endif +# else +# define IOBUFFERSIZE 1024 +# endif +# else +# define IOBUFFERSIZE (PATH_MAX + 1) +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +enum nsh_parser_e +{ + NSH_PARSER_NORMAL = 0, + NSH_PARSER_IF, + NSH_PARSER_THEN, + NSH_PARSER_ELSE +}; + +struct nsh_state_s +{ + uint8_t ns_ifcond : 1; /* Value of command in 'if' statement */ + uint8_t ns_disabled : 1; /* TRUE: Unconditionally disabled */ + uint8_t ns_unused : 4; + uint8_t ns_state : 2; /* Parser state (see enum nsh_parser_e) */ +}; + +struct nsh_parser_s +{ +#ifndef CONFIG_NSH_DISABLEBG + bool np_bg; /* true: The last command executed in background */ +#endif + bool np_redirect; /* true: Output from the last command was re-directed */ + bool np_fail; /* true: The last command failed */ +#ifndef CONFIG_NSH_DISABLESCRIPT + uint8_t np_ndx; /* Current index into np_st[] */ +#endif +#ifndef CONFIG_NSH_DISABLEBG + int np_nice; /* "nice" value applied to last background cmd */ +#endif + + /* This is a stack of parser state information. It supports nested + * execution of commands that span multiple lines (like if-then-else-fi) + */ + +#ifndef CONFIG_NSH_DISABLESCRIPT + struct nsh_state_s np_st[CONFIG_NSH_NESTDEPTH]; +#endif +}; + +struct nsh_vtbl_s; /* Defined in nsh_console.h */ +typedef int (*cmd_t)(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +extern const char g_nshgreeting[]; +extern const char g_nshprompt[]; +extern const char g_nshsyntax[]; +extern const char g_fmtargrequired[]; +extern const char g_fmtarginvalid[]; +extern const char g_fmtargrange[]; +extern const char g_fmtcmdnotfound[]; +extern const char g_fmtnosuch[]; +extern const char g_fmttoomanyargs[]; +extern const char g_fmtdeepnesting[]; +extern const char g_fmtcontext[]; +extern const char g_fmtcmdfailed[]; +extern const char g_fmtcmdoutofmemory[]; +extern const char g_fmtinternalerror[]; +#ifndef CONFIG_DISABLE_SIGNALS +extern const char g_fmtsignalrecvd[]; +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/* Initialization */ + +#ifdef CONFIG_NSH_ROMFSETC +int nsh_romfsetc(void); +#else +# define nsh_romfsetc() (-ENOSYS) +#endif + +#ifdef CONFIG_NET +int nsh_netinit(void); +#else +# define nsh_netinit() (-ENOSYS) +#endif + +#ifdef HAVE_USB_CONSOLE +int nsh_usbconsole(void); +#else +# define nsh_usbconsole() (-ENOSYS) +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NFILE_STREAMS > 0 && !defined(CONFIG_NSH_DISABLESCRIPT) +int nsh_script(FAR struct nsh_vtbl_s *vtbl, const char *cmd, const char *path); +#endif + +/* Architecture-specific initialization */ + +#ifdef CONFIG_NSH_ARCHINIT +int nsh_archinitialize(void); +#else +# define nsh_archinitialize() (-ENOSYS) +#endif + +/* Message handler */ + +int nsh_parse(FAR struct nsh_vtbl_s *vtbl, char *cmdline); + +/* Application interface */ + +#ifdef CONFIG_NSH_BUILTIN_APPS +int nsh_execapp(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd, FAR char **argv); +#endif + +/* Working directory support */ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +FAR const char *nsh_getcwd(void); +char *nsh_getfullpath(FAR struct nsh_vtbl_s *vtbl, const char *relpath); +void nsh_freefullpath(char *relpath); +#endif + +/* Debug */ + +void nsh_dumpbuffer(FAR struct nsh_vtbl_s *vtbl, const char *msg, + const uint8_t *buffer, ssize_t nbytes); + +/* USB debug support */ + +#if defined(CONFIG_USBDEV_TRACE) && defined(HAVE_USB_CONSOLE) +void nsh_usbtrace(void); +#else +# define nsh_usbtrace() +#endif + +/* Shell command handlers */ + +#ifndef CONFIG_NSH_DISABLE_ECHO + int cmd_echo(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif +#ifndef CONFIG_NSH_DISABLE_EXEC + int cmd_exec(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif +#ifndef CONFIG_NSH_DISABLE_MB + int cmd_mb(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif +#ifndef CONFIG_NSH_DISABLE_MH + int cmd_mh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif +#ifndef CONFIG_NSH_DISABLE_MW + int cmd_mw(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif +#ifndef CONFIG_NSH_DISABLE_FREE + int cmd_free(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif +#ifndef CONFIG_NSH_DISABLE_PS + int cmd_ps(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif +#ifndef CONFIG_NSH_DISABLE_XD + int cmd_xd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif + +#if !defined(CONFIG_NSH_DISABLESCRIPT) && !defined(CONFIG_NSH_DISABLE_TEST) + int cmd_test(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); + int cmd_lbracket(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif + +#ifndef CONFIG_DISABLE_CLOCK +# if defined (CONFIG_RTC) && !defined(CONFIG_NSH_DISABLE_DATE) + int cmd_date(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_CAT + int cmd_cat(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_CP + int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_DD + int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_LS + int cmd_ls(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# if defined(CONFIG_SYSLOG) && defined(CONFIG_RAMLOG_SYSLOG) && !defined(CONFIG_NSH_DISABLE_DMESG) + int cmd_dmesg(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# if CONFIG_NFILE_STREAMS > 0 && !defined(CONFIG_NSH_DISABLESCRIPT) +# ifndef CONFIG_NSH_DISABLE_SH + int cmd_sh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# endif /* CONFIG_NFILE_STREAMS && !CONFIG_NSH_DISABLESCRIPT */ +# ifndef CONFIG_DISABLE_MOUNTPOINT +# ifndef CONFIG_NSH_DISABLE_LOSETUP + int cmd_losetup(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_MKFIFO + int cmd_mkfifo(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifdef CONFIG_FS_READABLE +# ifndef CONFIG_NSH_DISABLE_DF + int cmd_df(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_MOUNT + int cmd_mount(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_UMOUNT + int cmd_umount(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifdef CONFIG_FS_WRITABLE +# ifndef CONFIG_NSH_DISABLE_MKDIR + int cmd_mkdir(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_MKRD + int cmd_mkrd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_MV + int cmd_mv(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_RM + int cmd_rm(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_RMDIR + int cmd_rmdir(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# endif /* CONFIG_FS_WRITABLE */ +# endif /* CONFIG_FS_READABLE */ +# ifdef CONFIG_FS_FAT +# ifndef CONFIG_NSH_DISABLE_MKFATFS + int cmd_mkfatfs(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# endif /* CONFIG_FS_FAT */ +# endif /* !CONFIG_DISABLE_MOUNTPOINT */ +# if !defined(CONFIG_DISABLE_ENVIRON) +# ifndef CONFIG_NSH_DISABLE_CD + int cmd_cd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_PWD + int cmd_pwd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# endif /* !CONFIG_DISABLE_MOUNTPOINT */ +#endif /* CONFIG_NFILE_DESCRIPTORS */ + +#if defined(CONFIG_NET) +# ifndef CONFIG_NSH_DISABLE_IFCONFIG + int cmd_ifconfig(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_GET + int cmd_get(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_PUT + int cmd_put(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif +#if defined(CONFIG_NET_TCP) && CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_WGET + int cmd_wget(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif +#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING) && \ + !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_DISABLE_SIGNALS) +# ifndef CONFIG_NSH_DISABLE_PING + int cmd_ping(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && \ + defined(CONFIG_FS_READABLE) && defined(CONFIG_NFS) +# ifndef CONFIG_NSH_DISABLE_NFSMOUNT + int cmd_nfsmount(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif +#endif + +#ifndef CONFIG_DISABLE_ENVIRON +# ifndef CONFIG_NSH_DISABLE_SET + int cmd_set(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_UNSET + int cmd_unset(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif /* CONFIG_DISABLE_ENVIRON */ + +#ifndef CONFIG_DISABLE_SIGNALS +# ifndef CONFIG_NSH_DISABLE_KILL + int cmd_kill(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_SLEEP + int cmd_sleep(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +# ifndef CONFIG_NSH_DISABLE_USLEEP + int cmd_usleep(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif +#endif /* CONFIG_DISABLE_SIGNALS */ + +#endif /* __APPS_NSHLIB_NSH_H */ diff --git a/apps/nshlib/nsh_apps.c b/apps/nshlib/nsh_apps.c new file mode 100644 index 000000000..e335c2e2c --- /dev/null +++ b/apps/nshlib/nsh_apps.c @@ -0,0 +1,187 @@ +/**************************************************************************** + * apps/nshlib/nsh_apps.c + * + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. + * Copyright (C) 2011 Uros Platise. All rights reserved. + * Author: Uros Platise <uros.platise@isotel.eu> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#ifdef CONFIG_SCHED_WAITPID +# include <sys/wait.h> +#endif + +#include <stdbool.h> +#include <errno.h> +#include <string.h> + +#include <apps/apps.h> + +#include "nsh.h" +#include "nsh_console.h" + +#ifdef CONFIG_NSH_BUILTIN_APPS + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_execapp + * + * Description: + * Attempt to execute the application task whose name is 'cmd' + * + * Returned Value: + * <0 If exec_namedapp() fails, then the negated errno value + * is returned. + * -1 (ERROR) if the application task corresponding to 'cmd' could not + * be started (possibly because it doesn not exist). + * 0 (OK) if the application task corresponding to 'cmd' was + * and successfully started. If CONFIG_SCHED_WAITPID is + * defined, this return value also indicates that the + * application returned successful status (EXIT_SUCCESS) + * 1 If CONFIG_SCHED_WAITPID is defined, then this return value + * indicates that the application task was spawned successfully + * but returned failure exit status. + * + ****************************************************************************/ + +int nsh_execapp(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd, + FAR char **argv) +{ + int ret = OK; + + /* Lock the scheduler to prevent the application from running until the + * waitpid() has been called. + */ + + sched_lock(); + + /* Try to find and execute the command within the list of builtin + * applications. + */ + + ret = exec_namedapp(cmd, (FAR const char **)argv); + if (ret >= 0) + { + /* The application was successfully started (but still blocked because the + * scheduler is locked). If the application was not backgrounded, then we + * need to wait here for the application to exit. + */ + +#ifdef CONFIG_SCHED_WAITPID + if (vtbl->np.np_bg == false) + { + int rc = 0; + + /* Wait for the application to exit. Since we have locked the + * scheduler above, we know that the application has not yet + * started and there is no possibility that it has already exited. + * The scheduler will be unlocked while waitpid is waiting and the + * application will be able to run. + */ + + ret = waitpid(ret, &rc, 0); + if (ret >= 0) + { + /* We can't return the exact status (nsh has nowhere to put it) + * so just pass back zero/nonzero in a fashion that doesn't look + * like an error. + */ + + ret = (rc == 0) ? OK : 1; + + /* TODO: Set the environment variable '?' to a string corresponding + * to WEXITSTATUS(rc) so that $? will expand to the exit status of + * the most recently executed task. + */ + } + } + else +#endif + { + struct sched_param param; + sched_getparam(0, ¶m); + nsh_output(vtbl, "%s [%d:%d]\n", cmd, ret, param.sched_priority); + + /* Backgrounded commands always 'succeed' as long as we can start + * them. + */ + + ret = OK; + } + } + + sched_unlock(); + + /* If exec_namedapp() or waitpid() failed, then return the negated errno + * value. + */ + + if (ret < 0) + { + return -errno; + } + + return ret; +} + +#endif /* CONFIG_NSH_BUILTIN_APPS */ diff --git a/apps/nshlib/nsh_console.c b/apps/nshlib/nsh_console.c new file mode 100644 index 000000000..b066e71f5 --- /dev/null +++ b/apps/nshlib/nsh_console.c @@ -0,0 +1,432 @@ +/**************************************************************************** + * apps/nshlib/nsh_serial.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 Gregory Nutt 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <string.h> +#include <stdarg.h> +#include <assert.h> +#include <errno.h> +#include <debug.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct serialsave_s +{ + int cn_outfd; /* Re-directed output file descriptor */ + FILE *cn_outstream; /* Re-directed output stream */ +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLEBG +static FAR struct nsh_vtbl_s *nsh_consoleclone(FAR struct nsh_vtbl_s *vtbl); +#endif +static void nsh_consolerelease(FAR struct nsh_vtbl_s *vtbl); +static ssize_t nsh_consolewrite(FAR struct nsh_vtbl_s *vtbl, FAR const void *buffer, size_t nbytes); +static int nsh_consoleoutput(FAR struct nsh_vtbl_s *vtbl, const char *fmt, ...); +static FAR char *nsh_consolelinebuffer(FAR struct nsh_vtbl_s *vtbl); +static void nsh_consoleredirect(FAR struct nsh_vtbl_s *vtbl, int fd, FAR uint8_t *save); +static void nsh_consoleundirect(FAR struct nsh_vtbl_s *vtbl, FAR uint8_t *save); +static void nsh_consoleexit(FAR struct nsh_vtbl_s *vtbl, int exitstatus); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_openifnotopen + ****************************************************************************/ + +static int nsh_openifnotopen(struct console_stdio_s *pstate) +{ + /* The stream is open in a lazy fashion. This is done because the file + * descriptor may be opened on a different task than the stream. + */ + + if (!pstate->cn_outstream) + { + pstate->cn_outstream = fdopen(pstate->cn_outfd, "w"); + if (!pstate->cn_outstream) + { + return ERROR; + } + } + return 0; +} + +/**************************************************************************** + * Name: nsh_closeifnotclosed + * + * Description: + * Close the output stream if it is not the standard output stream. + * + ****************************************************************************/ + +static void nsh_closeifnotclosed(struct console_stdio_s *pstate) +{ + if (pstate->cn_outstream == OUTSTREAM(pstate)) + { + fflush(OUTSTREAM(pstate)); + pstate->cn_outfd = OUTFD(pstate); + } + else + { + if (pstate->cn_outstream) + { + fflush(pstate->cn_outstream); + fclose(pstate->cn_outstream); + } + else if (pstate->cn_outfd >= 0 && pstate->cn_outfd != OUTFD(pstate)) + { + close(pstate->cn_outfd); + } + + pstate->cn_outfd = -1; + pstate->cn_outstream = NULL; + } +} + +/**************************************************************************** + * Name: nsh_consolewrite + * + * Description: + * write a buffer to the remote shell window. + * + * Currently only used by cat. + * + ****************************************************************************/ + +static ssize_t nsh_consolewrite(FAR struct nsh_vtbl_s *vtbl, FAR const void *buffer, size_t nbytes) +{ + FAR struct console_stdio_s *pstate = (FAR struct console_stdio_s *)vtbl; + ssize_t ret; + + /* The stream is open in a lazy fashion. This is done because the file + * descriptor may be opened on a different task than the stream. The + * actual open will then occur with the first output from the new task. + */ + + if (nsh_openifnotopen(pstate) != 0) + { + return (ssize_t)ERROR; + } + + /* Write the data to the output stream */ + + ret = fwrite(buffer, 1, nbytes, pstate->cn_outstream); + if (ret < 0) + { + dbg("[%d] Failed to send buffer: %d\n", pstate->cn_outfd, errno); + } + return ret; +} + +/**************************************************************************** + * Name: nsh_consoleoutput + * + * Description: + * Print a string to the currently selected stream. + * + ****************************************************************************/ + +static int nsh_consoleoutput(FAR struct nsh_vtbl_s *vtbl, const char *fmt, ...) +{ + FAR struct console_stdio_s *pstate = (FAR struct console_stdio_s *)vtbl; + va_list ap; + int ret; + + /* The stream is open in a lazy fashion. This is done because the file + * descriptor may be opened on a different task than the stream. The + * actual open will then occur with the first output from the new task. + */ + + if (nsh_openifnotopen(pstate) != 0) + { + return ERROR; + } + + va_start(ap, fmt); + ret = vfprintf(pstate->cn_outstream, fmt, ap); + va_end(ap); + + return ret; +} + +/**************************************************************************** + * Name: nsh_consolelinebuffer + * + * Description: + * Return a reference to the current line buffer + * + ****************************************************************************/ + +static FAR char *nsh_consolelinebuffer(FAR struct nsh_vtbl_s *vtbl) +{ + FAR struct console_stdio_s *pstate = (FAR struct console_stdio_s *)vtbl; + return pstate->cn_line; +} + +/**************************************************************************** + * Name: nsh_consoleclone + * + * Description: + * Make an independent copy of the vtbl + * + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLEBG +static FAR struct nsh_vtbl_s *nsh_consoleclone(FAR struct nsh_vtbl_s *vtbl) +{ + FAR struct console_stdio_s *pclone = nsh_newconsole(); + return &pclone->cn_vtbl; +} +#endif + +/**************************************************************************** + * Name: nsh_consolerelease + * + * Description: + * Release the cloned instance + * + ****************************************************************************/ + +static void nsh_consolerelease(FAR struct nsh_vtbl_s *vtbl) +{ + FAR struct console_stdio_s *pstate = (FAR struct console_stdio_s *)vtbl; + + /* Close the output stream */ + + nsh_closeifnotclosed(pstate); + + /* Close the console stream */ + +#ifdef CONFIG_NSH_CONDEV + (void)fclose(pstate->cn_constream); +#endif + + /* Then release the vtable container */ + + free(pstate); +} + +/**************************************************************************** + * Name: nsh_consoleredirect + * + * Description: + * Set up for redirected output. This function is called from nsh_parse() + * in two different contexts: + * + * 1) Redirected background commands of the form: command > xyz.text & + * + * In this case: + * - vtbl: A newly allocated and initialized instance created by + * nsh_consoleclone, + * - fd:- The file descriptor of the redirected output + * - save: NULL + * + * nsh_consolerelease() will perform the clean-up when the clone is + * destroyed. + * + * 2) Redirected foreground commands of the form: command > xyz.txt + * + * In this case: + * - vtbl: The current state structure, + * - fd: The file descriptor of the redirected output + * - save: Where to save the re-directed registers. + * + * nsh_consoleundirect() will perform the clean-up after the redirected + * command completes. + * + ****************************************************************************/ + +static void nsh_consoleredirect(FAR struct nsh_vtbl_s *vtbl, int fd, FAR uint8_t *save) +{ + FAR struct console_stdio_s *pstate = (FAR struct console_stdio_s *)vtbl; + FAR struct serialsave_s *ssave = (FAR struct serialsave_s *)save; + + /* Case 1: Redirected foreground commands */ + + if (ssave) + { + /* pstate->cn_outstream and cn_outfd refer refer to the + * currently opened output stream. If the output stream is open, flush + * any pending output. + */ + + if (pstate->cn_outstream) + { + fflush(pstate->cn_outstream); + } + + /* Save the current fd and stream values. These will be restored + * when nsh_consoleundirect() is called. + */ + + ssave->cn_outfd = pstate->cn_outfd; + ssave->cn_outstream = pstate->cn_outstream; + } + else + { + /* nsh_consoleclone() set pstate->cn_outfd and cn_outstream to refer + * to standard out. We just want to leave these alone and overwrite + * them with the fd for the re-directed stream. + */ + } + + /* In either case, set the fd of the new, re-directed output and nullify + * the output stream (it will be fdopen'ed if it is used). + */ + + pstate->cn_outfd = fd; + pstate->cn_outstream = NULL; +} + +/**************************************************************************** + * Name: nsh_consoleundirect + * + * Description: + * Set up for redirected output + * + ****************************************************************************/ + +static void nsh_consoleundirect(FAR struct nsh_vtbl_s *vtbl, FAR uint8_t *save) +{ + FAR struct console_stdio_s *pstate = (FAR struct console_stdio_s *)vtbl; + FAR struct serialsave_s *ssave = (FAR struct serialsave_s *)save; + + nsh_closeifnotclosed(pstate); + pstate->cn_outfd = ssave->cn_outfd; + pstate->cn_outstream = ssave->cn_outstream; +} + +/**************************************************************************** + * Name: nsh_consoleexit + * + * Description: + * Exit the shell task + * + ****************************************************************************/ + +static void nsh_consoleexit(FAR struct nsh_vtbl_s *vtbl, int exitstatus) +{ + /* Destroy ourself then exit with the provided status */ + + nsh_consolerelease(vtbl); + exit(exitstatus); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_newconsole + ****************************************************************************/ + +FAR struct console_stdio_s *nsh_newconsole(void) +{ + struct console_stdio_s *pstate = (struct console_stdio_s *)zalloc(sizeof(struct console_stdio_s)); + if (pstate) + { + /* Initialize the call table */ + +#ifndef CONFIG_NSH_DISABLEBG + pstate->cn_vtbl.clone = nsh_consoleclone; + pstate->cn_vtbl.release = nsh_consolerelease; +#endif + pstate->cn_vtbl.write = nsh_consolewrite; + pstate->cn_vtbl.output = nsh_consoleoutput; + pstate->cn_vtbl.linebuffer = nsh_consolelinebuffer; + pstate->cn_vtbl.redirect = nsh_consoleredirect; + pstate->cn_vtbl.undirect = nsh_consoleundirect; + pstate->cn_vtbl.exit = nsh_consoleexit; + + /* (Re-) open the console input device */ + +#ifdef CONFIG_NSH_CONDEV + pstate->cn_confd = open(CONFIG_NSH_CONDEV, O_RDWR); + if (pstate->cn_confd < 0) + { + free(pstate); + return NULL; + } + + /* Create a standard C stream on the console device */ + + pstate->cn_constream = fdopen(pstate->cn_confd, "r+"); + if (!pstate->cn_constream) + { + close(pstate->cn_confd); + free(pstate); + return NULL; + } +#endif + + /* Initialize the output stream */ + + pstate->cn_outfd = OUTFD(pstate); + pstate->cn_outstream = OUTSTREAM(pstate); + } + return pstate; +} diff --git a/apps/nshlib/nsh_console.h b/apps/nshlib/nsh_console.h new file mode 100644 index 000000000..8abf9154b --- /dev/null +++ b/apps/nshlib/nsh_console.h @@ -0,0 +1,159 @@ +/**************************************************************************** + * apps/nshlib/nsh_console.h + * + * Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_NSHLIB_NSH_CONSOLE_H +#define __APPS_NSHLIB_NSH_CONSOLE_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdint.h> +#include <stdbool.h> +#include <errno.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Method access macros */ + +#define nsh_clone(v) (v)->clone(v) +#define nsh_release(v) (v)->release(v) +#define nsh_write(v,b,n) (v)->write(v,b,n) +#define nsh_linebuffer(v) (v)->linebuffer(v) +#define nsh_redirect(v,f,s) (v)->redirect(v,f,s) +#define nsh_undirect(v,s) (v)->undirect(v,s) +#define nsh_exit(v,s) (v)->exit(v,s) + +#ifdef CONFIG_CPP_HAVE_VARARGS +# define nsh_output(v, fmt...) (v)->output(v, ##fmt) +#else +# define nsh_output vtbl->output +#endif + +/* Size of info to be saved in call to nsh_redirect */ + +#define SAVE_SIZE (sizeof(int) + sizeof(FILE*) + sizeof(bool)) + +/* Are we using the NuttX console for I/O? Or some other character device? */ + +#ifdef CONFIG_NSH_CONDEV +# define INFD(p) ((p)->cn_confd) +# define INSTREAM(p) ((p)->cn_constream) +# define OUTFD(p) ((p)->cn_confd) +# define OUTSTREAM(p) ((p)->cn_constream) +#else +# define INFD(p) 0 +# define INSTREAM(p) stdin +# define OUTFD(p) 1 +# define OUTSTREAM(p) stdout +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ +/* This describes a generic console front-end */ + +struct nsh_vtbl_s +{ + /* This function pointers are "hooks" into the front end logic to + * handle things like output of command results, redirection, etc. + * -- all of which must be done in a way that is unique to the nature + * of the front end. + */ + +#ifndef CONFIG_NSH_DISABLEBG + FAR struct nsh_vtbl_s *(*clone)(FAR struct nsh_vtbl_s *vtbl); + void (*addref)(FAR struct nsh_vtbl_s *vtbl); + void (*release)(FAR struct nsh_vtbl_s *vtbl); +#endif + ssize_t (*write)(FAR struct nsh_vtbl_s *vtbl, FAR const void *buffer, size_t nbytes); + int (*output)(FAR struct nsh_vtbl_s *vtbl, const char *fmt, ...); + FAR char *(*linebuffer)(FAR struct nsh_vtbl_s *vtbl); + void (*redirect)(FAR struct nsh_vtbl_s *vtbl, int fd, FAR uint8_t *save); + void (*undirect)(FAR struct nsh_vtbl_s *vtbl, FAR uint8_t *save); + void (*exit)(FAR struct nsh_vtbl_s *vtbl, int exitstatus); + + /* Parser state data */ + + struct nsh_parser_s np; +}; + +/* This structure describes a console front-end that is based on stdin and + * stdout (which is all of the supported console types at the time being). + */ + +struct console_stdio_s +{ + /* NSH front-end call table */ + + struct nsh_vtbl_s cn_vtbl; + + /* NSH input/output streams */ + +#ifdef CONFIG_NSH_CONDEV + int cn_confd; /* Console I/O file descriptor */ +#endif + int cn_outfd; /* Output file descriptor (possibly redirected) */ +#ifdef CONFIG_NSH_CONDEV + FILE *cn_constream; /* Console I/O stream (possibly redirected) */ +#endif + FILE *cn_outstream; /* Output stream */ + + /* Line input buffer */ + + char cn_line[CONFIG_NSH_LINELEN]; +}; + + + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/* Defined in nsh_console.c *************************************************/ + +FAR struct console_stdio_s *nsh_newconsole(void); + +#endif /* __APPS_NSHLIB_NSH_CONSOLE_H */ diff --git a/apps/nshlib/nsh_consolemain.c b/apps/nshlib/nsh_consolemain.c new file mode 100644 index 000000000..baf2e74b4 --- /dev/null +++ b/apps/nshlib/nsh_consolemain.c @@ -0,0 +1,169 @@ +/**************************************************************************** + * apps/nshlib/nsh_consolemain.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 Gregory Nutt 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <assert.h> + +#include <apps/readline.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_consolemain + * + * Description: + * This interfaces maybe to called or started with task_start to start a + * single an NSH instance that operates on stdin and stdout (/dev/console). + * This function does not return. + * + * Input Parameters: + * Standard task start-up arguements. These are not used. argc may be + * zero and argv may be NULL. + * + * Returned Values: + * This function does not normally return. exit() is usually called to + * terminate the NSH session. This function will return in the event of + * an error. In that case, a nonzero value is returned (1). + * + ****************************************************************************/ + +int nsh_consolemain(int argc, char *argv[]) +{ + FAR struct console_stdio_s *pstate = nsh_newconsole(); + DEBUGASSERT(pstate); + int ret; + + /* If we are using a USB serial console, then we will have to wait for the + * USB to be connected to the host. + */ + +#ifdef HAVE_USB_CONSOLE + ret = nsh_usbconsole(); + DEBUGASSERT(ret == OK); +#endif + + /* Present a greeting */ + + fputs(g_nshgreeting, pstate->cn_outstream); + fflush(pstate->cn_outstream); + + /* Execute the startup script */ + +#ifdef CONFIG_NSH_ROMFSETC + (void)nsh_script(&pstate->cn_vtbl, "init", NSH_INITPATH); +#endif + + /* Then enter the command line parsing loop */ + + for (;;) + { + /* For the case of debugging the USB console... dump collected USB trace data */ + + nsh_usbtrace(); + + /* Display the prompt string */ + + fputs(g_nshprompt, pstate->cn_outstream); + fflush(pstate->cn_outstream); + + /* Get the next line of input */ + + ret = readline(pstate->cn_line, CONFIG_NSH_LINELEN, + INSTREAM(pstate), OUTSTREAM(pstate)); + if (ret > 0) + { + /* Parse process the command */ + + (void)nsh_parse(&pstate->cn_vtbl, pstate->cn_line); + fflush(pstate->cn_outstream); + } + + /* Readline normally returns the number of characters read, + * but will return 0 on end of file or a negative value + * if an error occurs. Either will cause the session to + * terminate. + */ + + else + { + fprintf(pstate->cn_outstream, g_fmtcmdfailed, "nsh_consolemain", + "readline", NSH_ERRNO_OF(-ret)); + nsh_exit(&pstate->cn_vtbl, 1); + } + } + + /* Clean up */ + + nsh_exit(&pstate->cn_vtbl, 0); + + /* We do not get here, but this is necessary to keep some compilers happy */ + + return OK; +} diff --git a/apps/nshlib/nsh_dbgcmds.c b/apps/nshlib/nsh_dbgcmds.c new file mode 100644 index 000000000..384b377f3 --- /dev/null +++ b/apps/nshlib/nsh_dbgcmds.c @@ -0,0 +1,355 @@ +/**************************************************************************** + * apps/nshlib/dbg_dbgcmds.c + * + * Copyright (C) 2008-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <errno.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct dbgmem_s +{ + bool dm_write; /* true: perfrom write operation */ + void *dm_addr; /* Address to access */ + uint32_t dm_value; /* Value to write */ + unsigned int dm_count; /* The number of bytes to access */ +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: mem_parse + ****************************************************************************/ + +int mem_parse(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv, + struct dbgmem_s *mem) +{ + char *pcvalue = strchr(argv[1], '='); + unsigned long lvalue = 0; + + /* Check if we are writing a value */ + + if (pcvalue) + { + *pcvalue = '\0'; + pcvalue++; + + lvalue = (unsigned long)strtol(pcvalue, NULL, 16); + if (lvalue > 0xffffffff) + { + return -EINVAL; + } + + mem->dm_write = true; + mem->dm_value = (uint32_t)lvalue; + } + else + { + mem->dm_write = false; + mem->dm_value = 0; + } + + /* Get the address to be accessed */ + + mem->dm_addr = (void*)((uintptr_t)strtol(argv[1], NULL, 16)); + + /* Get the number of bytes to access */ + + if (argc > 2) + { + mem->dm_count = (unsigned int)strtol(argv[2], NULL, 16); + } + else + { + mem->dm_count = 1; + } + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_mb + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_MB +int cmd_mb(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct dbgmem_s mem; + volatile uint8_t *ptr; + int ret; + int i; + + ret = mem_parse(vtbl, argc, argv, &mem); + if (ret == 0) + { + /* Loop for the number of requested bytes */ + + for (i = 0, ptr = (volatile uint8_t*)mem.dm_addr; i < mem.dm_count; i++, ptr++) + { + /* Print the value at the address */ + + nsh_output(vtbl, " %p = 0x%02x", ptr, *ptr); + + /* Are we supposed to write a value to this address? */ + + if (mem.dm_write) + { + /* Yes, was the supplied value within range? */ + + if (mem.dm_value > 0x000000ff) + { + nsh_output(vtbl, g_fmtargrange, argv[0]); + return ERROR; + } + + /* Write the value and re-read the address so that we print its + * current value (if the address is a process address, then the + * value read might not necessarily be the value written). + */ + + *ptr = (uint8_t)mem.dm_value; + nsh_output(vtbl, " -> 0x%02x", *ptr); + } + + /* Make sure we end it with a newline */ + + nsh_output(vtbl, "\n", *ptr); + } + } + return ret; +} +#endif + +/**************************************************************************** + * Name: cmd_mh + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_MH +int cmd_mh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct dbgmem_s mem; + volatile uint16_t *ptr; + int ret; + int i; + + ret = mem_parse(vtbl, argc, argv, &mem); + if (ret == 0) + { + /* Loop for the number of requested bytes */ + + for (i = 0, ptr = (volatile uint16_t*)mem.dm_addr; i < mem.dm_count; i += 2, ptr++) + { + /* Print the value at the address */ + + nsh_output(vtbl, " %p = 0x%04x", ptr, *ptr); + + /* Are we supposed to write a value to this address? */ + + if (mem.dm_write) + { + /* Yes, was the supplied value within range? */ + + if (mem.dm_value > 0x0000ffff) + { + nsh_output(vtbl, g_fmtargrange, argv[0]); + return ERROR; + } + + /* Write the value and re-read the address so that we print its + * current value (if the address is a process address, then the + * value read might not necessarily be the value written). + */ + + *ptr = (uint16_t)mem.dm_value; + nsh_output(vtbl, " -> 0x%04x", *ptr); + } + + /* Make sure we end it with a newline */ + + nsh_output(vtbl, "\n", *ptr); + } + } + return ret; +} +#endif + +/**************************************************************************** + * Name: cmd_mw + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_MW +int cmd_mw(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct dbgmem_s mem; + volatile uint32_t *ptr; + int ret; + int i; + + ret = mem_parse(vtbl, argc, argv, &mem); + if (ret == 0) + { + /* Loop for the number of requested bytes */ + + for (i = 0, ptr = (volatile uint32_t*)mem.dm_addr; i < mem.dm_count; i += 4, ptr++) + { + /* Print the value at the address */ + + nsh_output(vtbl, " %p = 0x%08x", ptr, *ptr); + + /* Are we supposed to write a value to this address? */ + + if (mem.dm_write) + { + /* Write the value and re-read the address so that we print its + * current value (if the address is a process address, then the + * value read might not necessarily be the value written). + */ + + *ptr = mem.dm_value; + nsh_output(vtbl, " -> 0x%08x", *ptr); + } + + /* Make sure we end it with a newline */ + + nsh_output(vtbl, "\n", *ptr); + } + } + return ret; +} +#endif + +/**************************************************************************** + * Name: nsh_dumpbuffer + ****************************************************************************/ + +void nsh_dumpbuffer(FAR struct nsh_vtbl_s *vtbl, const char *msg, + const uint8_t *buffer, ssize_t nbytes) +{ + char line[128]; + int ch; + int i; + int j; + + nsh_output(vtbl, "%s:\n", msg); + for (i = 0; i < nbytes; i += 16) + { + sprintf(line, "%04x: ", i); + + for ( j = 0; j < 16; j++) + { + if (i + j < nbytes) + { + sprintf(&line[strlen(line)], "%02x ", buffer[i+j] ); + } + else + { + strcpy(&line[strlen(line)], " "); + } + } + + for ( j = 0; j < 16; j++) + { + if (i + j < nbytes) + { + ch = buffer[i+j]; + sprintf(&line[strlen(line)], "%c", ch >= 0x20 && ch <= 0x7e ? ch : '.'); + } + } + nsh_output(vtbl, "%s\n", line); + } +} + +/**************************************************************************** + * Name: cmd_xd + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_XD +int cmd_xd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + FAR char *addr; + FAR char *endptr; + int nbytes; + + addr = (char*)((uintptr_t)strtol(argv[1], &endptr, 16)); + if (argv[0][0] == '\0' || *endptr != '\0') + { + return ERROR; + } + + nbytes = (int)strtol(argv[2], &endptr, 0); + if (argv[0][0] == '\0' || *endptr != '\0' || nbytes < 0) + { + return ERROR; + } + + nsh_dumpbuffer(vtbl, "Hex dump", (uint8_t*)addr, nbytes); + return OK; +} +#endif diff --git a/apps/nshlib/nsh_ddcmd.c b/apps/nshlib/nsh_ddcmd.c new file mode 100644 index 000000000..40a3710b1 --- /dev/null +++ b/apps/nshlib/nsh_ddcmd.c @@ -0,0 +1,643 @@ +/**************************************************************************** + * apps/nshlib/nsh_ddcmd.c + * + * Copyright (C) 2008-2009, 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/stat.h> + +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <string.h> +#include <debug.h> +#include <errno.h> + +#include <nuttx/fs/fs.h> + +#include "nsh.h" +#include "nsh_console.h" + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_NSH_DISABLE_DD) + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* If no sector size is specified wity BS=, then the following default value + * is used. + */ + +#define DEFAULT_SECTSIZE 512 + +/* At present, piping of input and output are not support, i.e., both of= + * and if= arguments are required. + */ + +#undef CAN_PIPE_FROM_STD + +/* Function pointer calls are only need if block drivers are supported + * (or, rather, if mount points are supported in the file system) + */ + +#ifndef CONFIG_DISABLE_MOUNTPOINT +# define DD_INFD ((dd)->inf.fd) +# define DD_INHANDLE ((dd)->inf.handle) +# define DD_OUTFD ((dd)->outf.fd) +# define DD_OUTHANDLE ((dd)->outf.handle) +# define DD_READ(dd) ((dd)->infread(dd)) +# define DD_WRITE(dd) ((dd)->outfwrite(dd)) +# define DD_INCLOSE(dd) ((dd)->infclose(dd)) +# define DD_OUTCLOSE(dd) ((dd)->outfclose(dd)) +#else +# define DD_INFD ((dd)->infd) +# undef DD_INHANDLE +# define DD_OUTFD ((dd)->outfd) +# undef DD_OUTHANDLE +# define DD_READ(dd) dd_readch(dd) +# define DD_WRITE(dd) dd_writech(dd) +# define DD_INCLOSE(dd) dd_infclosech(dd) +# define DD_OUTCLOSE(dd) dd_outfclosech(dd) +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct dd_s +{ + FAR struct nsh_vtbl_s *vtbl; + +#ifndef CONFIG_DISABLE_MOUNTPOINT + union + { + FAR void *handle; /* BCH lib handle for block device*/ + int fd; /* File descriptor of the character device */ + } inf; +#else + int infd; /* File descriptor of the input device */ +#endif + +#ifndef CONFIG_DISABLE_MOUNTPOINT + union + { + FAR void *handle; /* BCH lib handle for block device*/ + int fd; /* File descriptor of the character device */ + } outf; +#else + int outfd; /* File descriptor of the output device */ +#endif + + uint32_t nsectors; /* Number of sectors to transfer */ + uint32_t sector; /* The current sector number */ + uint32_t skip; /* The number of sectors skipped on input */ + bool eof; /* true: The of the input or output file has been hit */ + uint16_t sectsize; /* Size of one sector */ + uint16_t nbytes; /* Number of valid bytes in the buffer */ + uint8_t *buffer; /* Buffer of data to write to the output file */ + + /* Function pointers to handle differences between block and character devices */ + +#ifndef CONFIG_DISABLE_MOUNTPOINT + int (*infread)(struct dd_s *dd); + void (*infclose)(struct dd_s *dd); + int (*outfwrite)(struct dd_s *dd); + void (*outfclose)(struct dd_s *dd); +#endif +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const char g_dd[] = "dd"; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: dd_outfcloseblk + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_MOUNTPOINT +static void dd_outfcloseblk(struct dd_s *dd) +{ + (void)bchlib_teardown(DD_OUTHANDLE); +} +#endif + + +/**************************************************************************** + * Name: dd_outfclosech + ****************************************************************************/ + +static void dd_outfclosech(struct dd_s *dd) +{ + (void)close(DD_OUTFD); +} + +/**************************************************************************** + * Name: dd_infcloseblk + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_MOUNTPOINT +static void dd_infcloseblk(struct dd_s *dd) +{ + (void)bchlib_teardown(DD_INHANDLE); +} +#endif + +/**************************************************************************** + * Name: dd_infclosech + ****************************************************************************/ + +static void dd_infclosech(struct dd_s *dd) +{ + (void)close(DD_INFD); +} + +/**************************************************************************** + * Name: dd_writeblk + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_MOUNTPOINT +static int dd_writeblk(struct dd_s *dd) +{ + ssize_t nbytes; + off_t offset = (dd->sector - dd->skip) * dd->sectsize; + + /* Write the sector at the specified offset */ + + nbytes = bchlib_write(DD_OUTHANDLE, (char*)dd->buffer, offset, dd->sectsize); + if (nbytes < 0) + { + /* bchlib_write return -EFBIG on attempts to write past the end of + * the device. + */ + + if (nbytes == -EFBIG) + { + dd->eof = true; /* Set end-of-file */ + } + else + { + FAR struct nsh_vtbl_s *vtbl = dd->vtbl; + nsh_output(vtbl, g_fmtcmdfailed, g_dd, "bshlib_write", NSH_ERRNO_OF(-nbytes)); + return ERROR; + } + } + + return OK; +} +#endif + +/**************************************************************************** + * Name: dd_writech + ****************************************************************************/ + +static int dd_writech(struct dd_s *dd) +{ + uint8_t *buffer = dd->buffer; + uint16_t written ; + ssize_t nbytes; + + /* Is the out buffer full (or is this the last one)? */ + + written = 0; + do + { + nbytes = write(DD_OUTFD, buffer, dd->sectsize - written); + if (nbytes < 0) + { + FAR struct nsh_vtbl_s *vtbl = dd->vtbl; + nsh_output(vtbl, g_fmtcmdfailed, g_dd, "write", NSH_ERRNO_OF(-nbytes)); + return ERROR; + } + + written += nbytes; + buffer += nbytes; + } + while (written < dd->sectsize); + + return OK; +} + +/**************************************************************************** + * Name: dd_readblk + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_MOUNTPOINT +static int dd_readblk(struct dd_s *dd) +{ + ssize_t nbytes; + off_t offset = dd->sector * dd->sectsize; + + nbytes = bchlib_read(DD_INHANDLE, (char*)dd->buffer, offset, dd->sectsize); + if (nbytes < 0) + { + FAR struct nsh_vtbl_s *vtbl = dd->vtbl; + nsh_output(vtbl, g_fmtcmdfailed, g_dd, "bshlib_read", NSH_ERRNO_OF(-nbytes)); + return ERROR; + } + + /* bchlib_read return 0 on attempts to write past the end of the device. */ + + dd->nbytes = nbytes; + dd->eof = (nbytes == 0); + return OK; +} +#endif + +/**************************************************************************** + * Name: dd_readch + ****************************************************************************/ + +static int dd_readch(struct dd_s *dd) +{ + uint8_t *buffer = dd->buffer; + ssize_t nbytes; + + dd->nbytes = 0; + do + { + nbytes = read(DD_INFD, buffer, dd->sectsize - dd->nbytes); + if (nbytes < 0) + { + FAR struct nsh_vtbl_s *vtbl = dd->vtbl; + nsh_output(vtbl, g_fmtcmdfailed, g_dd, "read", NSH_ERRNO_OF(-nbytes)); + return ERROR; + } + + dd->nbytes += nbytes; + buffer += nbytes; + } + while (dd->nbytes < dd->sectsize && nbytes > 0); + + dd->eof |= (dd->nbytes == 0); + return OK; +} + +/**************************************************************************** + * Name: dd_infopen + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_MOUNTPOINT +static int dd_filetype(const char *filename) +{ + struct stat sb; + int ret; + + /* Get the type of the file */ + + ret = stat(filename, &sb); + if (ret < 0) + { + return ERROR; /* Return -1 on failure */ + } + + return S_ISBLK(sb.st_mode); /* Return true(1) if block, false(0) if char */ +} +#endif + +/**************************************************************************** + * Name: dd_infopen + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_MOUNTPOINT +static inline int dd_infopen(const char *name, struct dd_s *dd) +{ + FAR struct nsh_vtbl_s *vtbl = dd->vtbl; + int ret; + int type; + + /* Get the type of the input file */ + + type = dd_filetype(name); + if (type < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, g_dd, "stat", NSH_ERRNO_OF(-type)); + return type; + } + + /* Open the input file */ + + if (!type) + { + DD_INFD = open(name, O_RDONLY); + if (DD_INFD < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, g_dd, "open", NSH_ERRNO); + return ERROR; + } + + dd->infread = dd_readch; /* Character oriented read */ + dd->infclose = dd_infclosech; + } + else + { + ret = bchlib_setup(name, true, &DD_INHANDLE); + if (ret < 0) + { + return ERROR; + } + + dd->infread = dd_readblk; + dd->infclose = dd_infcloseblk; + } + return OK; +} +#else +static inline int dd_infopen(const char *name, struct dd_s *dd) +{ + DD_INFD = open(name, O_RDONLY); + if (DD_INFD < 0) + { + FAR struct nsh_vtbl_s *vtbl = dd->vtbl; + nsh_output(vtbl, g_fmtcmdfailed, g_dd, "open", NSH_ERRNO); + return ERROR; + } + return OK; +} +#endif + +/**************************************************************************** + * Name: dd_outfopen + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_MOUNTPOINT +static inline int dd_outfopen(const char *name, struct dd_s *dd) +{ + int type; + int ret = OK; + + /* Get the type of the output file */ + + type = dd_filetype(name); + + /* Open the block driver for input */ + + if (type == true) + { + ret = bchlib_setup(name, true, &DD_OUTHANDLE); + if (ret < 0) + { + return ERROR; + } + + dd->outfwrite = dd_writeblk; /* Block oriented write */ + dd->outfclose = dd_outfcloseblk; + } + + /* Otherwise, the file is character oriented or does not exist */ + + else + { + DD_OUTFD = open(name, O_WRONLY|O_CREAT|O_TRUNC, 0644); + if (DD_OUTFD < 0) + { + FAR struct nsh_vtbl_s *vtbl = dd->vtbl; + nsh_output(vtbl, g_fmtcmdfailed, g_dd, "open", NSH_ERRNO); + return ERROR; + } + + dd->outfwrite = dd_writech; /* Character oriented write */ + dd->outfclose = dd_outfclosech; + } + return OK; +} +#else +static inline int dd_outfopen(const char *name, struct dd_s *dd) +{ + DD_OUTFD = open(name, O_WRONLY|O_CREAT|O_TRUNC, 0644); + if (DD_OUTFD < 0) + { + nsh_output(dd->vtbl, g_fmtcmdfailed, g_dd, "open", NSH_ERRNO); + return ERROR; + } + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_dd + ****************************************************************************/ + +int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct dd_s dd; + char *infile = NULL; + char *outfile = NULL; + int ret = ERROR; + int i; + + /* Initialize the dd structure */ + + memset(&dd, 0, sizeof(struct dd_s)); + dd.vtbl = vtbl; /* For nsh_output */ + dd.sectsize = DEFAULT_SECTSIZE; /* Sector size if 'bs=' not provided */ + dd.nsectors = 0xffffffff; /* MAX_UINT32 */ + + /* If no IF= option is provided on the command line, then read + * from stdin. + */ + +#ifdef CAN_PIPE_FROM_STD + DD_INFD = 0; /* stdin */ +#ifndef CONFIG_NSH_DISABLE_DD + dd.infread = readch; /* Character oriented read */ + dd.infclose = noclose; /* Don't close stdin */ +#endif +#endif + /* If no OF= option is provided on the command line, then write + * to stdout. + */ + +#ifdef CAN_PIPE_FROM_STD + DD_OUTDF = 1; /* stdout */ +#ifndef CONFIG_NSH_DISABLE_DD + dd.outfwrite = writech; /* Character oriented write */ + dd.outfclose = noclose; /* Don't close stdout */ +#endif +#endif + + /* Parse command line parameters */ + + for (i = 1; i < argc; i++) + { + if (strncmp(argv[i], "if=", 3) == 0) + { + infile = nsh_getfullpath(vtbl, &argv[i][3]); + } + else if (strncmp(argv[i], "of=", 3) == 0) + { + outfile = nsh_getfullpath(vtbl, &argv[i][3]); + } + else if (strncmp(argv[i], "bs=", 3) == 0) + { + dd.sectsize = atoi(&argv[i][3]); + } + else if (strncmp(argv[i], "count=", 6) == 0) + { + dd.nsectors = atoi(&argv[i][6]); + } + else if (strncmp(argv[i], "skip=", 5) == 0) + { + dd.skip = atoi(&argv[i][5]); + } + } + +#ifndef CAN_PIPE_FROM_STD + if (!infile || !outfile) + { + nsh_output(vtbl, g_fmtargrequired, g_dd); + goto errout_with_paths; + } +#endif + + if (dd.skip < 0 || dd.skip > dd.nsectors) + { + nsh_output(vtbl, g_fmtarginvalid, g_dd); + goto errout_with_paths; + } + + /* Allocate the I/O buffer */ + + dd.buffer = malloc(dd.sectsize); + if (!dd.buffer) + { + nsh_output(vtbl, g_fmtcmdoutofmemory, g_dd); + goto errout_with_paths; + } + + /* Open the input file */ + + ret = dd_infopen(infile, &dd); + if (ret < 0) + { + goto errout_with_paths; + } + + /* Open the output file */ + + ret = dd_outfopen(outfile, &dd); + if (ret < 0) + { + goto errout_with_inf; + } + + /* Then perform the data transfer */ + + dd.sector = 0; + while (!dd.eof && dd.nsectors > 0) + { + /* Read one sector from from the input */ + + ret = DD_READ(&dd); + if (ret < 0) + { + goto errout_with_outf; + } + + /* Has the incoming data stream ended? */ + + if (!dd.eof) + { + /* Pad with zero if necessary (at the end of file only) */ + + for (i = dd.nbytes; i < dd.sectsize; i++) + { + dd.buffer[i] = 0; + } + + /* Write one sector to the output file */ + + if (dd.sector >= dd.skip) + { + ret = DD_WRITE(&dd); + if (ret < 0) + { + goto errout_with_outf; + } + + /* Decrement to show that a sector was written */ + + dd.nsectors--; + } + + /* Increment the sector number */ + + dd.sector++; + } + } + ret = OK; + +errout_with_outf: + DD_INCLOSE(&dd); +errout_with_inf: + DD_OUTCLOSE(&dd); + free(dd.buffer); +errout_with_paths: + if (infile) + { + free(infile); + } + if (outfile) + { + free(outfile); + } + return ret; +} + +#endif /* CONFIG_NFILE_DESCRIPTORS && !CONFIG_NSH_DISABLE_DD */ + diff --git a/apps/nshlib/nsh_envcmds.c b/apps/nshlib/nsh_envcmds.c new file mode 100644 index 000000000..07b775517 --- /dev/null +++ b/apps/nshlib/nsh_envcmds.c @@ -0,0 +1,338 @@ +/**************************************************************************** + * apps/nshlib/nsh_envcmds.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <libgen.h> +#include <errno.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +static const char g_pwd[] = "PWD"; +static const char g_oldpwd[] = "OLDPWD"; +static const char g_home[] = CONFIG_LIB_HOMEDIR; +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_getwd + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +static inline FAR const char *nsh_getwd(const char *wd) +{ + const char *val; + + /* If no working directory is defined, then default to the home directory */ + + val = getenv(wd); + if (!val) + { + val = g_home; + } + return val; +} +#endif + +/**************************************************************************** + * Name: nsh_getdirpath + ****************************************************************************/ + +static inline char *nsh_getdirpath(FAR struct nsh_vtbl_s *vtbl, + const char *dirpath, const char *relpath) +{ + char *alloc; + int len; + + /* Handle the special case where the dirpath is simply "/" */ + + if (strcmp(dirpath, "/") == 0) + { + len = strlen(relpath) + 2; + alloc = (char*)malloc(len); + if (alloc) + { + sprintf(alloc, "/%s", relpath); + } + } + else + { + len = strlen(dirpath) + strlen(relpath) + 2; + alloc = (char*)malloc(len); + if (alloc) + { + sprintf(alloc, "%s/%s", dirpath, relpath); + } + } + + if (!alloc) + { + nsh_output(vtbl, g_fmtcmdoutofmemory, "nsh_getdirpath"); + } + return alloc; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_getwd + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +FAR const char *nsh_getcwd(void) +{ + return nsh_getwd(g_pwd); +} +#endif + +/**************************************************************************** + * Name: nsh_getfullpath + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +char *nsh_getfullpath(FAR struct nsh_vtbl_s *vtbl, const char *relpath) +{ + const char *wd; + + /* Handle some special cases */ + + if (!relpath || relpath[0] == '\0') + { + /* No relative path provided */ + + return strdup(g_home); + } + else if (relpath[0] == '/') + { + return strdup(relpath); + } + + /* Get the path to the current working directory */ + + wd = nsh_getcwd(); + + /* Fake the '.' directory */ + + if (strcmp(relpath, ".") == 0) + { + return strdup(wd); + } + + /* Return the full path */ + + return nsh_getdirpath(vtbl, wd, relpath); +} +#endif + +/**************************************************************************** + * Name: nsh_freefullpath + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +void nsh_freefullpath(char *relpath) +{ + if (relpath) + { + free(relpath); + } +} +#endif + +/**************************************************************************** + * Name: cmd_cd + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +#ifndef CONFIG_NSH_DISABLE_CD +int cmd_cd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + const char *path = argv[1]; + char *alloc = NULL; + char *fullpath = NULL; + int ret = OK; + + /* Check for special arguments */ + + if (argc < 2 || strcmp(path, "~") == 0) + { + path = g_home; + } + else if (strcmp(path, "-") == 0) + { + alloc = strdup(nsh_getwd(g_oldpwd)); + path = alloc; + } + else if (strcmp(path, "..") == 0) + { + alloc = strdup(nsh_getcwd()); + path = dirname(alloc); + } + else + { + fullpath = nsh_getfullpath(vtbl, path); + path = fullpath; + } + + /* Set the new workding directory */ + + ret = chdir(path); + if (ret != 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "chdir", NSH_ERRNO); + ret = ERROR; + } + + /* Free any memory that was allocated */ + + if (alloc) + { + free(alloc); + } + + if (fullpath) + { + nsh_freefullpath(fullpath); + } + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_echo + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_ECHO +int cmd_echo(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + int i; + + /* echo each argument, separated by a space as it must have been on the + * command line + */ + + for (i = 1; i < argc; i++) + { + nsh_output(vtbl, "%s ", argv[i]); + } + nsh_output(vtbl, "\n"); + return OK; +} +#endif + +/**************************************************************************** + * Name: cmd_pwd + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +#ifndef CONFIG_NSH_DISABLE_PWD +int cmd_pwd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + nsh_output(vtbl, "%s\n", nsh_getcwd()); + return OK; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_set + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_ENVIRON +#ifndef CONFIG_NSH_DISABLE_SET +int cmd_set(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + int ret = setenv(argv[1], argv[2], TRUE); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "setenv", NSH_ERRNO); + } + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_unset + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_ENVIRON +#ifndef CONFIG_NSH_DISABLE_UNSET +int cmd_unset(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + int ret = unsetenv(argv[1]); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "unsetenv", NSH_ERRNO); + } + return ret; +} +#endif +#endif diff --git a/apps/nshlib/nsh_fscmds.c b/apps/nshlib/nsh_fscmds.c new file mode 100644 index 000000000..1a9f2eb57 --- /dev/null +++ b/apps/nshlib/nsh_fscmds.c @@ -0,0 +1,1299 @@ +/**************************************************************************** + * apps/nshlib/nsh_fscmds.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> + +#if CONFIG_NFILE_DESCRIPTORS > 0 +# include <sys/stat.h> +# include <fcntl.h> +# if !defined(CONFIG_DISABLE_MOUNTPOINT) +# ifdef CONFIG_FS_READABLE /* Need at least one filesytem in configuration */ +# include <sys/mount.h> +# include <nuttx/ramdisk.h> +# endif +# ifdef CONFIG_FS_FAT +# include <nuttx/fs/mkfatfs.h> +# endif +# ifdef CONFIG_NFS +# include <sys/socket.h> +# include <netinet/in.h> +# include <nuttx/fs/nfs.h> +# endif +# ifdef CONFIG_RAMLOG_SYSLOG +# include <nuttx/ramlog.h> +# endif +#endif +#endif + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <dirent.h> +#include <limits.h> +#include <libgen.h> +#include <errno.h> +#include <debug.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define LSFLAGS_SIZE 1 +#define LSFLAGS_LONG 2 +#define LSFLAGS_RECURSIVE 4 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +typedef int (*direntry_handler_t)(FAR struct nsh_vtbl_s *, const char *, + struct dirent *, void *); + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* Common buffer for file I/O. Note the use of this common buffer precludes + * multiple copies of NSH running concurrently. It should be allocated per + * NSH instance and retained in the "vtbl" as is done for the telnet + * connection. + */ + +static char g_iobuffer[IOBUFFERSIZE]; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: trim_dir + ****************************************************************************/ + +static void trim_dir(char *arg) +{ + /* Skip any trailing '/' characters (unless it is also the leading '/') */ + + int len = strlen(arg) - 1; + while (len > 0 && arg[len] == '/') + { + arg[len] = '\0'; + len--; + } +} + +/**************************************************************************** + * Name: nsh_getdirpath + ****************************************************************************/ + +static char *nsh_getdirpath(const char *path, const char *file) +{ + /* Handle the case where all that is left is '/' */ + + if (strcmp(path, "/") == 0) + { + sprintf(g_iobuffer, "/%s", file); + } + else + { + sprintf(g_iobuffer, "%s/%s", path, file); + } + + g_iobuffer[PATH_MAX] = '\0'; + return strdup(g_iobuffer); +} + +/**************************************************************************** + * Name: foreach_direntry + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +static int foreach_direntry(FAR struct nsh_vtbl_s *vtbl, const char *cmd, const char *dirpath, + direntry_handler_t handler, void *pvarg) +{ + DIR *dirp; + int ret = OK; + + /* Trim trailing '/' from directory names */ + +#ifdef CONFIG_NSH_FULLPATH + trim_dir(arg); +#endif + + /* Open the directory */ + + dirp = opendir(dirpath); + + if (!dirp) + { + /* Failed to open the directory */ + + nsh_output(vtbl, g_fmtnosuch, cmd, "directory", dirpath); + return ERROR; + } + + /* Read each directory entry */ + + for (;;) + { + struct dirent *entryp = readdir(dirp); + if (!entryp) + { + /* Finished with this directory */ + + break; + } + + /* Call the handler with this directory entry */ + + if (handler(vtbl, dirpath, entryp, pvarg) < 0) + { + /* The handler reported a problem */ + + ret = ERROR; + break; + } + } + + closedir(dirp); + return ret; +} +#endif + +/**************************************************************************** + * Name: ls_specialdir + ****************************************************************************/ + +static inline int ls_specialdir(const char *dir) +{ + /* '.' and '..' directories are not listed like normal directories */ + + return (strcmp(dir, ".") == 0 || strcmp(dir, "..") == 0); +} + +/**************************************************************************** + * Name: ls_handler + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +static int ls_handler(FAR struct nsh_vtbl_s *vtbl, const char *dirpath, struct dirent *entryp, void *pvarg) +{ + unsigned int lsflags = (unsigned int)pvarg; + int ret; + + /* Check if any options will require that we stat the file */ + + if ((lsflags & (LSFLAGS_SIZE|LSFLAGS_LONG)) != 0) + { + struct stat buf; + char *fullpath = nsh_getdirpath(dirpath, entryp->d_name); + + /* Yes, stat the file */ + + ret = stat(fullpath, &buf); + free(fullpath); + if (ret != 0) + { + nsh_output(vtbl, g_fmtcmdfailed, "ls", "stat", NSH_ERRNO); + return ERROR; + } + + if ((lsflags & LSFLAGS_LONG) != 0) + { + char details[] = "----------"; + if (S_ISDIR(buf.st_mode)) + { + details[0]='d'; + } + else if (S_ISCHR(buf.st_mode)) + { + details[0]='c'; + } + else if (S_ISBLK(buf.st_mode)) + { + details[0]='b'; + } + + if ((buf.st_mode & S_IRUSR) != 0) + { + details[1]='r'; + } + + if ((buf.st_mode & S_IWUSR) != 0) + { + details[2]='w'; + } + + if ((buf.st_mode & S_IXUSR) != 0) + { + details[3]='x'; + } + + if ((buf.st_mode & S_IRGRP) != 0) + { + details[4]='r'; + } + + if ((buf.st_mode & S_IWGRP) != 0) + { + details[5]='w'; + } + + if ((buf.st_mode & S_IXGRP) != 0) + { + details[6]='x'; + } + + if ((buf.st_mode & S_IROTH) != 0) + { + details[7]='r'; + } + + if ((buf.st_mode & S_IWOTH) != 0) + { + details[8]='w'; + } + + if ((buf.st_mode & S_IXOTH) != 0) + { + details[9]='x'; + } + + nsh_output(vtbl, " %s", details); + } + + if ((lsflags & LSFLAGS_SIZE) != 0) + { + nsh_output(vtbl, "%8d", buf.st_size); + } + } + + /* then provide the filename that is common to normal and verbose output */ + +#ifdef CONFIG_NSH_FULLPATH + nsh_output(vtbl, " %s/%s", arg, entryp->d_name); +#else + nsh_output(vtbl, " %s", entryp->d_name); +#endif + + if (DIRENT_ISDIRECTORY(entryp->d_type) && !ls_specialdir(entryp->d_name)) + { + nsh_output(vtbl, "/\n"); + } + else + { + nsh_output(vtbl, "\n"); + } + return OK; +} +#endif + +/**************************************************************************** + * Name: ls_recursive + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +static int ls_recursive(FAR struct nsh_vtbl_s *vtbl, const char *dirpath, + struct dirent *entryp, void *pvarg) +{ + int ret = OK; + + /* Is this entry a directory (and not one of the special directories, . and ..)? */ + + if (DIRENT_ISDIRECTORY(entryp->d_type) && !ls_specialdir(entryp->d_name)) + { + /* Yes.. */ + + char *newpath; + newpath = nsh_getdirpath(dirpath, entryp->d_name); + + /* List the directory contents */ + + nsh_output(vtbl, "%s:\n", newpath); + + /* Traverse the directory */ + + ret = foreach_direntry(vtbl, "ls", newpath, ls_handler, pvarg); + if (ret == 0) + { + /* Then recurse to list each directory within the directory */ + + ret = foreach_direntry(vtbl, "ls", newpath, ls_recursive, pvarg); + free(newpath); + } + } + return ret; +} +#endif + +/**************************************************************************** + * Name: cat_common + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_CAT +static int cat_common(FAR struct nsh_vtbl_s *vtbl, FAR const char *cmd, + FAR const char *filename) +{ + char buffer[IOBUFFERSIZE]; + int fd; + int ret = OK; + + /* Open the file for reading */ + + fd = open(filename, O_RDONLY); + if (fd < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, cmd, "open", NSH_ERRNO); + return ERROR; + } + + /* And just dump it byte for byte into stdout */ + + for (;;) + { + int nbytesread = read(fd, buffer, IOBUFFERSIZE); + + /* Check for read errors */ + + if (nbytesread < 0) + { + int errval = errno; + + /* EINTR is not an error (but will stop stop the cat) */ + +#ifndef CONFIG_DISABLE_SIGNALS + if (errval == EINTR) + { + nsh_output(vtbl, g_fmtsignalrecvd, cmd); + } + else +#endif + { + nsh_output(vtbl, g_fmtcmdfailed, cmd, "read", NSH_ERRNO_OF(errval)); + } + + ret = ERROR; + break; + } + + /* Check for data successfully read */ + + else if (nbytesread > 0) + { + int nbyteswritten = 0; + + while (nbyteswritten < nbytesread) + { + ssize_t n = nsh_write(vtbl, buffer, nbytesread); + if (n < 0) + { + int errval = errno; + + /* EINTR is not an error (but will stop stop the cat) */ + + #ifndef CONFIG_DISABLE_SIGNALS + if (errval == EINTR) + { + nsh_output(vtbl, g_fmtsignalrecvd, cmd); + } + else +#endif + { + nsh_output(vtbl, g_fmtcmdfailed, cmd, "write", NSH_ERRNO); + } + + ret = ERROR; + break; + } + else + { + nbyteswritten += n; + } + } + } + + /* Otherwise, it is the end of file */ + + else + { + break; + } + } + + (void)close(fd); + return ret; +} +#endif +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_cat + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_CAT +int cmd_cat(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *fullpath; + int i; + int ret = OK; + + /* Loop for each file name on the command line */ + + for (i = 1; i < argc && ret == OK; i++) + { + /* Get the fullpath to the file */ + + fullpath = nsh_getfullpath(vtbl, argv[i]); + if (!fullpath) + { + ret = ERROR; + } + else + { + /* Dump the file to the console */ + + ret = cat_common(vtbl, argv[0], fullpath); + + /* Free the allocated full path */ + + nsh_freefullpath(fullpath); + } + } + + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_dmesg + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_SYSLOG) && \ + defined(CONFIG_RAMLOG_SYSLOG) && !defined(CONFIG_NSH_DISABLE_DMESG) +int cmd_dmesg(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + return cat_common(vtbl, argv[0], CONFIG_SYSLOG_DEVPATH); +} +#endif + +/**************************************************************************** + * Name: cmd_cp + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_CP +int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct stat buf; + char *srcpath = NULL; + char *destpath = NULL; + char *allocpath = NULL; + int oflags = O_WRONLY|O_CREAT|O_TRUNC; + int rdfd; + int wrfd; + int ret = ERROR; + + /* Get the full path to the source file */ + + srcpath = nsh_getfullpath(vtbl, argv[1]); + if (!srcpath) + { + goto errout; + } + + /* Open the source file for reading */ + + rdfd = open(srcpath, O_RDONLY); + if (rdfd < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + goto errout_with_srcpath; + } + + /* Get the full path to the destination file or directory */ + + destpath = nsh_getfullpath(vtbl, argv[2]); + if (!destpath) + { + goto errout_with_rdfd; + } + + /* Check if the destination is a directory */ + + ret = stat(destpath, &buf); + if (ret == 0) + { + /* Something exists here... is it a directory? */ + + if (S_ISDIR(buf.st_mode)) + { + /* Yes, it is a directory. Remove any trailing '/' characters from the path */ + + trim_dir(argv[2]); + + /* Construct the full path to the new file */ + + allocpath = nsh_getdirpath(argv[2], basename(argv[1]) ); + if (!allocpath) + { + nsh_output(vtbl, g_fmtcmdoutofmemory, argv[0]); + goto errout_with_destpath; + } + + /* Open then dest for writing */ + + nsh_freefullpath(destpath); + destpath = allocpath; + } + else if (!S_ISREG(buf.st_mode)) + { + /* Maybe it is a driver? */ + + oflags = O_WRONLY; + } + } + + /* Now open the destination */ + + wrfd = open(destpath, oflags, 0666); + if (wrfd < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + goto errout_with_allocpath; + } + + /* Now copy the file */ + + for (;;) + { + int nbytesread; + int nbyteswritten; + + do + { + nbytesread = read(rdfd, g_iobuffer, IOBUFFERSIZE); + if (nbytesread == 0) + { + /* End of file */ + + ret = OK; + goto errout_with_wrfd; + } + else if (nbytesread < 0) + { + /* EINTR is not an error (but will still stop the copy) */ + +#ifndef CONFIG_DISABLE_SIGNALS + if (errno == EINTR) + { + nsh_output(vtbl, g_fmtsignalrecvd, argv[0]); + } + else +#endif + { + /* Read error */ + + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO); + } + goto errout_with_wrfd; + } + } + while (nbytesread <= 0); + + do + { + nbyteswritten = write(wrfd, g_iobuffer, nbytesread); + if (nbyteswritten >= 0) + { + nbytesread -= nbyteswritten; + } + else + { + /* EINTR is not an error (but will still stop the copy) */ + +#ifndef CONFIG_DISABLE_SIGNALS + if (errno == EINTR) + { + nsh_output(vtbl, g_fmtsignalrecvd, argv[0]); + } + else +#endif + { + /* Read error */ + + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "write", NSH_ERRNO); + } + goto errout_with_wrfd; + } + } + while (nbytesread > 0); + } + +errout_with_wrfd: + close(wrfd); + +errout_with_allocpath: + if (allocpath) + { + free(allocpath); + } + +errout_with_destpath: + if (destpath && !allocpath) + { + nsh_freefullpath(destpath); + } + +errout_with_rdfd: + close(rdfd); + +errout_with_srcpath: + if (srcpath) + { + nsh_freefullpath(srcpath); + } +errout: + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_losetup + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) +#ifndef CONFIG_NSH_DISABLE_LOSETUP +int cmd_losetup(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *loopdev = NULL; + char *filepath = NULL; + bool teardown = false; + bool readonly = false; + off_t offset = 0; + bool badarg = false; + int ret = ERROR; + int option; + + /* Get the losetup options: Two forms are supported: + * + * losetup -d <loop-device> + * losetup [-o <offset>] [-r] <loop-device> <filename> + * + * NOTE that the -o and -r options are accepted with the -d option, but + * will be ignored. + */ + + while ((option = getopt(argc, argv, "d:o:r")) != ERROR) + { + switch (option) + { + case 'd': + loopdev = nsh_getfullpath(vtbl, optarg); + teardown = true; + break; + + case 'o': + offset = atoi(optarg); + break; + + case 'r': + readonly = true; + break; + + case '?': + default: + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + badarg = true; + break; + } + } + + /* If a bad argument was encountered, then return without processing the command */ + + if (badarg) + { + goto errout_with_paths; + } + + /* If this is not a tear down operation, then additional command line + * parameters are required. + */ + + if (!teardown) + { + /* There must be two arguments on the command line after the options */ + + if (optind + 1 < argc) + { + loopdev = nsh_getfullpath(vtbl, argv[optind]); + optind++; + + filepath = nsh_getfullpath(vtbl, argv[optind]); + optind++; + } + else + { + nsh_output(vtbl, g_fmtargrequired, argv[0]); + goto errout_with_paths; + } + } + + /* There should be nothing else on the command line */ + + if (optind < argc) + { + nsh_output(vtbl, g_fmttoomanyargs, argv[0]); + goto errout_with_paths; + } + + /* Perform the teardown operation */ + + if (teardown) + { + /* Tear down the loop device. */ + + ret = loteardown(loopdev); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "loteardown", NSH_ERRNO_OF(-ret)); + goto errout_with_paths; + } + } + else + { + /* Set up the loop device */ + + ret = losetup(loopdev, filepath, 512, offset, readonly); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "losetup", NSH_ERRNO_OF(-ret)); + goto errout_with_paths; + } + } + + /* Free memory */ + +errout_with_paths: + if (loopdev) + { + free(loopdev); + } + + if (filepath) + { + free(filepath); + } + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_ls + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_LS +int cmd_ls(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + const char *relpath; + unsigned int lsflags = 0; + char *fullpath; + bool badarg = false; + int ret; + + /* Get the ls options */ + + int option; + while ((option = getopt(argc, argv, "lRs")) != ERROR) + { + switch (option) + { + case 'l': + lsflags |= (LSFLAGS_SIZE|LSFLAGS_LONG); + break; + + case 'R': + lsflags |= LSFLAGS_RECURSIVE; + break; + + case 's': + lsflags |= LSFLAGS_SIZE; + break; + + case '?': + default: + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + badarg = true; + break; + } + } + + /* If a bad argument was encountered, then return without processing the command */ + + if (badarg) + { + return ERROR; + } + + /* There may be one argument after the options */ + + if (optind + 1 < argc) + { + nsh_output(vtbl, g_fmttoomanyargs, argv[0]); + return ERROR; + } + else if (optind >= argc) + { +#ifndef CONFIG_DISABLE_ENVIRON + relpath = nsh_getcwd(); +#else + nsh_output(vtbl, g_fmtargrequired, argv[0]); + return ERROR; +#endif + } + else + { + relpath = argv[optind]; + } + + /* Get the fullpath to the directory */ + + fullpath = nsh_getfullpath(vtbl, relpath); + if (!fullpath) + { + return ERROR; + } + + /* List the directory contents */ + + nsh_output(vtbl, "%s:\n", fullpath); + ret = foreach_direntry(vtbl, "ls", fullpath, ls_handler, (void*)lsflags); + if (ret == OK && (lsflags & LSFLAGS_RECURSIVE) != 0) + { + /* Then recurse to list each directory within the directory */ + + ret = foreach_direntry(vtbl, "ls", fullpath, ls_recursive, (void*)lsflags); + } + nsh_freefullpath(fullpath); + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_mkdir + ****************************************************************************/ + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +#ifndef CONFIG_NSH_DISABLE_MKDIR +int cmd_mkdir(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *fullpath = nsh_getfullpath(vtbl, argv[1]); + int ret = ERROR; + + if (fullpath) + { + ret = mkdir(fullpath, 0777); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "mkdir", NSH_ERRNO); + } + nsh_freefullpath(fullpath); + } + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_mkfatfs + ****************************************************************************/ + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_FAT) +#ifndef CONFIG_NSH_DISABLE_MKFATFS +int cmd_mkfatfs(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct fat_format_s fmt = FAT_FORMAT_INITIALIZER; + char *fullpath = nsh_getfullpath(vtbl, argv[1]); + int ret = ERROR; + + if (fullpath) + { + ret = mkfatfs(fullpath, &fmt); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "mkfatfs", NSH_ERRNO); + } + nsh_freefullpath(fullpath); + } + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_mkfifo + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_MKFIFO +int cmd_mkfifo(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *fullpath = nsh_getfullpath(vtbl, argv[1]); + int ret = ERROR; + + if (fullpath) + { + ret = mkfifo(fullpath, 0777); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "mkfifo", NSH_ERRNO); + } + nsh_freefullpath(fullpath); + } + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_mkrd + ****************************************************************************/ + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +#ifndef CONFIG_NSH_DISABLE_MKRD +int cmd_mkrd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + const char *fmt; + uint8_t *buffer; + uint32_t nsectors; + bool badarg = false; + int sectsize = 512; + int minor = 0; + int ret; + + /* Get the mkrd options */ + + int option; + while ((option = getopt(argc, argv, ":m:s:")) != ERROR) + { + switch (option) + { + case 'm': + minor = atoi(optarg); + if (minor < 0 || minor > 255) + { + nsh_output(vtbl, g_fmtargrange, argv[0]); + badarg = true; + } + break; + + case 's': + sectsize = atoi(optarg); + if (minor < 0 || minor > 16384) + { + nsh_output(vtbl, g_fmtargrange, argv[0]); + badarg = true; + } + break; + + case ':': + nsh_output(vtbl, g_fmtargrequired, argv[0]); + badarg = true; + break; + + case '?': + default: + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + badarg = true; + break; + } + } + + /* If a bad argument was encountered, then return without processing the command */ + + if (badarg) + { + return ERROR; + } + + /* There should be exactly on parameter left on the command-line */ + + if (optind == argc-1) + { + nsectors = (uint32_t)atoi(argv[optind]); + } + else if (optind >= argc) + { + fmt = g_fmttoomanyargs; + goto errout_with_fmt; + } + else + { + fmt = g_fmtargrequired; + goto errout_with_fmt; + } + + /* Allocate the memory backing up the ramdisk */ + + buffer = (uint8_t*)malloc(sectsize * nsectors); + if (!buffer) + { + fmt = g_fmtcmdoutofmemory; + goto errout_with_fmt; + } + +#ifdef CONFIG_DEBUG_VERBOSE + memset(buffer, 0, sectsize * nsectors); +#endif + dbg("RAMDISK at %p\n", buffer); + + /* Then register the ramdisk */ + + ret = ramdisk_register(minor, buffer, nsectors, sectsize, true); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "ramdisk_register", NSH_ERRNO_OF(-ret)); + free(buffer); + return ERROR; + } + return ret; + +errout_with_fmt: + nsh_output(vtbl, fmt, argv[0]); + return ERROR; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_mv + ****************************************************************************/ + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +#ifndef CONFIG_NSH_DISABLE_MV +int cmd_mv(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *oldpath; + char *newpath; + int ret; + + /* Get the full path to the old and new file paths */ + + oldpath = nsh_getfullpath(vtbl, argv[1]); + if (!oldpath) + { + return ERROR; + } + + newpath = nsh_getfullpath(vtbl, argv[2]); + if (!newpath) + { + nsh_freefullpath(newpath); + return ERROR; + } + + /* Perform the mount */ + + ret = rename(oldpath, newpath); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "rename", NSH_ERRNO); + } + + /* Free the file paths */ + + nsh_freefullpath(oldpath); + nsh_freefullpath(newpath); + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_rm + ****************************************************************************/ + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +#ifndef CONFIG_NSH_DISABLE_RM +int cmd_rm(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *fullpath = nsh_getfullpath(vtbl, argv[1]); + int ret = ERROR; + + if (fullpath) + { + ret = unlink(fullpath); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "unlink", NSH_ERRNO); + } + nsh_freefullpath(fullpath); + } + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_rmdir + ****************************************************************************/ + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +#ifndef CONFIG_NSH_DISABLE_RMDIR +int cmd_rmdir(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *fullpath = nsh_getfullpath(vtbl, argv[1]); + int ret = ERROR; + + if (fullpath) + { + ret = rmdir(fullpath); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "rmdir", NSH_ERRNO); + } + nsh_freefullpath(fullpath); + } + return ret; +} +#endif +#endif + +/**************************************************************************** + * Name: nsh_script + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NFILE_STREAMS > 0 && !defined(CONFIG_NSH_DISABLESCRIPT) +int nsh_script(FAR struct nsh_vtbl_s *vtbl, const char *cmd, const char *path) +{ + char *fullpath; + FILE *stream; + char *buffer; + char *pret; + int ret = ERROR; + + /* The path to the script may be relative to the current working directory */ + + fullpath = nsh_getfullpath(vtbl, path); + if (!fullpath) + { + return ERROR; + } + + /* Get a reference to the common input buffer */ + + buffer = nsh_linebuffer(vtbl); + if (buffer) + { + /* Open the file containing the script */ + + stream = fopen(fullpath, "r"); + if (!stream) + { + nsh_output(vtbl, g_fmtcmdfailed, cmd, "fopen", NSH_ERRNO); + nsh_freefullpath(fullpath); + return ERROR; + } + + /* Loop, processing each command line in the script file (or + * until an error occurs) + */ + + do + { + /* Get the next line of input from the file */ + + fflush(stdout); + pret = fgets(buffer, CONFIG_NSH_LINELEN, stream); + if (pret) + { + /* Parse process the command. NOTE: this is recursive... + * we got to cmd_sh via a call to nsh_parse. So some + * considerable amount of stack may be used. + */ + + ret = nsh_parse(vtbl, buffer); + } + } + while (pret && ret == OK); + fclose(stream); + } + + nsh_freefullpath(fullpath); + return ret; +} +#endif + +/**************************************************************************** + * Name: cmd_sh + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NFILE_STREAMS > 0 && !defined(CONFIG_NSH_DISABLESCRIPT) +#ifndef CONFIG_NSH_DISABLE_SH +int cmd_sh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + return nsh_script(vtbl, argv[0], argv[1]); +} +#endif +#endif diff --git a/apps/nshlib/nsh_init.c b/apps/nshlib/nsh_init.c new file mode 100644 index 000000000..7c7e78ea1 --- /dev/null +++ b/apps/nshlib/nsh_init.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * apps/nshlib/nsh_init.c + * + * Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include "nsh.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_initialize + * + * Description: + * This nterfaces is used to initialize the NuttShell (NSH). + * nsh_initialize() should be called one during application start-up prior + * to executing either nsh_consolemain() or nsh_telnetstart(). + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ + +void nsh_initialize(void) +{ + /* Mount the /etc filesystem */ + + (void)nsh_romfsetc(); + + /* Perform architecture-specific initialization (if available) */ + + (void)nsh_archinitialize(); + + /* Bring up the network */ + + (void)nsh_netinit(); +} + diff --git a/apps/nshlib/nsh_mmcmds.c b/apps/nshlib/nsh_mmcmds.c new file mode 100644 index 000000000..545ae60ad --- /dev/null +++ b/apps/nshlib/nsh_mmcmds.c @@ -0,0 +1,96 @@ +/**************************************************************************** + * apps/nshlib/nsh_mmcmds.c + * + * 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 + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_free + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_FREE +int cmd_free(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct mallinfo mem; + +#ifdef CONFIG_CAN_PASS_STRUCTS + mem = mallinfo(); +#else + (void)mallinfo(&mem); +#endif + + nsh_output(vtbl, " total used free largest\n"); + nsh_output(vtbl, "Mem: %11d%11d%11d%11d\n", + mem.arena, mem.uordblks, mem.fordblks, mem.mxordblk); + + return OK; +} +#endif /* !CONFIG_NSH_DISABLE_FREE */ diff --git a/apps/nshlib/nsh_mntcmds.c b/apps/nshlib/nsh_mntcmds.c new file mode 100644 index 000000000..b5935bdea --- /dev/null +++ b/apps/nshlib/nsh_mntcmds.c @@ -0,0 +1,416 @@ +/**************************************************************************** + * apps/nshlib/nsh_mntcmds.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <sys/mount.h> +#include <sys/statfs.h> + +#include <stdint.h> +#include <stdbool.h> +#include <unistd.h> +#include <errno.h> +#include <debug.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: df_handler + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) && \ + defined(CONFIG_FS_READABLE) && !defined(CONFIG_NSH_DISABLE_MOUNT) +static int df_handler(FAR const char *mountpoint, + FAR struct statfs *statbuf, FAR void *arg) +{ + FAR struct nsh_vtbl_s *vtbl = (FAR struct nsh_vtbl_s *)arg; + + DEBUGASSERT(mountpoint && statbuf && vtbl); + + nsh_output(vtbl, "%6ld %8ld %8ld %8ld %s\n", + statbuf->f_bsize, statbuf->f_blocks, + statbuf->f_blocks - statbuf->f_bavail, statbuf->f_bavail, + mountpoint); + + return OK; +} +#endif + +/**************************************************************************** + * Name: mount_handler + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) && \ + defined(CONFIG_FS_READABLE) && !defined(CONFIG_NSH_DISABLE_MOUNT) +static int mount_handler(FAR const char *mountpoint, + FAR struct statfs *statbuf, FAR void *arg) +{ + FAR struct nsh_vtbl_s *vtbl = (FAR struct nsh_vtbl_s *)arg; + FAR const char *fstype; + + DEBUGASSERT(mountpoint && statbuf && vtbl); + + /* Get the file system type */ + + switch (statbuf->f_type) + { +#ifdef CONFIG_FS_FAT + case MSDOS_SUPER_MAGIC: + fstype = "vfat"; + break; +#endif + +#ifdef CONFIG_FS_ROMFS + case ROMFS_MAGIC: + fstype = "romfs"; + break; +#endif + +#ifdef CONFIG_APPS_BINDIR + case BINFS_MAGIC: + fstype = "bindir"; + break; +#endif + +#ifdef CONFIG_FS_NXFFS + case NXFFS_MAGIC: + fstype = "nxffs"; + break; +#endif + +#ifdef CONFIG_NFS + case NFS_SUPER_MAGIC: + fstype = "nfs"; + break; +#endif + + default: + fstype = "Unrecognized"; + break; + } + + nsh_output(vtbl, " %s type %s\n", mountpoint, fstype); + return OK; +} +#endif + +/**************************************************************************** + * Name: mount_show + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) && \ + defined(CONFIG_FS_READABLE) && !defined(CONFIG_NSH_DISABLE_MOUNT) +static inline int mount_show(FAR struct nsh_vtbl_s *vtbl, FAR const char *progname) +{ + return foreach_mountpoint(mount_handler, (FAR void *)vtbl); +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_df + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) && \ + defined(CONFIG_FS_READABLE) && !defined(CONFIG_NSH_DISABLE_DF) +int cmd_df(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + nsh_output(vtbl, " Block Number\n"); + nsh_output(vtbl, " Size Blocks Used Available Mounted on\n"); + + return foreach_mountpoint(df_handler, (FAR void *)vtbl); +} +#endif + +/**************************************************************************** + * Name: cmd_mount + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) && \ + defined(CONFIG_FS_READABLE) && !defined(CONFIG_NSH_DISABLE_MOUNT) +int cmd_mount(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *source; + char *target; + char *filesystem = 0; + bool badarg = false; + int ret; + + /* The mount command behaves differently if no parameters are provided */ + + if (argc < 2) + { + return mount_show(vtbl, argv[0]); + } + + /* Get the mount options */ + + int option; + while ((option = getopt(argc, argv, ":t:")) != ERROR) + { + switch (option) + { + case 't': + filesystem = optarg; + break; + + case ':': + nsh_output(vtbl, g_fmtargrequired, argv[0]); + badarg = true; + break; + + case '?': + default: + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + badarg = true; + break; + } + } + + /* If a bad argument was encountered, then return without processing the command */ + + if (badarg) + { + return ERROR; + } + + /* There are two required arguments after the options */ + + if (optind + 2 < argc) + { + nsh_output(vtbl, g_fmttoomanyargs, argv[0]); + return ERROR; + } + else if (optind + 2 > argc) + { + nsh_output(vtbl, g_fmtargrequired, argv[0]); + return ERROR; + } + + /* The source and target paths might be relative to the current + * working directory. + */ + + source = nsh_getfullpath(vtbl, argv[optind]); + if (!source) + { + return ERROR; + } + + target = nsh_getfullpath(vtbl, argv[optind+1]); + if (!target) + { + nsh_freefullpath(source); + return ERROR; + } + + /* Perform the mount */ + + ret = mount(source, target, filesystem, 0, NULL); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "mount", NSH_ERRNO); + } + + nsh_freefullpath(source); + nsh_freefullpath(target); + return ret; +} +#endif + +/**************************************************************************** + * Name: cmd_nfsmount + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) && \ + defined(CONFIG_NET) && defined(CONFIG_NFS) && !defined(CONFIG_NSH_DISABLE_NFSMOUNT) +int cmd_nfsmount(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct nfs_args data; + FAR char *address; + FAR char *lpath; + FAR char *rpath; + bool badarg = false; +#ifdef CONFIG_NET_IPv6 + FAR struct sockaddr_in6 *sin; + struct in6_addr inaddr; +#else + FAR struct sockaddr_in *sin; + struct in_addr inaddr; +#endif + int ret; + + /* If a bad argument was encountered, then return without processing the + * command. + */ + + if (badarg) + { + return ERROR; + } + + /* The fist argument on the command line should be the NFS server IP address + * in standard IPv4 (or IPv6) dot format. + */ + + address = argv[1]; + if (!address) + { + return ERROR; + } + + /* The local mount point path (lpath) might be relative to the current working + * directory. + */ + + lpath = nsh_getfullpath(vtbl, argv[2]); + if (!lpath) + { + return ERROR; + } + + /* Get the remote mount point path */ + + rpath = argv[3]; + + /* Convert the IP address string into its binary form */ + +#ifdef CONFIG_NET_IPv6 + ret = inet_pton(AF_INET6, address, &inaddr); +#else + ret = inet_pton(AF_INET, address, &inaddr); +#endif + if (ret != 1) + { + nsh_freefullpath(lpath); + return ERROR; + } + + /* Place all of the NFS arguements into the nfs_args structure */ + + memset(&data, 0, sizeof(data)); + +#ifdef CONFIG_NET_IPv6 + sin = (FAR struct sockaddr_in6 *)&data.addr; + sin->sin_family = AF_INET6; + sin->sin_port = htons(NFS_PMAPPORT); + memcpy(&sin->sin6_addr, &inaddr, sizeof(struct in6_addr)); + data.addrlen = sizeof(struct sockaddr_in6); +#else + sin = (FAR struct sockaddr_in *)&data.addr; + sin->sin_family = AF_INET; + sin->sin_port = htons(NFS_PMAPPORT); + sin->sin_addr = inaddr; + data.addrlen = sizeof(struct sockaddr_in); +#endif + + data.sotype = SOCK_DGRAM; + data.path = rpath; + data.flags = 0; /* 0=Use all defaults */ + + /* Perform the mount */ + + ret = mount(NULL, lpath, "nfs", 0, (FAR void *)&data); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "mount", NSH_ERRNO); + } + + /* We no longer need the allocated mount point path */ + + nsh_freefullpath(lpath); + return ret; +} +#endif + +/**************************************************************************** + * Name: cmd_umount + ****************************************************************************/ + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) && \ + defined(CONFIG_FS_READABLE) && !defined(CONFIG_NSH_DISABLE_UMOUNT) +int cmd_umount(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *fullpath = nsh_getfullpath(vtbl, argv[1]); + int ret = ERROR; + + if (fullpath) + { + /* Perform the umount */ + + ret = umount(fullpath); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "umount", NSH_ERRNO); + } + nsh_freefullpath(fullpath); + } + + return ret; +} +#endif diff --git a/apps/nshlib/nsh_netcmds.c b/apps/nshlib/nsh_netcmds.c new file mode 100644 index 000000000..cfea5a08a --- /dev/null +++ b/apps/nshlib/nsh_netcmds.c @@ -0,0 +1,864 @@ +/**************************************************************************** + * apps/nshlib/nsh_netcmds.c + * + * Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#ifdef CONFIG_NET + +#include <sys/stat.h> /* Needed for open */ +#include <stdint.h> +#include <stdbool.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <sched.h> +#include <fcntl.h> /* Needed for open */ +#include <libgen.h> /* Needed for basename */ +#include <errno.h> + +#include <nuttx/net/net.h> +#include <nuttx/clock.h> +#include <net/ethernet.h> +#include <nuttx/net/uip/uip.h> +#include <nuttx/net/uip/uip-arch.h> +#include <netinet/ether.h> + +#ifdef CONFIG_NET_STATISTICS +# include <nuttx/net/uip/uip.h> +#endif + +#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING) && \ + !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_DISABLE_SIGNALS) +# include <apps/netutils/uiplib.h> +#endif + +#if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +# include <apps/netutils/uiplib.h> +# include <apps/netutils/tftp.h> +#endif + +#if defined(CONFIG_NET_TCP) && CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_WGET +# include <apps/netutils/uiplib.h> +# include <apps/netutils/webclient.h> +# endif +#endif + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define DEFAULT_PING_DATALEN 56 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +#if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +struct tftpc_args_s +{ + bool binary; /* true:binary ("octect") false:text ("netascii") */ + bool allocated; /* true: destpath is allocated */ + char *destpath; /* Path at destination */ + const char *srcpath; /* Path at src */ + in_addr_t ipaddr; /* Host IP address */ +}; +#endif + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING) && \ + !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_DISABLE_SIGNALS) +static uint16_t g_pingid = 0; +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: ping_newid + ****************************************************************************/ + +#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING) && \ + !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_DISABLE_SIGNALS) +static inline uint16_t ping_newid(void) +{ + irqstate_t save = irqsave(); + uint16_t ret = ++g_pingid; + irqrestore(save); + return ret; +} +#endif + +/**************************************************************************** + * Name: uip_statistics + ****************************************************************************/ + +#if defined(CONFIG_NET_STATISTICS) && !defined(CONFIG_NSH_DISABLE_IFCONFIG) +static inline void uip_statistics(FAR struct nsh_vtbl_s *vtbl) +{ + nsh_output(vtbl, "uIP IP "); +#ifdef CONFIG_NET_TCP + nsh_output(vtbl, " TCP"); +#endif +#ifdef CONFIG_NET_UDP + nsh_output(vtbl, " UDP"); +#endif +#ifdef CONFIG_NET_ICMP + nsh_output(vtbl, " ICMP"); +#endif + nsh_output(vtbl, "\n"); + + /* Received packets */ + + nsh_output(vtbl, "Received %04x",uip_stat.ip.recv); +#ifdef CONFIG_NET_TCP + nsh_output(vtbl, " %04x",uip_stat.tcp.recv); +#endif +#ifdef CONFIG_NET_UDP + nsh_output(vtbl, " %04x",uip_stat.udp.recv); +#endif +#ifdef CONFIG_NET_ICMP + nsh_output(vtbl, " %04x",uip_stat.icmp.recv); +#endif + nsh_output(vtbl, "\n"); + + /* Dropped packets */ + + nsh_output(vtbl, "Dropped %04x",uip_stat.ip.drop); +#ifdef CONFIG_NET_TCP + nsh_output(vtbl, " %04x",uip_stat.tcp.drop); +#endif +#ifdef CONFIG_NET_UDP + nsh_output(vtbl, " %04x",uip_stat.udp.drop); +#endif +#ifdef CONFIG_NET_ICMP + nsh_output(vtbl, " %04x",uip_stat.icmp.drop); +#endif + nsh_output(vtbl, "\n"); + + nsh_output(vtbl, " IP VHL: %04x HBL: %04x\n", + uip_stat.ip.vhlerr, uip_stat.ip.hblenerr); + nsh_output(vtbl, " LBL: %04x Frg: %04x\n", + uip_stat.ip.lblenerr, uip_stat.ip.fragerr); + + nsh_output(vtbl, " Checksum %04x",uip_stat.ip.chkerr); +#ifdef CONFIG_NET_TCP + nsh_output(vtbl, " %04x",uip_stat.tcp.chkerr); +#endif +#ifdef CONFIG_NET_UDP + nsh_output(vtbl, " %04x",uip_stat.udp.chkerr); +#endif +#ifdef CONFIG_NET_ICMP + nsh_output(vtbl, " ----"); +#endif + nsh_output(vtbl, "\n"); + +#ifdef CONFIG_NET_TCP + nsh_output(vtbl, " TCP ACK: %04x SYN: %04x\n", + uip_stat.tcp.ackerr, uip_stat.tcp.syndrop); + nsh_output(vtbl, " RST: %04x %04x\n", + uip_stat.tcp.rst, uip_stat.tcp.synrst); +#endif + + nsh_output(vtbl, " Type %04x",uip_stat.ip.protoerr); +#ifdef CONFIG_NET_TCP + nsh_output(vtbl, " ----"); +#endif +#ifdef CONFIG_NET_UDP + nsh_output(vtbl, " ----"); +#endif +#ifdef CONFIG_NET_ICMP + nsh_output(vtbl, " %04x",uip_stat.icmp.typeerr); +#endif + nsh_output(vtbl, "\n"); + + /* Sent packets */ + + nsh_output(vtbl, "Sent ----",uip_stat.ip.sent); +#ifdef CONFIG_NET_TCP + nsh_output(vtbl, " %04x",uip_stat.tcp.sent); +#endif +#ifdef CONFIG_NET_UDP + nsh_output(vtbl, " %04x",uip_stat.udp.sent); +#endif +#ifdef CONFIG_NET_ICMP + nsh_output(vtbl, " %04x",uip_stat.icmp.sent); +#endif + nsh_output(vtbl, "\n"); + +#ifdef CONFIG_NET_TCP + nsh_output(vtbl, " Rexmit ---- %04x",uip_stat.tcp.rexmit); +#ifdef CONFIG_NET_UDP + nsh_output(vtbl, " ----"); +#endif +#ifdef CONFIG_NET_ICMP + nsh_output(vtbl, " ----"); +#endif + nsh_output(vtbl, "\n"); +#endif + nsh_output(vtbl, "\n"); +} +#else +# define uip_statistics(vtbl) +#endif + + +/**************************************************************************** + * Name: ifconfig_callback + ****************************************************************************/ + +int ifconfig_callback(FAR struct uip_driver_s *dev, void *arg) +{ + struct nsh_vtbl_s *vtbl = (struct nsh_vtbl_s*)arg; + struct in_addr addr; + + nsh_output(vtbl, "%s\tHWaddr %s\n", dev->d_ifname, ether_ntoa(&dev->d_mac)); + addr.s_addr = dev->d_ipaddr; + nsh_output(vtbl, "\tIPaddr:%s ", inet_ntoa(addr)); + addr.s_addr = dev->d_draddr; + nsh_output(vtbl, "DRaddr:%s ", inet_ntoa(addr)); + addr.s_addr = dev->d_netmask; + nsh_output(vtbl, "Mask:%s\n\n", inet_ntoa(addr)); + return OK; +} + +/**************************************************************************** + * Name: tftpc_parseargs + ****************************************************************************/ + +#if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +int tftpc_parseargs(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv, + struct tftpc_args_s *args) +{ + FAR const char *fmt = g_fmtarginvalid; + bool badarg = false; + int option; + + /* Get the ping options */ + + memset(args, 0, sizeof(struct tftpc_args_s)); + while ((option = getopt(argc, argv, ":bnf:h:")) != ERROR) + { + switch (option) + { + case 'b': + args->binary = true; + break; + + case 'n': + args->binary = false; + break; + + case 'f': + args->destpath = optarg; + break; + + case 'h': + if (!uiplib_ipaddrconv(optarg, (FAR unsigned char*)&args->ipaddr)) + { + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + badarg = true; + } + break; + + case ':': + nsh_output(vtbl, g_fmtargrequired, argv[0]); + badarg = true; + break; + + case '?': + default: + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + badarg = true; + break; + } + } + + /* If a bad argument was encountered, then return without processing the command */ + + if (badarg) + { + return ERROR; + } + + /* There should be exactly one parameter left on the command-line */ + + if (optind == argc-1) + { + args->srcpath = argv[optind]; + } + + /* optind == argc means that there is nothing left on the command-line */ + + else if (optind >= argc) + { + fmt = g_fmtargrequired; + goto errout; + } + + /* optind < argc-1 means that there are too many arguments on the + * command-line + */ + + else + { + fmt = g_fmttoomanyargs; + goto errout; + } + + /* The HOST IP address is also required */ + + if (!args->ipaddr) + { + fmt = g_fmtargrequired; + goto errout; + } + + /* If the destpath was not provided, then we have do a little work. */ + + if (!args->destpath) + { + char *tmp1; + char *tmp2; + + /* Copy the srcpath... baseanme might modify it */ + + fmt = g_fmtcmdoutofmemory; + tmp1 = strdup(args->srcpath); + if (!tmp1) + { + goto errout; + } + + /* Get the basename of the srcpath */ + + tmp2 = basename(tmp1); + if (!tmp2) + { + free(tmp1); + goto errout; + } + + /* Use that basename as the destpath */ + + args->destpath = strdup(tmp2); + free(tmp1); + if (!args->destpath) + { + goto errout; + } + args->allocated = true; + } + + return OK; + +errout: + nsh_output(vtbl, fmt, argv[0]); + return ERROR; +} +#endif + +/**************************************************************************** + * Name: wget_callback + ****************************************************************************/ + +#if defined(CONFIG_NET_TCP) && CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_WGET +static void wget_callback(FAR char **buffer, int offset, int datend, + FAR int *buflen, FAR void *arg) +{ + (void)write((int)arg, &((*buffer)[offset]), datend - offset); +} +#endif +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_get + ****************************************************************************/ + +#if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_GET +int cmd_get(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct tftpc_args_s args; + char *fullpath; + + /* Parse the input parameter list */ + + if (tftpc_parseargs(vtbl, argc, argv, &args) != OK) + { + return ERROR; + } + + /* Get the full path to the local file */ + + fullpath = nsh_getfullpath(vtbl, args.srcpath); + + /* Then perform the TFTP get operation */ + + if (tftpget(args.srcpath, fullpath, args.ipaddr, args.binary) != OK) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "tftpget", NSH_ERRNO); + } + + /* Release any allocated memory */ + + if (args.allocated) + { + free(args.destpath); + } + free(fullpath); + return OK; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_ifconfig + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_IFCONFIG +int cmd_ifconfig(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct in_addr addr; + in_addr_t ip; + + /* With one or no arguments, ifconfig simply shows the status of ethernet + * device: + * + * ifconfig + * ifconfig [nic_name] + */ + + if (argc <= 2) + { + netdev_foreach(ifconfig_callback, vtbl); + uip_statistics(vtbl); + return OK; + } + + /* If both the network interface name and an IP address are supplied as + * arguments, then ifconfig will set the address of the ethernet device: + * + * ifconfig nic_name ip_address + */ + + /* Set host ip address */ + + ip = addr.s_addr = inet_addr(argv[2]); + uip_sethostaddr(argv[1], &addr); + + /* Set gateway */ + + ip = NTOHL(ip); + ip &= ~0x000000ff; + ip |= 0x00000001; + + addr.s_addr = HTONL(ip); + uip_setdraddr(argv[1], &addr); + + /* Set netmask */ + + addr.s_addr = inet_addr("255.255.255.0"); + uip_setnetmask(argv[1], &addr); + + return OK; +} +#endif + +/**************************************************************************** + * Name: cmd_ping + ****************************************************************************/ + +#if defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING) && \ + !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_DISABLE_SIGNALS) +#ifndef CONFIG_NSH_DISABLE_PING +int cmd_ping(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + FAR const char *fmt = g_fmtarginvalid; + const char *staddr; + uip_ipaddr_t ipaddr; + uint32_t start; + uint32_t next; + uint32_t dsec = 10; + uint16_t id; + bool badarg = false; + int count = 10; + int option; + int seqno; + int replies = 0; + int elapsed; + int tmp; + int i; + + /* Get the ping options */ + + while ((option = getopt(argc, argv, ":c:i:")) != ERROR) + { + switch (option) + { + case 'c': + count = atoi(optarg); + if (count < 1 || count > 10000) + { + nsh_output(vtbl, g_fmtargrange, argv[0]); + badarg = true; + } + break; + + case 'i': + tmp = atoi(optarg); + if (tmp < 1 || tmp >= 4294) + { + nsh_output(vtbl, g_fmtargrange, argv[0]); + badarg = true; + } + else + { + dsec = 10 * tmp; + } + break; + + case ':': + nsh_output(vtbl, g_fmtargrequired, argv[0]); + badarg = true; + break; + + case '?': + default: + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + badarg = true; + break; + } + } + + /* If a bad argument was encountered, then return without processing the command */ + + if (badarg) + { + return ERROR; + } + + /* There should be exactly on parameter left on the command-line */ + + if (optind == argc-1) + { + staddr = argv[optind]; + if (!uiplib_ipaddrconv(staddr, (FAR unsigned char*)&ipaddr)) + { + goto errout; + } + } + else if (optind >= argc) + { + fmt = g_fmttoomanyargs; + goto errout; + } + else + { + fmt = g_fmtargrequired; + goto errout; + } + + /* Get the ID to use */ + + id = ping_newid(); + + /* Loop for the specified count */ + + nsh_output(vtbl, "PING %s %d bytes of data\n", staddr, DEFAULT_PING_DATALEN); + start = g_system_timer; + for (i = 1; i <= count; i++) + { + /* Send the ECHO request and wait for the response */ + + next = g_system_timer; + seqno = uip_ping(ipaddr, id, i, DEFAULT_PING_DATALEN, dsec); + + /* Was any response returned? We can tell if a non-negative sequence + * number was returned. + */ + + if (seqno >= 0 && seqno <= i) + { + /* Get the elpased time from the time that the request was + * sent until the response was received. If we got a response + * to an earlier request, then fudge the elpased time. + */ + + elapsed = TICK2MSEC(g_system_timer - next); + if (seqno < i) + { + elapsed += 100*dsec*(i - seqno); + } + + /* Report the receipt of the reply */ + + nsh_output(vtbl, "%d bytes from %s: icmp_seq=%d time=%d ms\n", + DEFAULT_PING_DATALEN, staddr, seqno, elapsed); + replies++; + } + + /* Wait for the remainder of the interval. If the last seqno<i, + * then this is a bad idea... we will probably lose the response + * to the current request! + */ + + elapsed = TICK2DSEC(g_system_timer - next); + if (elapsed < dsec) + { + usleep(100000*dsec); + } + } + + /* Get the total elapsed time */ + + elapsed = TICK2MSEC(g_system_timer - start); + + /* Calculate the percentage of lost packets */ + + tmp = (100*(count - replies) + (count >> 1)) / count; + + nsh_output(vtbl, "%d packets transmitted, %d received, %d%% packet loss, time %d ms\n", + count, replies, tmp, elapsed); + return OK; + +errout: + nsh_output(vtbl, fmt, argv[0]); + return ERROR; +} +#endif +#endif /* CONFIG_NET_ICMP && CONFIG_NET_ICMP_PING */ + +/**************************************************************************** + * Name: cmd_put + ****************************************************************************/ + +#if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_PUT +int cmd_put(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + struct tftpc_args_s args; + char *fullpath; + + /* Parse the input parameter list */ + + if (tftpc_parseargs(vtbl, argc, argv, &args) != OK) + { + return ERROR; + } + + /* Get the full path to the local file */ + + fullpath = nsh_getfullpath(vtbl, args.srcpath); + + /* Then perform the TFTP put operation */ + + if (tftpput(fullpath, args.destpath, args.ipaddr, args.binary) != OK) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "tftpput", NSH_ERRNO); + } + + /* Release any allocated memory */ + + if (args.allocated) + { + free(args.destpath); + } + free(fullpath); + return OK; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_wget + ****************************************************************************/ + +#if defined(CONFIG_NET_TCP) && CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_WGET +int cmd_wget(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *localfile = NULL; + char *allocfile = NULL; + char *buffer = NULL; + char *fullpath = NULL; + char *url; + const char *fmt; + bool badarg = false; + int option; + int fd = -1; + int ret; + + /* Get the wget options */ + + while ((option = getopt(argc, argv, ":o:")) != ERROR) + { + switch (option) + { + case 'o': + localfile = optarg; + break; + + case ':': + nsh_output(vtbl, g_fmtargrequired, argv[0]); + badarg = true; + break; + + case '?': + default: + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + badarg = true; + break; + } + } + + /* If a bad argument was encountered, then return without processing the command */ + + if (badarg) + { + return ERROR; + } + + /* There should be exactly on parameter left on the command-line */ + + if (optind == argc-1) + { + url = argv[optind]; + } + else if (optind >= argc) + { + fmt = g_fmttoomanyargs; + goto errout; + } + else + { + fmt = g_fmtargrequired; + goto errout; + } + + /* Get the local file name */ + + if (!localfile) + { + allocfile = strdup(url); + localfile = basename(allocfile); + } + + /* Get the full path to the local file */ + + fullpath = nsh_getfullpath(vtbl, localfile); + + /* Open the local file for writing */ + + fd = open(fullpath, O_WRONLY|O_CREAT|O_TRUNC, 0644); + if (fd < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + ret = ERROR; + goto exit; + } + + /* Allocate an I/O buffer */ + + buffer = malloc(512); + if (!buffer) + { + fmt = g_fmtcmdoutofmemory; + goto errout; + } + + /* And perform the wget */ + + ret = wget(url, buffer, 512, wget_callback, (FAR void *)fd); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "wget", NSH_ERRNO); + goto exit; + } + + /* Free allocated resources */ + +exit: + if (fd >= 0) + { + close(fd); + } + if (allocfile) + { + free(allocfile); + } + if (fullpath) + { + free(fullpath); + } + if (buffer) + { + free(buffer); + } + return ret; + +errout: + nsh_output(vtbl, fmt, argv[0]); + ret = ERROR; + goto exit; +} +#endif +#endif + +#endif /* CONFIG_NET */ diff --git a/apps/nshlib/nsh_netinit.c b/apps/nshlib/nsh_netinit.c new file mode 100644 index 000000000..5d74b30d7 --- /dev/null +++ b/apps/nshlib/nsh_netinit.c @@ -0,0 +1,170 @@ +/**************************************************************************** + * apps/nshlib/nsh_netinit.c + * + * Copyright (C) 2010-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * This is influenced by similar logic from uIP: + * + * Author: Adam Dunkels <adam@sics.se> + * Copyright (c) 2003, Adam Dunkels. + * 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 of the Institute 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 INSTITUTE 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 INSTITUTE 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. + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <debug.h> + +#include <net/if.h> + +#include <apps/netutils/uiplib.h> +#if defined(CONFIG_NSH_DHCPC) +# include <apps/netutils/resolv.h> +# include <apps/netutils/dhcpc.h> +#endif + +#include "nsh.h" + +#ifdef CONFIG_NET + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_netinit + * + * Description: + * Initialize the network per the selected NuttX configuration + * + ****************************************************************************/ + +int nsh_netinit(void) +{ + struct in_addr addr; +#if defined(CONFIG_NSH_DHCPC) + FAR void *handle; +#endif +#if defined(CONFIG_NSH_DHCPC) || defined(CONFIG_NSH_NOMAC) + uint8_t mac[IFHWADDRLEN]; +#endif + +/* Many embedded network interfaces must have a software assigned MAC */ + +#ifdef CONFIG_NSH_NOMAC + mac[0] = 0x00; + mac[1] = 0xe0; + mac[2] = 0xde; + mac[3] = 0xad; + mac[4] = 0xbe; + mac[5] = 0xef; + uip_setmacaddr("eth0", mac); +#endif + + /* Set up our host address */ + +#if !defined(CONFIG_NSH_DHCPC) + addr.s_addr = HTONL(CONFIG_NSH_IPADDR); +#else + addr.s_addr = 0; +#endif + uip_sethostaddr("eth0", &addr); + + /* Set up the default router address */ + + addr.s_addr = HTONL(CONFIG_NSH_DRIPADDR); + uip_setdraddr("eth0", &addr); + + /* Setup the subnet mask */ + + addr.s_addr = HTONL(CONFIG_NSH_NETMASK); + uip_setnetmask("eth0", &addr); + +#if defined(CONFIG_NSH_DHCPC) + /* Set up the resolver */ + + resolv_init(); +#endif + +#if defined(CONFIG_NSH_DHCPC) + /* Get the MAC address of the NIC */ + + uip_getmacaddr("eth0", mac); + + /* Set up the DHCPC modules */ + + handle = dhcpc_open(&mac, IFHWADDRLEN); + + /* Get an IP address. Note that there is no logic for renewing the IP address in this + * example. The address should be renewed in ds.lease_time/2 seconds. + */ + + if (handle) + { + struct dhcpc_state ds; + (void)dhcpc_request(handle, &ds); + uip_sethostaddr("eth1", &ds.ipaddr); + if (ds.netmask.s_addr != 0) + { + uip_setnetmask("eth0", &ds.netmask); + } + if (ds.default_router.s_addr != 0) + { + uip_setdraddr("eth0", &ds.default_router); + } + if (ds.dnsaddr.s_addr != 0) + { + resolv_conf(&ds.dnsaddr); + } + dhcpc_close(handle); + } +#endif + return OK; +} + +#endif /* CONFIG_NET */ diff --git a/apps/nshlib/nsh_parse.c b/apps/nshlib/nsh_parse.c new file mode 100644 index 000000000..00319a98b --- /dev/null +++ b/apps/nshlib/nsh_parse.c @@ -0,0 +1,1367 @@ +/**************************************************************************** + * apps/nshlib/nsh_parse.c + * + * Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/stat.h> + +#include <stdint.h> +#include <stdbool.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <sched.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/version.h> + +#ifndef CONFIG_NSH_DISABLEBG +# include <pthread.h> +#endif + +#ifdef CONFIG_NSH_BUILTIN_APPS +# include <apps/apps.h> +#endif +#include <apps/nsh.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Argument list size + * + * argv[0]: The command name. + * argv[1]: The beginning of argument (up to NSH_MAX_ARGUMENTS) + * argv[argc-3]: Possibly '>' or '>>' + * argv[argc-2]: Possibly <file> + * argv[argc-1]: Possibly '&' (if pthreads are enabled) + * argv[argc]: NULL terminating pointer + * + * Maximum size is NSH_MAX_ARGUMENTS+5 + */ + +#ifndef CONFIG_NSH_DISABLEBG +# define MAX_ARGV_ENTRIES (NSH_MAX_ARGUMENTS+5) +#else +# define MAX_ARGV_ENTRIES (NSH_MAX_ARGUMENTS+4) +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +struct cmdmap_s +{ + const char *cmd; /* Name of the command */ + cmd_t handler; /* Function that handles the command */ + uint8_t minargs; /* Minimum number of arguments (including command) */ + uint8_t maxargs; /* Maximum number of arguments (including command) */ + const char *usage; /* Usage instructions for 'help' command */ +}; + +#ifndef CONFIG_NSH_DISABLEBG +struct cmdarg_s +{ + FAR struct nsh_vtbl_s *vtbl; /* For front-end interaction */ + int fd; /* FD for output redirection */ + int argc; /* Number of arguments in argv */ + FAR char *argv[MAX_ARGV_ENTRIES]; /* Argument list */ +}; +#endif + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_HELP + static int cmd_help(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif + +#ifndef CONFIG_NSH_DISABLE_EXIT + static int cmd_exit(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +#endif +static int cmd_unrecognized(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const char g_delim[] = " \t\n"; +static const char g_redirect1[] = ">"; +static const char g_redirect2[] = ">>"; +static const char g_exitstatus[] = "$?"; +static const char g_success[] = "0"; +static const char g_failure[] = "1"; + +static const struct cmdmap_s g_cmdmap[] = +{ +#if !defined(CONFIG_NSH_DISABLESCRIPT) && !defined(CONFIG_NSH_DISABLE_TEST) + { "[", cmd_lbracket, 4, NSH_MAX_ARGUMENTS, "<expression> ]" }, +#endif + +#ifndef CONFIG_NSH_DISABLE_HELP + { "?", cmd_help, 1, 1, NULL }, +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_CAT + { "cat", cmd_cat, 2, NSH_MAX_ARGUMENTS, "<path> [<path> [<path> ...]]" }, +# endif +#ifndef CONFIG_DISABLE_ENVIRON +# ifndef CONFIG_NSH_DISABLE_CD + { "cd", cmd_cd, 1, 2, "[<dir-path>|-|~|..]" }, +# endif +#endif +# ifndef CONFIG_NSH_DISABLE_CP + { "cp", cmd_cp, 3, 3, "<source-path> <dest-path>" }, +# endif +#endif + +#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) + { "date", cmd_date, 1, 3, "[-s \"MMM DD HH:MM:SS YYYY\"]" }, +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_NSH_DISABLE_DD) + { "dd", cmd_dd, 3, 6, "if=<infile> of=<outfile> [bs=<sectsize>] [count=<sectors>] [skip=<sectors>]" }, +# endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) && \ + defined(CONFIG_FS_READABLE) && !defined(CONFIG_NSH_DISABLE_DF) + { "df", cmd_df, 1, 1, NULL }, +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_SYSLOG) && \ + defined(CONFIG_RAMLOG_SYSLOG) && !defined(CONFIG_NSH_DISABLE_DMESG) + { "dmesg", cmd_dmesg, 1, 1, NULL }, +#endif + +#ifndef CONFIG_NSH_DISABLE_ECHO +# ifndef CONFIG_DISABLE_ENVIRON + { "echo", cmd_echo, 0, NSH_MAX_ARGUMENTS, "[<string|$name> [<string|$name>...]]" }, +# else + { "echo", cmd_echo, 0, NSH_MAX_ARGUMENTS, "[<string> [<string>...]]" }, +# endif +#endif + +#ifndef CONFIG_NSH_DISABLE_EXEC + { "exec", cmd_exec, 2, 3, "<hex-address>" }, +#endif +#ifndef CONFIG_NSH_DISABLE_EXIT + { "exit", cmd_exit, 1, 1, NULL }, +#endif + +#ifndef CONFIG_NSH_DISABLE_FREE + { "free", cmd_free, 1, 1, NULL }, +#endif + +#if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_GET + { "get", cmd_get, 4, 7, "[-b|-n] [-f <local-path>] -h <ip-address> <remote-path>" }, +# endif +#endif + +#ifndef CONFIG_NSH_DISABLE_HELP + { "help", cmd_help, 1, 1, NULL }, +#endif + +#ifdef CONFIG_NET +# ifndef CONFIG_NSH_DISABLE_IFCONFIG + { "ifconfig", cmd_ifconfig, 1, 3, "[nic_name [ip]]" }, +# endif +#endif + +#ifndef CONFIG_DISABLE_SIGNALS +# ifndef CONFIG_NSH_DISABLE_KILL + { "kill", cmd_kill, 3, 3, "-<signal> <pid>" }, +# endif +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_MOUNTPOINT) +# ifndef CONFIG_NSH_DISABLE_LOSETUP + { "losetup", cmd_losetup, 3, 6, "[-d <dev-path>] | [[-o <offset>] [-r] <dev-path> <file-path>]" }, +# endif +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_LS + { "ls", cmd_ls, 1, 5, "[-lRs] <dir-path>" }, +# endif +#endif + +#ifndef CONFIG_NSH_DISABLE_MB + { "mb", cmd_mb, 2, 3, "<hex-address>[=<hex-value>][ <hex-byte-count>]" }, +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +# ifndef CONFIG_NSH_DISABLE_MKDIR + { "mkdir", cmd_mkdir, 2, 2, "<path>" }, +# endif +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_FAT) +# ifndef CONFIG_NSH_DISABLE_MKFATFS + { "mkfatfs", cmd_mkfatfs, 2, 2, "<path>" }, +# endif +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_MKFIFO + { "mkfifo", cmd_mkfifo, 2, 2, "<path>" }, +# endif +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +# ifndef CONFIG_NSH_DISABLE_MKRD + { "mkrd", cmd_mkrd, 2, 6, "[-m <minor>] [-s <sector-size>] <nsectors>" }, +# endif +#endif + +#ifndef CONFIG_NSH_DISABLE_MH + { "mh", cmd_mh, 2, 3, "<hex-address>[=<hex-value>][ <hex-byte-count>]" }, +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_READABLE) +# ifndef CONFIG_NSH_DISABLE_MOUNT + { "mount", cmd_mount, 1, 5, "[-t <fstype> <block-device> <mount-point>]" }, +# endif +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +# ifndef CONFIG_NSH_DISABLE_MV + { "mv", cmd_mv, 3, 3, "<old-path> <new-path>" }, +# endif +#endif + +#ifndef CONFIG_NSH_DISABLE_MW + { "mw", cmd_mw, 2, 3, "<hex-address>[=<hex-value>][ <hex-byte-count>]" }, +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && \ + defined(CONFIG_NET) && defined(CONFIG_NFS) +# ifndef CONFIG_NSH_DISABLE_NFSMOUNT + { "nfsmount", cmd_nfsmount, 4, 4, "<server-address> <mount-point> <remote-path>" }, +# endif +#endif + +#if defined(CONFIG_NET) && defined(CONFIG_NET_ICMP) && defined(CONFIG_NET_ICMP_PING) && \ + !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_DISABLE_SIGNALS) +# ifndef CONFIG_NSH_DISABLE_PING + { "ping", cmd_ping, 2, 6, "[-c <count>] [-i <interval>] <ip-address>" }, +# endif +#endif + +#ifndef CONFIG_NSH_DISABLE_PS + { "ps", cmd_ps, 1, 1, NULL }, +#endif + +#if defined(CONFIG_NET_UDP) && CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_PUT + { "put", cmd_put, 4, 7, "[-b|-n] [-f <remote-path>] -h <ip-address> <local-path>" }, +# endif +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 && !defined(CONFIG_DISABLE_ENVIRON) +# ifndef CONFIG_NSH_DISABLE_PWD + { "pwd", cmd_pwd, 1, 1, NULL }, +# endif +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_WRITABLE) +# ifndef CONFIG_NSH_DISABLE_RM + { "rm", cmd_rm, 2, 2, "<file-path>" }, +# endif +# ifndef CONFIG_NSH_DISABLE_RMDIR + { "rmdir", cmd_rmdir, 2, 2, "<dir-path>" }, +# endif +#endif + +#ifndef CONFIG_DISABLE_ENVIRON +# ifndef CONFIG_NSH_DISABLE_SET + { "set", cmd_set, 3, 3, "<name> <value>" }, +# endif +#endif + +#if CONFIG_NFILE_DESCRIPTORS > 0 && CONFIG_NFILE_STREAMS > 0 && !defined(CONFIG_NSH_DISABLESCRIPT) +# ifndef CONFIG_NSH_DISABLE_SH + { "sh", cmd_sh, 2, 2, "<script-path>" }, +# endif +#endif + +#ifndef CONFIG_DISABLE_SIGNALS +# ifndef CONFIG_NSH_DISABLE_SLEEP + { "sleep", cmd_sleep, 2, 2, "<sec>" }, +# endif +#endif + +#if !defined(CONFIG_NSH_DISABLESCRIPT) && !defined(CONFIG_NSH_DISABLE_TEST) + { "test", cmd_test, 3, NSH_MAX_ARGUMENTS, "<expression>" }, +#endif + +#if !defined(CONFIG_DISABLE_MOUNTPOINT) && CONFIG_NFILE_DESCRIPTORS > 0 && defined(CONFIG_FS_READABLE) +# ifndef CONFIG_NSH_DISABLE_UMOUNT + { "umount", cmd_umount, 2, 2, "<dir-path>" }, +# endif +#endif + +#ifndef CONFIG_DISABLE_ENVIRON +# ifndef CONFIG_NSH_DISABLE_UNSET + { "unset", cmd_unset, 2, 2, "<name>" }, +# endif +#endif + +#ifndef CONFIG_DISABLE_SIGNALS +# ifndef CONFIG_NSH_DISABLE_USLEEP + { "usleep", cmd_usleep, 2, 2, "<usec>" }, +# endif +#endif + +#if defined(CONFIG_NET_TCP) && CONFIG_NFILE_DESCRIPTORS > 0 +# ifndef CONFIG_NSH_DISABLE_GET + { "wget", cmd_wget, 2, 4, "[-o <local-path>] <url>" }, +# endif +#endif + +#ifndef CONFIG_NSH_DISABLE_XD + { "xd", cmd_xd, 3, 3, "<hex-address> <byte-count>" }, +#endif + { NULL, NULL, 1, 1, NULL } +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* If NuttX versioning information is available, Include that information + * in the NSH greeting. + */ + +#if CONFIG_VERSION_MAJOR != 0 || CONFIG_VERSION_MINOR != 0 +const char g_nshgreeting[] = "\nNuttShell (NSH) NuttX-" CONFIG_VERSION_STRING "\n"; +#else +const char g_nshgreeting[] = "\nNuttShell (NSH)\n"; +#endif + +/* The NSH prompt */ + +const char g_nshprompt[] = "nsh> "; + +/* Common, message formats */ + +const char g_nshsyntax[] = "nsh: %s: syntax error\n"; +const char g_fmtargrequired[] = "nsh: %s: missing required argument(s)\n"; +const char g_fmtarginvalid[] = "nsh: %s: argument invalid\n"; +const char g_fmtargrange[] = "nsh: %s: value out of range\n"; +const char g_fmtcmdnotfound[] = "nsh: %s: command not found\n"; +const char g_fmtnosuch[] = "nsh: %s: no such %s: %s\n"; +const char g_fmttoomanyargs[] = "nsh: %s: too many arguments\n"; +const char g_fmtdeepnesting[] = "nsh: %s: nesting too deep\n"; +const char g_fmtcontext[] = "nsh: %s: not valid in this context\n"; +#ifdef CONFIG_NSH_STRERROR +const char g_fmtcmdfailed[] = "nsh: %s: %s failed: %s\n"; +#else +const char g_fmtcmdfailed[] = "nsh: %s: %s failed: %d\n"; +#endif +const char g_fmtcmdoutofmemory[] = "nsh: %s: out of memory\n"; +const char g_fmtinternalerror[] = "nsh: %s: Internal error\n"; +#ifndef CONFIG_DISABLE_SIGNALS +const char g_fmtsignalrecvd[] = "nsh: %s: Interrupted by signal\n"; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_help + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_HELP +static int cmd_help(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + const struct cmdmap_s *ptr; +#ifdef CONFIG_NSH_BUILTIN_APPS + FAR const char * name; + int i; +#endif + + nsh_output(vtbl, "NSH command forms:\n"); +#ifndef CONFIG_NSH_DISABLEBG + nsh_output(vtbl, " [nice [-d <niceness>>]] <cmd> [> <file>|>> <file>] [&]\n"); +#else + nsh_output(vtbl, " <cmd> [> <file>|>> <file>]\n"); +#endif +#ifndef CONFIG_NSH_DISABLESCRIPT + nsh_output(vtbl, "OR\n"); + nsh_output(vtbl, " if <cmd>\n"); + nsh_output(vtbl, " then\n"); + nsh_output(vtbl, " [sequence of <cmd>]\n"); + nsh_output(vtbl, " else\n"); + nsh_output(vtbl, " [sequence of <cmd>]\n"); + nsh_output(vtbl, " fi\n"); +#endif + nsh_output(vtbl, "Where <cmd> is one of:\n"); + for (ptr = g_cmdmap; ptr->cmd; ptr++) + { + if (ptr->usage) + { + nsh_output(vtbl, " %s %s\n", ptr->cmd, ptr->usage); + } + else + { + nsh_output(vtbl, " %s\n", ptr->cmd); + } + } + + /* List the set of available built-in commands */ + +#ifdef CONFIG_NSH_BUILTIN_APPS + nsh_output(vtbl, "\nBuiltin Apps:\n"); + for (i = 0; (name = namedapp_getname(i)) != NULL; i++) + { + nsh_output(vtbl, " %s\n", name); + } +#endif + + return OK; +} +#endif + +/**************************************************************************** + * Name: cmd_unrecognized + ****************************************************************************/ + +static int cmd_unrecognized(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + nsh_output(vtbl, g_fmtcmdnotfound, argv[0]); + return ERROR; +} + +/**************************************************************************** + * Name: cmd_exit + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_EXIT +static int cmd_exit(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + nsh_exit(vtbl, 0); + return OK; +} +#endif + +/**************************************************************************** + * Name: nsh_execute + * + * Description: + * Exectue the command in argv[0] + * + * Returned Value: + * <0 If exec_namedapp() fails, then the negated errno value + * is returned. + * -1 (ERRROR) if the command was unsuccessful + * 0 (OK) if the command was successful + * 1 if an application task was spawned successfully, but + * returned failure exit status. + * + ****************************************************************************/ + +static int nsh_execute(FAR struct nsh_vtbl_s *vtbl, int argc, char *argv[]) +{ + const struct cmdmap_s *cmdmap; + const char *cmd; + cmd_t handler = cmd_unrecognized; + int ret; + + /* The form of argv is: + * + * argv[0]: The command name. This is argv[0] when the arguments + * are, finally, received by the command vtblr + * argv[1]: The beginning of argument (up to NSH_MAX_ARGUMENTS) + * argv[argc]: NULL terminating pointer + */ + + cmd = argv[0]; + + /* Try to find a command in the application library. */ + +#ifdef CONFIG_NSH_BUILTIN_APPS + ret = nsh_execapp(vtbl, cmd, argv); + + /* If the built-in application was successfully started, return OK + * or 1 (if the application returned a non-zero exit status). + */ + + if (ret >= 0) + { + return ret; + } +#endif + + /* See if the command is one that we understand */ + + for (cmdmap = g_cmdmap; cmdmap->cmd; cmdmap++) + { + if (strcmp(cmdmap->cmd, cmd) == 0) + { + /* Check if a valid number of arguments was provided. We + * do this simple, imperfect checking here so that it does + * not have to be performed in each command. + */ + + if (argc < cmdmap->minargs) + { + /* Fewer than the minimum number were provided */ + + nsh_output(vtbl, g_fmtargrequired, cmd); + return ERROR; + } + else if (argc > cmdmap->maxargs) + { + /* More than the maximum number were provided */ + + nsh_output(vtbl, g_fmttoomanyargs, cmd); + return ERROR; + } + else + { + /* A valid number of arguments were provided (this does + * not mean they are right). + */ + + handler = cmdmap->handler; + break; + } + } + } + + ret = handler(vtbl, argc, argv); + return ret; +} + +/**************************************************************************** + * Name: nsh_releaseargs + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLEBG +static void nsh_releaseargs(struct cmdarg_s *arg) +{ + FAR struct nsh_vtbl_s *vtbl = arg->vtbl; + int i; + + /* If the output was redirected, then file descriptor should + * be closed. The created task has its one, independent copy of + * the file descriptor + */ + + if (vtbl->np.np_redirect) + { + (void)close(arg->fd); + } + + /* Released the cloned vtbl instance */ + + nsh_release(vtbl); + + /* Release the cloned args */ + + for (i = 0; i < arg->argc; i++) + { + free(arg->argv[i]); + } + free(arg); +} +#endif + +/**************************************************************************** + * Name: nsh_child + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLEBG +static pthread_addr_t nsh_child(pthread_addr_t arg) +{ + struct cmdarg_s *carg = (struct cmdarg_s *)arg; + int ret; + + dbg("BG %s\n", carg->argv[0]); + + /* Execute the specified command on the child thread */ + + ret = nsh_execute(carg->vtbl, carg->argc, carg->argv); + + /* Released the cloned arguments */ + + dbg("BG %s complete\n", carg->argv[0]); + nsh_releaseargs(carg); + return (void*)ret; +} +#endif + +/**************************************************************************** + * Name: nsh_cloneargs + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLEBG +static inline struct cmdarg_s *nsh_cloneargs(FAR struct nsh_vtbl_s *vtbl, + int fd, int argc, char *argv[]) +{ + struct cmdarg_s *ret = (struct cmdarg_s *)zalloc(sizeof(struct cmdarg_s)); + int i; + + if (ret) + { + ret->vtbl = vtbl; + ret->fd = fd; + ret->argc = argc; + + for (i = 0; i < argc; i++) + { + ret->argv[i] = strdup(argv[i]); + } + } + return ret; +} +#endif + +/**************************************************************************** + * Name: nsh_argument + ****************************************************************************/ + +char *nsh_argument(FAR struct nsh_vtbl_s *vtbl, char **saveptr) +{ + char *pbegin = *saveptr; + char *pend = NULL; + const char *term; +#ifndef CONFIG_DISABLE_ENVIRON + bool quoted = false; +#endif + + /* Find the beginning of the next token */ + + for (; + *pbegin && strchr(g_delim, *pbegin) != NULL; + pbegin++); + + /* If we are at the end of the string with nothing + * but delimiters found, then return NULL. + */ + + if (!*pbegin) + { + return NULL; + } + + /* Does the token begin with '>' -- redirection of output? */ + + if (*pbegin == '>') + { + /* Yes.. does it begin with ">>"? */ + + if (*(pbegin + 1) == '>') + { + *saveptr = pbegin + 2; + pbegin = (char*)g_redirect2; + } + else + { + *saveptr = pbegin + 1; + pbegin = (char*)g_redirect1; + } + } + + /* Does the token begin with '#' -- comment */ + + else if (*pbegin == '#') + { + /* Return NULL meaning that we are at the end of the line */ + + *saveptr = pbegin; + pbegin = NULL; + } + else + { + /* Otherwise, we are going to have to parse to find the end of + * the token. Does the token begin with '"'? + */ + + if (*pbegin == '"') + { + /* Yes.. then only another '"' can terminate the string */ + + pbegin++; + term = "\""; +#ifndef CONFIG_DISABLE_ENVIRON + quoted = true; +#endif + } + else + { + /* No, then any of the usual terminators will terminate the argument */ + + term = g_delim; + } + + /* Find the end of the string */ + + for (pend = pbegin + 1; + *pend && strchr(term, *pend) == NULL; + pend++); + + /* pend either points to the end of the string or to + * the first delimiter after the string. + */ + + if (*pend) + { + /* Turn the delimiter into a null terminator */ + + *pend++ = '\0'; + } + + /* Save the pointer where we left off */ + + *saveptr = pend; + +#ifndef CONFIG_DISABLE_ENVIRON + /* Check for references to environment variables */ + + if (pbegin[0] == '$' && !quoted) + { + /* Check for built-in variables */ + + if (strcmp(pbegin, g_exitstatus) == 0) + { + if (vtbl->np.np_fail) + { + return (char*)g_failure; + } + else + { + return (char*)g_success; + } + } + + /* Not a built-in? Return the value of the environment variable with this name */ + + else + { + char *value = getenv(pbegin+1); + if (value) + { + return value; + } + else + { + return (char*)""; + } + } + } +#endif + } + + /* Return the beginning of the token. */ + + return pbegin; +} + +/**************************************************************************** + * Name: nsh_cmdenabled + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLESCRIPT +static inline bool nsh_cmdenabled(FAR struct nsh_vtbl_s *vtbl) +{ + struct nsh_parser_s *np = &vtbl->np; + bool ret = !np->np_st[np->np_ndx].ns_disabled; + if (ret) + { + switch (np->np_st[np->np_ndx].ns_state) + { + case NSH_PARSER_NORMAL : + case NSH_PARSER_IF: + default: + break; + + case NSH_PARSER_THEN: + ret = !np->np_st[np->np_ndx].ns_ifcond; + break; + + case NSH_PARSER_ELSE: + ret = np->np_st[np->np_ndx].ns_ifcond; + break; + } + } + return ret; +} +#endif + +/**************************************************************************** + * Name: nsh_ifthenelse + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLESCRIPT +static inline int nsh_ifthenelse(FAR struct nsh_vtbl_s *vtbl, FAR char **ppcmd, FAR char **saveptr) +{ + struct nsh_parser_s *np = &vtbl->np; + FAR char *cmd = *ppcmd; + bool disabled; + + if (cmd) + { + /* Check if the command is preceeded by "if" */ + + if (strcmp(cmd, "if") == 0) + { + /* Get the cmd following the if */ + + *ppcmd = nsh_argument(vtbl, saveptr); + if (!*ppcmd) + { + nsh_output(vtbl, g_fmtarginvalid, "if"); + goto errout; + } + + /* Verify that "if" is valid in this context */ + + if (np->np_st[np->np_ndx].ns_state != NSH_PARSER_NORMAL && + np->np_st[np->np_ndx].ns_state != NSH_PARSER_THEN && + np->np_st[np->np_ndx].ns_state != NSH_PARSER_ELSE) + { + nsh_output(vtbl, g_fmtcontext, "if"); + goto errout; + } + + /* Check if we have exceeded the maximum depth of nesting */ + + if (np->np_ndx >= CONFIG_NSH_NESTDEPTH-1) + { + nsh_output(vtbl, g_fmtdeepnesting, "if"); + goto errout; + } + + /* "Push" the old state and set the new state */ + + disabled = !nsh_cmdenabled(vtbl); + np->np_ndx++; + np->np_st[np->np_ndx].ns_state = NSH_PARSER_IF; + np->np_st[np->np_ndx].ns_disabled = disabled; + np->np_st[np->np_ndx].ns_ifcond = false; + } + else if (strcmp(cmd, "then") == 0) + { + /* Get the cmd following the then -- there shouldn't be one */ + + *ppcmd = nsh_argument(vtbl, saveptr); + if (*ppcmd) + { + nsh_output(vtbl, g_fmtarginvalid, "then"); + goto errout; + } + + /* Verify that "then" is valid in this context */ + + if (np->np_st[np->np_ndx].ns_state != NSH_PARSER_IF) + { + nsh_output(vtbl, g_fmtcontext, "then"); + goto errout; + } + np->np_st[np->np_ndx].ns_state = NSH_PARSER_THEN; + } + else if (strcmp(cmd, "else") == 0) + { + /* Get the cmd following the else -- there shouldn't be one */ + + *ppcmd = nsh_argument(vtbl, saveptr); + if (*ppcmd) + { + nsh_output(vtbl, g_fmtarginvalid, "else"); + goto errout; + } + + /* Verify that "then" is valid in this context */ + + if (np->np_st[np->np_ndx].ns_state != NSH_PARSER_THEN) + { + nsh_output(vtbl, g_fmtcontext, "else"); + goto errout; + } + np->np_st[np->np_ndx].ns_state = NSH_PARSER_ELSE; + } + else if (strcmp(cmd, "fi") == 0) + { + /* Get the cmd following the fi -- there should be one */ + + *ppcmd = nsh_argument(vtbl, saveptr); + if (*ppcmd) + { + nsh_output(vtbl, g_fmtarginvalid, "fi"); + goto errout; + } + + /* Verify that "fi" is valid in this context */ + + if (np->np_st[np->np_ndx].ns_state != NSH_PARSER_THEN && + np->np_st[np->np_ndx].ns_state != NSH_PARSER_ELSE) + { + nsh_output(vtbl, g_fmtcontext, "fi"); + goto errout; + } + + if (np->np_ndx < 1) /* Shouldn't happen */ + { + nsh_output(vtbl, g_fmtinternalerror, "if"); + goto errout; + } + + /* "Pop" the previous state */ + + np->np_ndx--; + } + else if (np->np_st[np->np_ndx].ns_state == NSH_PARSER_IF) + { + nsh_output(vtbl, g_fmtcontext, cmd); + goto errout; + } + } + return OK; + +errout: + np->np_ndx = 0; + np->np_st[0].ns_state = NSH_PARSER_NORMAL; + np->np_st[0].ns_disabled = false; + np->np_st[0].ns_ifcond = false; + return ERROR; +} +#endif + +/**************************************************************************** + * Name: nsh_saveresult + ****************************************************************************/ + +static inline int nsh_saveresult(FAR struct nsh_vtbl_s *vtbl, bool result) +{ + struct nsh_parser_s *np = &vtbl->np; + +#ifndef CONFIG_NSH_DISABLESCRIPT + if (np->np_st[np->np_ndx].ns_state == NSH_PARSER_IF) + { + np->np_fail = false; + np->np_st[np->np_ndx].ns_ifcond = result; + return OK; + } + else +#endif + { + np->np_fail = result; + return result ? ERROR : OK; + } +} + +/**************************************************************************** + * Name: nsh_nice + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLEBG +static inline int nsh_nice(FAR struct nsh_vtbl_s *vtbl, FAR char **ppcmd, FAR char **saveptr) +{ + FAR char *cmd = *ppcmd; + + vtbl->np.np_nice = 0; + if (cmd) + { + /* Check if the command is preceded by "nice" */ + + if (strcmp(cmd, "nice") == 0) + { + /* Nicenesses range from -20 (most favorable scheduling) to 19 + * (least favorable). Default is 10. + */ + + vtbl->np.np_nice = 10; + + /* Get the cmd (or -d option of nice command) */ + + cmd = nsh_argument(vtbl, saveptr); + if (cmd && strcmp(cmd, "-d") == 0) + { + FAR char *val = nsh_argument(vtbl, saveptr); + if (val) + { + char *endptr; + vtbl->np.np_nice = (int)strtol(val, &endptr, 0); + if (vtbl->np.np_nice > 19 || vtbl->np.np_nice < -20 || + endptr == val || *endptr != '\0') + { + nsh_output(vtbl, g_fmtarginvalid, "nice"); + return ERROR; + } + cmd = nsh_argument(vtbl, saveptr); + } + } + + /* Return the real command name */ + + *ppcmd = cmd; + } + } + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_parse + * + * Description: + * This function parses and executes one NSH command. + * + ****************************************************************************/ + +int nsh_parse(FAR struct nsh_vtbl_s *vtbl, char *cmdline) +{ + FAR char *argv[MAX_ARGV_ENTRIES]; + FAR char *saveptr; + FAR char *cmd; + FAR char *redirfile = NULL; + int fd = -1; + int oflags = 0; + int argc; + int ret; + + /* Initialize parser state */ + + memset(argv, 0, MAX_ARGV_ENTRIES*sizeof(FAR char *)); +#ifndef CONFIG_NSH_DISABLEBG + vtbl->np.np_bg = false; +#endif + vtbl->np.np_redirect = false; + + /* Parse out the command at the beginning of the line */ + + saveptr = cmdline; + cmd = nsh_argument(vtbl, &saveptr); + + /* Handler if-then-else-fi */ + +#ifndef CONFIG_NSH_DISABLESCRIPT + if (nsh_ifthenelse(vtbl, &cmd, &saveptr) != 0) + { + goto errout; + } +#endif + + /* Handle nice */ + +#ifndef CONFIG_NSH_DISABLEBG + if (nsh_nice(vtbl, &cmd, &saveptr) != 0) + { + goto errout; + } +#endif + + /* Check if any command was provided -OR- if command processing is + * currently disabled. + */ + +#ifndef CONFIG_NSH_DISABLESCRIPT + if (!cmd || !nsh_cmdenabled(vtbl)) +#else + if (!cmd) +#endif + { + /* An empty line is not an error and an unprocessed command cannot + * generate an error, but neither should they change the last + * command status. + */ + + return OK; + } + + /* Parse all of the arguments following the command name. The form + * of argv is: + * + * argv[0]: The command name. + * argv[1]: The beginning of argument (up to NSH_MAX_ARGUMENTS) + * argv[argc-3]: Possibly '>' or '>>' + * argv[argc-2]: Possibly <file> + * argv[argc-1]: Possibly '&' + * argv[argc]: NULL terminating pointer + * + * Maximum size is NSH_MAX_ARGUMENTS+5 + */ + + argv[0] = cmd; + for (argc = 1; argc < MAX_ARGV_ENTRIES-1; argc++) + { + argv[argc] = nsh_argument(vtbl, &saveptr); + if (!argv[argc]) + { + break; + } + } + + argv[argc] = NULL; + + /* Check if the command should run in background */ + +#ifndef CONFIG_NSH_DISABLEBG + if (argc > 1 && strcmp(argv[argc-1], "&") == 0) + { + vtbl->np.np_bg = true; + argv[argc-1] = NULL; + argc--; + } +#endif + + /* Check if the output was re-directed using > or >> */ + + if (argc > 2) + { + /* Check for redirection to a new file */ + + if (strcmp(argv[argc-2], g_redirect1) == 0) + { + vtbl->np.np_redirect = true; + oflags = O_WRONLY|O_CREAT|O_TRUNC; + redirfile = nsh_getfullpath(vtbl, argv[argc-1]); + argc -= 2; + } + + /* Check for redirection by appending to an existing file */ + + else if (strcmp(argv[argc-2], g_redirect2) == 0) + { + vtbl->np.np_redirect = true; + oflags = O_WRONLY|O_CREAT|O_APPEND; + redirfile = nsh_getfullpath(vtbl, argv[argc-1]); + argc -= 2; + } + } + + /* Redirected output? */ + + if (vtbl->np.np_redirect) + { + /* Open the redirection file. This file will eventually + * be closed by a call to either nsh_release (if the command + * is executed in the background) or by nsh_undirect if the + * command is executed in the foreground. + */ + + fd = open(redirfile, oflags, 0666); + nsh_freefullpath(redirfile); + redirfile = NULL; + + if (fd < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, cmd, "open", NSH_ERRNO); + goto errout; + } + } + + /* Check if the maximum number of arguments was exceeded */ + + if (argc > NSH_MAX_ARGUMENTS) + { + nsh_output(vtbl, g_fmttoomanyargs, cmd); + } + + /* Handle the case where the command is executed in background. + * However is app is to be started as namedapp new process will + * be created anyway, so skip this step. */ + +#ifndef CONFIG_NSH_DISABLEBG + if (vtbl->np.np_bg +#ifdef CONFIG_NSH_BUILTIN_APPS + && namedapp_isavail(argv[0]) < 0 +#endif + ) + { + struct sched_param param; + struct nsh_vtbl_s *bkgvtbl; + struct cmdarg_s *args; + pthread_attr_t attr; + pthread_t thread; + + /* Get a cloned copy of the vtbl with reference count=1. + * after the command has been processed, the nsh_release() call + * at the end of nsh_child() will destroy the clone. + */ + + bkgvtbl = nsh_clone(vtbl); + if (!bkgvtbl) + { + goto errout_with_redirect; + } + + /* Create a container for the command arguments */ + + args = nsh_cloneargs(bkgvtbl, fd, argc, argv); + if (!args) + { + nsh_release(bkgvtbl); + goto errout_with_redirect; + } + + /* Handle redirection of output via a file descriptor */ + + if (vtbl->np.np_redirect) + { + (void)nsh_redirect(bkgvtbl, fd, NULL); + } + + /* Get the execution priority of this task */ + + ret = sched_getparam(0, ¶m); + if (ret != 0) + { + nsh_output(vtbl, g_fmtcmdfailed, cmd, "sched_getparm", NSH_ERRNO); + nsh_releaseargs(args); + nsh_release(bkgvtbl); + goto errout; + } + + /* Determine the priority to execute the command */ + + if (vtbl->np.np_nice != 0) + { + int priority = param.sched_priority - vtbl->np.np_nice; + if (vtbl->np.np_nice < 0) + { + int max_priority = sched_get_priority_max(SCHED_NSH); + if (priority > max_priority) + { + priority = max_priority; + } + } + else + { + int min_priority = sched_get_priority_min(SCHED_NSH); + if (priority < min_priority) + { + priority = min_priority; + } + } + param.sched_priority = priority; + } + + /* Set up the thread attributes */ + + (void)pthread_attr_init(&attr); + (void)pthread_attr_setschedpolicy(&attr, SCHED_NSH); + (void)pthread_attr_setschedparam(&attr, ¶m); + + /* Execute the command as a separate thread at the appropriate priority */ + + ret = pthread_create(&thread, &attr, nsh_child, (pthread_addr_t)args); + if (ret != 0) + { + nsh_output(vtbl, g_fmtcmdfailed, cmd, "pthread_create", NSH_ERRNO_OF(ret)); + nsh_releaseargs(args); + nsh_release(bkgvtbl); + goto errout; + } + + nsh_output(vtbl, "%s [%d:%d]\n", cmd, thread, param.sched_priority); + } + else +#endif + { + uint8_t save[SAVE_SIZE]; + + /* Handle redirection of output via a file descriptor */ + + if (vtbl->np.np_redirect) + { + nsh_redirect(vtbl, fd, save); + } + + /* Then execute the command in "foreground" -- i.e., while the user waits + * for the next prompt. nsh_execute will return: + * + * -1 (ERRROR) if the command was unsuccessful + * 0 (OK) if the command was successful + * 1 if an application task was spawned successfully, but + * returned failure exit status. + */ + + ret = nsh_execute(vtbl, argc, argv); + + /* Restore the original output. Undirect will close the redirection + * file descriptor. + */ + + if (vtbl->np.np_redirect) + { + nsh_undirect(vtbl, save); + } + + /* Treat both errors and non-zero return codes as "errors" so that + * it is possible to test for non-zero returns in nsh scripts. + */ + + if (ret != OK) + { + goto errout; + } + } + + /* Return success if the command succeeded (or at least, starting of the + * command task succeeded). + */ + + return nsh_saveresult(vtbl, false); + +#ifndef CONFIG_NSH_DISABLEBG +errout_with_redirect: + if (vtbl->np.np_redirect) + { + close(fd); + } +#endif +errout: + return nsh_saveresult(vtbl, true); +} diff --git a/apps/nshlib/nsh_proccmds.c b/apps/nshlib/nsh_proccmds.c new file mode 100644 index 000000000..487214501 --- /dev/null +++ b/apps/nshlib/nsh_proccmds.c @@ -0,0 +1,310 @@ +/**************************************************************************** + * apps/nshlib/nsh_proccmds.c + * + * Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <sched.h> +#include <errno.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* The returned value should be zero for sucess or TRUE or non zero for + * failure or FALSE. + */ + +typedef int (*exec_t)(void); + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_PS +static const char *g_statenames[] = +{ + "INVALID ", + "PENDING ", + "READY ", + "RUNNING ", + "INACTIVE", + "WAITSEM ", +#ifndef CONFIG_DISABLE_MQUEUE + "WAITSIG ", +#endif +#ifndef CONFIG_DISABLE_MQUEUE + "MQNEMPTY", + "MQNFULL " +#endif +}; + +static const char *g_ttypenames[4] = +{ + "TASK ", + "PTHREAD", + "KTHREAD", + "--?-- " +}; +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: ps_task + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_PS +static void ps_task(FAR _TCB *tcb, FAR void *arg) +{ + struct nsh_vtbl_s *vtbl = (struct nsh_vtbl_s*)arg; +#if CONFIG_MAX_TASK_ARGS > 2 + int i; +#endif + + /* Show task status */ + + nsh_output(vtbl, "%5d %3d %4s %7s%c%c %8s ", + tcb->pid, tcb->sched_priority, + tcb->flags & TCB_FLAG_ROUND_ROBIN ? "RR " : "FIFO", + g_ttypenames[(tcb->flags & TCB_FLAG_TTYPE_MASK) >> TCB_FLAG_TTYPE_SHIFT], + tcb->flags & TCB_FLAG_NONCANCELABLE ? 'N' : ' ', + tcb->flags & TCB_FLAG_CANCEL_PENDING ? 'P' : ' ', + g_statenames[tcb->task_state]); + + /* Show task name and arguments */ + + nsh_output(vtbl, "%s(", tcb->argv[0]); + + /* Special case 1st argument (no comma) */ + + if (tcb->argv[1]) + { + nsh_output(vtbl, "%p", tcb->argv[1]); + } + + /* Then any additional arguments */ + +#if CONFIG_MAX_TASK_ARGS > 2 + for (i = 2; i <= CONFIG_MAX_TASK_ARGS && tcb->argv[i]; i++) + { + nsh_output(vtbl, ", %p", tcb->argv[i]); + } +#endif + nsh_output(vtbl, ")\n"); +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_exec + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_EXEC +int cmd_exec(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *endptr; + uintptr_t addr; + + addr = (uintptr_t)strtol(argv[1], &endptr, 0); + if (!addr || endptr == argv[1] || *endptr != '\0') + { + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + return ERROR; + } + + nsh_output(vtbl, "Calling %p\n", (exec_t)addr); + return ((exec_t)addr)(); +} +#endif + +/**************************************************************************** + * Name: cmd_ps + ****************************************************************************/ + +#ifndef CONFIG_NSH_DISABLE_PS +int cmd_ps(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + nsh_output(vtbl, "PID PRI SCHD TYPE NP STATE NAME\n"); + sched_foreach(ps_task, vtbl); + return OK; +} +#endif + +/**************************************************************************** + * Name: cmd_kill + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_SIGNALS +#ifndef CONFIG_NSH_DISABLE_KILL +int cmd_kill(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *ptr; + char *endptr; + long signal; + long pid; + + /* Check incoming parameters. The first parameter should be "-<signal>" */ + + ptr = argv[1]; + if (*ptr != '-' || ptr[1] < '0' || ptr[1] > '9') + { + goto invalid_arg; + } + + /* Extract the signal number */ + + signal = strtol(&ptr[1], &endptr, 0); + + /* The second parameter should be <pid> */ + + ptr = argv[2]; + if (*ptr < '0' || *ptr > '9') + { + goto invalid_arg; + } + + /* Extract athe pid */ + + pid = strtol(ptr, &endptr, 0); + + /* Send the signal. Kill return values: + * + * EINVAL An invalid signal was specified. + * EPERM The process does not have permission to send the signal to any + * of the target processes. + * ESRCH The pid or process group does not exist. + * ENOSYS Do not support sending signals to process groups. + */ + + if (kill((pid_t)pid, (int)signal) == 0) + { + return OK; + } + + switch (errno) + { + case EINVAL: + goto invalid_arg; + + case ESRCH: + nsh_output(vtbl, g_fmtnosuch, argv[0], "task", argv[2]); + return ERROR; + + case EPERM: + case ENOSYS: + default: + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "kill", NSH_ERRNO); + return ERROR; + } + +invalid_arg: + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + return ERROR; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_sleep + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_SIGNALS +#ifndef CONFIG_NSH_DISABLE_SLEEP +int cmd_sleep(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *endptr; + long secs; + + secs = strtol(argv[1], &endptr, 0); + if (!secs || endptr == argv[1] || *endptr != '\0') + { + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + return ERROR; + } + sleep(secs); + return OK; +} +#endif +#endif + +/**************************************************************************** + * Name: cmd_usleep + ****************************************************************************/ + +#ifndef CONFIG_DISABLE_SIGNALS +#ifndef CONFIG_NSH_DISABLE_USLEEP +int cmd_usleep(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *endptr; + long usecs; + + usecs = strtol(argv[1], &endptr, 0); + if (!usecs || endptr == argv[1] || *endptr != '\0') + { + nsh_output(vtbl, g_fmtarginvalid, argv[0]); + return ERROR; + } + usleep(usecs); + return OK; +} +#endif +#endif diff --git a/apps/nshlib/nsh_romfsetc.c b/apps/nshlib/nsh_romfsetc.c new file mode 100644 index 000000000..4134b45a3 --- /dev/null +++ b/apps/nshlib/nsh_romfsetc.c @@ -0,0 +1,124 @@ +/**************************************************************************** + * apps/nshlib/nsh_romfsetc.c + * + * Copyright (C) 2008-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/mount.h> +#include <debug.h> +#include <errno.h> + +#include <nuttx/ramdisk.h> + +#include "nsh.h" + +#ifdef CONFIG_NSH_ROMFSETC + +/* Should we use the default ROMFS image? Or a custom, board-specific + * ROMFS image? + */ + +#ifdef CONFIG_NSH_ARCHROMFS +# include <arch/board/nsh_romfsimg.h> +#else +# include "nsh_romfsimg.h" +#endif + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_romfsetc + ****************************************************************************/ + +int nsh_romfsetc(void) +{ + int ret; + + /* Create a ROM disk for the /etc filesystem */ + + ret = romdisk_register(CONFIG_NSH_ROMFSDEVNO, romfs_img, + NSECTORS(romfs_img_len), CONFIG_NSH_ROMFSSECTSIZE); + if (ret < 0) + { + dbg("nsh: romdisk_register failed: %d\n", -ret); + return ERROR; + } + + /* Mount the file system */ + + vdbg("Mounting ROMFS filesystem at target=%s with source=%s\n", + CONFIG_NSH_ROMFSMOUNTPT, MOUNT_DEVNAME); + + ret = mount(MOUNT_DEVNAME, CONFIG_NSH_ROMFSMOUNTPT, "romfs", MS_RDONLY, NULL); + if (ret < 0) + { + dbg("nsh: mount(%s,%s,romfs) failed: %d\n", + MOUNT_DEVNAME, CONFIG_NSH_ROMFSMOUNTPT, errno); + return ERROR; + } + return OK; +} + +#endif /* CONFIG_NSH_ROMFSETC */ + diff --git a/apps/nshlib/nsh_romfsimg.h b/apps/nshlib/nsh_romfsimg.h new file mode 100644 index 000000000..49b0ad166 --- /dev/null +++ b/apps/nshlib/nsh_romfsimg.h @@ -0,0 +1,89 @@ +unsigned char romfs_img[] = { + 0x2d, 0x72, 0x6f, 0x6d, 0x31, 0x66, 0x73, 0x2d, 0x00, 0x00, 0x01, 0x50, + 0x9f, 0x13, 0x82, 0x87, 0x4e, 0x53, 0x48, 0x49, 0x6e, 0x69, 0x74, 0x56, + 0x6f, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x49, + 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0xd1, 0xff, 0xff, 0x97, + 0x2e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x20, + 0x00, 0x00, 0x00, 0x00, 0xd1, 0xd1, 0xff, 0x80, 0x2e, 0x2e, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, + 0x68, 0x2d, 0x96, 0x03, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa0, + 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0xd1, 0xff, 0xff, 0x00, + 0x2e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x3a, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x6e, 0x8d, 0x9c, 0xab, 0x58, 0x72, 0x63, 0x53, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x23, 0x20, 0x43, 0x72, 0x65, 0x61, 0x74, 0x65, 0x20, 0x61, 0x20, 0x52, + 0x41, 0x4d, 0x44, 0x49, 0x53, 0x4b, 0x20, 0x61, 0x6e, 0x64, 0x20, 0x6d, + 0x6f, 0x75, 0x6e, 0x74, 0x20, 0x69, 0x74, 0x20, 0x61, 0x74, 0x20, 0x2f, + 0x74, 0x6d, 0x70, 0x0a, 0x0a, 0x6d, 0x6b, 0x72, 0x64, 0x20, 0x2d, 0x6d, + 0x20, 0x32, 0x20, 0x2d, 0x73, 0x20, 0x35, 0x31, 0x32, 0x20, 0x31, 0x30, + 0x32, 0x34, 0x0a, 0x6d, 0x6b, 0x66, 0x61, 0x74, 0x66, 0x73, 0x20, 0x2f, + 0x64, 0x65, 0x76, 0x2f, 0x72, 0x61, 0x6d, 0x32, 0x0a, 0x6d, 0x6f, 0x75, + 0x6e, 0x74, 0x20, 0x2d, 0x74, 0x20, 0x76, 0x66, 0x61, 0x74, 0x20, 0x2f, + 0x64, 0x65, 0x76, 0x2f, 0x72, 0x61, 0x6d, 0x32, 0x20, 0x2f, 0x74, 0x6d, + 0x70, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, + 0x00, 0x00, 0x00, 0x00, 0xd1, 0xd1, 0xff, 0xe0, 0x2e, 0x2e, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00 +}; +unsigned int romfs_img_len = 1024; diff --git a/apps/nshlib/nsh_telnetd.c b/apps/nshlib/nsh_telnetd.c new file mode 100644 index 000000000..0117aad04 --- /dev/null +++ b/apps/nshlib/nsh_telnetd.c @@ -0,0 +1,187 @@ +/**************************************************************************** + * apps/nshlib/nsh_telnetd.c + * + * Copyright (C) 2007-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 Gregory Nutt 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <unistd.h> +#include <assert.h> +#include <debug.h> + +#include <apps/netutils/telnetd.h> + +#include "nsh.h" +#include "nsh_console.h" + +#ifdef CONFIG_NSH_TELNET + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_telnetmain + ****************************************************************************/ + +int nsh_telnetmain(int argc, char *argv[]) +{ + FAR struct console_stdio_s *pstate = nsh_newconsole(); + DEBUGASSERT(pstate != NULL); + + dbg("Session [%d] Started\n", getpid()); + + /* Present a greeting */ + + fputs(g_nshgreeting, pstate->cn_outstream); + fflush(pstate->cn_outstream); + + /* Execute the startup script */ + +#if defined(CONFIG_NSH_ROMFSETC) && !defined(CONFIG_NSH_CONSOLE) + (void)nsh_script(&pstate->cn_vtbl, "init", NSH_INITPATH); +#endif + + /* Then enter the command line parsing loop */ + + for (;;) + { + /* Display the prompt string */ + + fputs(g_nshprompt, pstate->cn_outstream); + fflush(pstate->cn_outstream); + + /* Get the next line of input from the Telnet client */ + + if (fgets(pstate->cn_line, CONFIG_NSH_LINELEN, INSTREAM(pstate)) != NULL) + { + /* Parse process the received Telnet command */ + + (void)nsh_parse(&pstate->cn_vtbl, pstate->cn_line); + fflush(pstate->cn_outstream); + } + else + { + fprintf(pstate->cn_outstream, g_fmtcmdfailed, "nsh_telnetmain", + "fgets", NSH_ERRNO); + nsh_exit(&pstate->cn_vtbl, 1); + } + } + + /* Clean up */ + + nsh_exit(&pstate->cn_vtbl, 0); + + /* We do not get here, but this is necessary to keep some compilers happy */ + + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_telnetstart + * + * Description: + * nsh_telnetstart() starts the Telnet daemon that will allow multiple + * NSH connections via Telnet. This function returns immediately after + * the daemon has been started. + * + * Input Parameters: + * None. All of the properties of the Telnet daemon are controlled by + * NuttX configuration setting. + * + * Returned Values: + * Zero if the Telnet daemon was successfully started. A negated errno + * value will be returned on failure. + * + ****************************************************************************/ + +int nsh_telnetstart(void) +{ + struct telnetd_config_s config; + int ret; + + /* Configure the telnet daemon */ + + config.d_port = HTONS(CONFIG_NSH_TELNETD_PORT); + config.d_priority = CONFIG_NSH_TELNETD_DAEMONPRIO; + config.d_stacksize = CONFIG_NSH_TELNETD_DAEMONSTACKSIZE; + config.t_priority = CONFIG_NSH_TELNETD_CLIENTPRIO; + config.t_stacksize = CONFIG_NSH_TELNETD_CLIENTSTACKSIZE; + config.t_entry = nsh_telnetmain; + + /* Start the telnet daemon */ + + vdbg("Starting the Telnet daemon\n"); + ret = telnetd_start(&config); + if (ret < 0) + { + dbg("Failed to tart the Telnet daemon: %d\n", ret); + } + + return ret; +} + +#endif /* CONFIG_NSH_TELNET */ diff --git a/apps/nshlib/nsh_test.c b/apps/nshlib/nsh_test.c new file mode 100644 index 000000000..6e1b65e5f --- /dev/null +++ b/apps/nshlib/nsh_test.c @@ -0,0 +1,438 @@ +/**************************************************************************** + * apps/nshlib/nsh_test.c + * + * Copyright (C) 2008, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* Test syntax: + * + * expression = simple-expression | !expression | + * expression -o expression | expression -a expression + * + * simple-expression = unary-expression | binary-expression + * + * unary-expression = string-unary | file-unary + * + * string-unary = -n string | -z string + * + * file-unary = -b file | -c file | -d file | -e file | -f file | + * -r file | -s file | -w file + * + * binary-expression = string-binary | numeric-binary + * + * string-binary = string = string | string == string | string != string + * + * numeric-binary = integer -eq integer | integer -ge integer | + * integer -gt integer | integer -le integer | + * integer -lt integer | integer -ne integer + * + * Note that the smallest expression consists of two strings. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/stat.h> +#include <stdlib.h> +#include <string.h> + +#include "nsh.h" +#include "nsh_console.h" + +#if !defined(CONFIG_NSH_DISABLESCRIPT) && !defined(CONFIG_NSH_DISABLE_TEST) + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define TEST_TRUE OK +#define TEST_FALSE ERROR +#define TEST_ERROR 1 + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: binaryexpression + ****************************************************************************/ + +static inline int binaryexpression(FAR struct nsh_vtbl_s *vtbl, char **argv) +{ + char *endptr; + long integer1; + long integer2; + + /* STRING2 = STRING2 */ + + if (strcmp(argv[1], "=") == 0 || strcmp(argv[1], "==") == 0) + { + /* Return true if the strings are identical */ + + return strcmp(argv[0], argv[2]) == 0 ? TEST_TRUE : TEST_FALSE; + } + + /* STRING1 != STRING2 */ + + if (strcmp(argv[1], "!=") == 0) + { + /* Return true if the strings are different */ + + return strcmp(argv[0], argv[2]) != 0 ? TEST_TRUE : TEST_FALSE; + } + + /* The remaining operators assuming that the two values are integers */ + + integer1 = strtol(argv[0], &endptr, 0); + if (argv[0][0] == '\0' || *endptr != '\0') + { + return TEST_ERROR; + } + + integer2 = strtol(argv[2], &endptr, 0); + if (argv[2][0] == '\0' || *endptr != '\0') + { + return TEST_ERROR; + } + + /* INTEGER1 -eq INTEGER2 */ + + if (strcmp(argv[1], "-eq") == 0) + { + /* Return true if the strings are different */ + + return integer1 == integer2 ? TEST_TRUE : TEST_FALSE; + } + + /* INTEGER1 -ge INTEGER2 */ + + if (strcmp(argv[1], "-ge") == 0) + { + /* Return true if the strings are different */ + + return integer1 >= integer2 ? TEST_TRUE : TEST_FALSE; + } + + /* INTEGER1 -gt INTEGER2 */ + + if (strcmp(argv[1], "-gt") == 0) + { + /* Return true if the strings are different */ + + return integer1 > integer2 ? TEST_TRUE : TEST_FALSE; + } + + /* INTEGER1 -le INTEGER2 */ + + if (strcmp(argv[1], "-le") == 0) + { + /* Return true if the strings are different */ + + return integer1 <= integer2 ? TEST_TRUE : TEST_FALSE; + } + + /* INTEGER1 -lt INTEGER2 */ + + if (strcmp(argv[1], "-lt") == 0) + { + /* Return true if the strings are different */ + + return integer1 < integer2 ? TEST_TRUE : TEST_FALSE; + } + + /* INTEGER1 -ne INTEGER2 */ + + if (strcmp(argv[1], "-ne") == 0) + { + /* Return true if the strings are different */ + + return integer1 != integer2 ? TEST_TRUE : TEST_FALSE; + } + + return TEST_ERROR; +} + +/**************************************************************************** + * Name: unaryexpression + ****************************************************************************/ + +static inline int unaryexpression(FAR struct nsh_vtbl_s *vtbl, char **argv) +{ + struct stat buf; + char *fullpath; + int ret; + + /* -n STRING */ + + if (strcmp(argv[0], "-n") == 0) + { + /* Return true if the length of the string is non-zero */ + + return strlen(argv[1]) != 0 ? TEST_TRUE : TEST_FALSE; + } + + /* -z STRING */ + + if (strcmp(argv[0], "-z") == 0) + { + /* Return true if the length of the string is zero */ + + return strlen(argv[1]) == 0 ? TEST_TRUE : TEST_FALSE; + } + + /* All of the remaining assume that the following argument is the + * path to a file. + */ + + fullpath = nsh_getfullpath(vtbl, argv[1]); + if (!fullpath) + { + return TEST_FALSE; + } + + ret = stat(fullpath, &buf); + nsh_freefullpath(fullpath); + + if (ret != 0) + { + /* The file does not exist (or another error occurred) -- return FALSE */ + + return TEST_FALSE; + } + + /* -b FILE */ + + if (strcmp(argv[0], "-b") == 0) + { + /* Return true if the path is a block device */ + + return S_ISBLK(buf.st_mode) ? TEST_TRUE : TEST_FALSE; + } + + /* -c FILE */ + + if (strcmp(argv[0], "-c") == 0) + { + /* Return true if the path is a character device */ + + return S_ISCHR(buf.st_mode) ? TEST_TRUE : TEST_FALSE; + } + + /* -d FILE */ + + if (strcmp(argv[0], "-d") == 0) + { + /* Return true if the path is a directory */ + + return S_ISDIR(buf.st_mode) ? TEST_TRUE : TEST_FALSE; + } + + /* -e FILE */ + + if (strcmp(argv[0], "-e") == 0) + { + /* Return true if the file exists */ + + return TEST_TRUE; + } + + /* -f FILE */ + + if (strcmp(argv[0], "-f") == 0) + { + /* Return true if the path refers to a regular file */ + + return S_ISREG(buf.st_mode) ? TEST_TRUE : TEST_FALSE; + } + + /* -r FILE */ + + if (strcmp(argv[0], "-r") == 0) + { + /* Return true if the file is readable */ + + return (buf.st_mode & (S_IRUSR|S_IRGRP|S_IROTH)) != 0 ? TEST_TRUE : TEST_FALSE; + } + + /* -s FILE */ + + if (strcmp(argv[0], "-s") == 0) + { + /* Return true if the size of the file is greater than zero */ + + return buf.st_size > 0 ? TEST_TRUE : TEST_FALSE; + } + + /* -w FILE */ + + if (strcmp(argv[0], "-w") == 0) + { + /* Return true if the file is write-able */ + + return (buf.st_mode & (S_IWUSR|S_IWGRP|S_IWOTH)) != 0 ? TEST_TRUE : TEST_FALSE; + } + + /* Unrecognized operator */ + + return TEST_ERROR; +} + +/**************************************************************************** + * Name: expression + ****************************************************************************/ + +static int expression(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + int value; + int i = 0; + + /* Check for unary operations on expressions */ + + if (strcmp(argv[0], "!") == 0) + { + if (argc < 2) + { + goto errout_syntax; + } + return expression(vtbl, argc-1, &argv[1]) == TEST_TRUE ? TEST_FALSE : TEST_TRUE; + } + + /* Check for unary operations on simple, typed arguments */ + + else if (argv[0][0] == '-') + { + if (argc < 2) + { + goto errout_syntax; + } + i += 2; + value = unaryexpression(vtbl, argv); + } + + /* Check for binary operations on simple, typed arguments */ + + else + { + if (argc < 3) + { + goto errout_syntax; + } + i += 3; + value = binaryexpression(vtbl, argv); + } + + /* Test if there any failure */ + + if (value == TEST_ERROR) + { + goto errout_syntax; + } + + /* Is there anything after the simple expression? */ + + if (i < argc) + { + /* EXPRESSION -a EXPRESSION */ + + if (strcmp(argv[i], "-a") == 0) + { + if (value != TEST_TRUE) + { + return TEST_FALSE; + } + else + { + i++; + return expression(vtbl, argc-i, &argv[i]); + } + } + + /* EXPRESSION -o EXPRESSION */ + + else if (strcmp(argv[i], "-o") == 0) + { + if (value == TEST_TRUE) + { + return TEST_TRUE; + } + else + { + i++; + return expression(vtbl, argc-i, &argv[i]); + } + } + else + { + goto errout_syntax; + } + } + return value; + +errout_syntax: + nsh_output(vtbl, g_nshsyntax, "test"); + return TEST_FALSE; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_test + ****************************************************************************/ + +int cmd_test(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + return expression(vtbl, argc-1, &argv[1]); +} + +/**************************************************************************** + * Name: cmd_lbracket + ****************************************************************************/ + +int cmd_lbracket(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + if (strcmp(argv[argc-1], "]") != 0) + { + nsh_output(vtbl, g_nshsyntax, argv[0]); + return ERROR; + } + else + { + return expression(vtbl, argc-2, &argv[1]); + } +} + +#endif /* !CONFIG_NSH_DISABLESCRIPT && !CONFIG_NSH_DISABLE_TEST */ diff --git a/apps/nshlib/nsh_timcmds.c b/apps/nshlib/nsh_timcmds.c new file mode 100644 index 000000000..bc2bda6f4 --- /dev/null +++ b/apps/nshlib/nsh_timcmds.c @@ -0,0 +1,331 @@ +/**************************************************************************** + * apps/nshlib/dbg_timcmds.c + * + * 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 + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <time.h> + +#include "nsh.h" +#include "nsh_console.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define MAX_TIME_STRING 80 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) +static FAR const char * const g_datemontab[] = +{ + "jan", "feb", "mar", "apr", "may", "jun", + "jul", "aug", "sep", "oct", "nov", "dec" +}; +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: date_month + ****************************************************************************/ + +#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) +static inline int date_month(FAR const char *abbrev) +{ + int i; + + for (i = 0; i < 12; i++) + { + if (strncasecmp(g_datemontab[i], abbrev, 3) == 0) + { + return i; + } + } + return ERROR; +} +#endif + +/**************************************************************************** + * Name: date_gettime + ****************************************************************************/ + +#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) +static inline int date_showtime(FAR struct nsh_vtbl_s *vtbl, FAR const char *name) +{ + static const char format[] = "%b %d %H:%M:%S %Y"; + struct timespec ts; + struct tm tm; + char timbuf[MAX_TIME_STRING]; + int ret; + + /* Get the current time */ + + ret = clock_gettime(CLOCK_REALTIME, &ts); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, name, "clock_gettime", NSH_ERRNO); + return ERROR; + } + + /* Break the current time up into the format needed by strftime */ + + (void)gmtime_r((FAR const time_t*)&ts.tv_sec, &tm); + + /* Show the current time in the requested format */ + + (void)strftime(timbuf, MAX_TIME_STRING, format, &tm); + nsh_output(vtbl, "%s\n", timbuf); + return OK; +} +#endif + +/**************************************************************************** + * Name: date_settime + ****************************************************************************/ + +#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) +static inline int date_settime(FAR struct nsh_vtbl_s *vtbl, FAR const char *name, + FAR char *newtime) +{ + struct timespec ts; + struct tm tm; + FAR char *token; + FAR char *saveptr; + long result; + int ret; + + /* Only this date format is supported: MMM DD HH:MM:SS YYYY */ + /* Get the month abbreviation */ + + token = strtok_r(newtime, " \t",&saveptr); + if (token == NULL) + { + goto errout_bad_parm; + } + + tm.tm_mon = date_month(token); + if (tm.tm_mon < 0) + { + goto errout_bad_parm; + } + + /* Get the day of the month. NOTE: Accepts day-of-month up to 31 for all months */ + + token = strtok_r(NULL, " \t",&saveptr); + if (token == NULL) + { + goto errout_bad_parm; + } + + result = strtol(token, NULL, 10); + if (result < 1 || result > 31) + { + goto errout_bad_parm; + } + tm.tm_mday = (int)result; + + /* Get the hours */ + + token = strtok_r(NULL, " \t:", &saveptr); + if (token == NULL) + { + goto errout_bad_parm; + } + + result = strtol(token, NULL, 10); + if (result < 0 || result > 23) + { + goto errout_bad_parm; + } + tm.tm_hour = (int)result; + + /* Get the minutes */ + + token = strtok_r(NULL, " \t:", &saveptr); + if (token == NULL) + { + goto errout_bad_parm; + } + + result = strtol(token, NULL, 10); + if (result < 0 || result > 59) + { + goto errout_bad_parm; + } + tm.tm_min = (int)result; + + /* Get the seconds */ + + token = strtok_r(NULL, " \t:", &saveptr); + if (token == NULL) + { + goto errout_bad_parm; + } + + result = strtol(token, NULL, 10); + if (result < 0 || result > 61) + { + goto errout_bad_parm; + } + tm.tm_sec = (int)result; + + /* And finally the year */ + + token = strtok_r(NULL, " \t", &saveptr); + if (token == NULL) + { + goto errout_bad_parm; + } + + result = strtol(token, NULL, 10); + if (result < 1900 || result > 2100) + { + goto errout_bad_parm; + } + tm.tm_year = (int)result - 1900; + + /* Convert this to the right form, then set the timer */ + + ts.tv_sec = mktime(&tm); + ts.tv_nsec = 0; + + ret = clock_settime(CLOCK_REALTIME, &ts); + if (ret < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, name, "clock_settime", NSH_ERRNO); + return ERROR; + } + return OK; + +errout_bad_parm: + nsh_output(vtbl, g_fmtarginvalid, name); + return ERROR; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cmd_date + ****************************************************************************/ + +#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) +int cmd_date(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + FAR char *newtime = NULL; + FAR const char *errfmt; + bool badarg = false; + int option; + int ret; + + /* Get the date options: date [-s time] [+FORMAT] */ + + while ((option = getopt(argc, argv, "s:")) != ERROR) + { + if (option == 's') + { + /* We will be setting the time */ + + newtime = optarg; + } + else /* option = '?' */ + { + /* We need to parse to the end anyway so that getopt stays healthy */ + + badarg = true; + } + } + + /* If a bad argument was encountered then exit with an error */ + + if (badarg) + { + errfmt = g_fmtarginvalid; + goto errout; + } + + /* optind < argc-1 means that there are additional, unexpected arguments on + * th command-line + */ + + if (optind < argc) + { + errfmt = g_fmttoomanyargs; + goto errout; + } + + /* Display or set the time */ + + if (newtime) + { + ret = date_settime(vtbl, argv[0], newtime); + } + else + { + ret = date_showtime(vtbl, argv[0]); + } + return ret; + +errout: + nsh_output(vtbl, errfmt, argv[0]); + return ERROR; +} +#endif diff --git a/apps/nshlib/nsh_usbdev.c b/apps/nshlib/nsh_usbdev.c new file mode 100644 index 000000000..3d123532a --- /dev/null +++ b/apps/nshlib/nsh_usbdev.c @@ -0,0 +1,243 @@ +/**************************************************************************** + * apps/nshlib/nsh_usbdev.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <assert.h> +#include <debug.h> + +#ifdef CONFIG_CDCACM +# include <nuttx/usb/cdcacm.h> +#endif + +#ifdef CONFIG_CDCACM +# include <nuttx/usb/pl2303.h> +#endif + +#include "nsh.h" + +#ifdef CONFIG_USBDEV + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#if defined(CONFIG_DEBUG) || defined(CONFIG_NSH_USBCONSOLE) +# define trmessage lib_lowprintf +#else +# define trmessage printf +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_tracecallback + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_TRACE +static int nsh_tracecallback(struct usbtrace_s *trace, void *arg) +{ + usbtrace_trprintf((trprintf_t)trmessage, trace->event, trace->value); + return 0; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nsh_usbconsole + ****************************************************************************/ + +#ifdef HAVE_USB_CONSOLE +int nsh_usbconsole(void) +{ + char inch; + ssize_t nbytes; + int nlc; + int fd; + int ret; + + /* Initialize any USB tracing options that were requested */ + +#ifdef CONFIG_USBDEV_TRACE + usbtrace_enable(TRACE_BITSET); +#endif + + /* Don't start the NSH console until the console device is ready. Chances + * are, we get here with no functional console. The USB console will not + * be available until the device is connected to the host and until the + * host-side application opens the connection. + */ + + /* Initialize the USB serial driver */ + +#if defined(CONFIG_PL2303) || defined(CONFIG_CDCACM) +#ifdef CONFIG_CDCACM + ret = cdcacm_initialize(CONFIG_NSH_UBSDEV_MINOR, NULL); +#else + ret = usbdev_serialinitialize(CONFIG_NSH_UBSDEV_MINOR); +#endif + DEBUGASSERT(ret == OK); +#endif + + /* Open the USB serial device for read/write access */ + + do + { + /* Try to open the console */ + + fd = open(CONFIG_NSH_USBCONDEV, O_RDWR); + if (fd < 0) + { + /* ENOTCONN means that the USB device is not yet connected. Anything + * else is bad. + */ + + DEBUGASSERT(errno == ENOTCONN); + + /* Sleep a bit and try again */ + + sleep(2); + } + } + while (fd < 0); + + /* Now waiting until we successfully read a carriage return a few times. + * That is a sure way of know that there is something at the other end of + * the USB serial connection that is ready to talk with us. The user needs + * to hit ENTER a few times to get things started. + */ + + nlc = 0; + do + { + /* Read one byte */ + + inch = 0; + nbytes = read(fd, &inch, 1); + + /* Is it a carriage return (or maybe a newline)? */ + + if (nbytes == 1 && (inch == '\n' || inch == '\r')) + { + /* Yes.. increment the count */ + + nlc++; + } + else + { + /* No.. Reset the count. We need to see 3 in a row to continue. */ + + nlc = 0; + } + } + while (nlc < 3); + + /* Make sure the stdin, stdout, and stderr are closed */ + + (void)fclose(stdin); + (void)fclose(stdout); + (void)fclose(stderr); + + /* Dup the fd to create standard fd 0-2 */ + + (void)dup2(fd, 0); + (void)dup2(fd, 1); + (void)dup2(fd, 2); + + /* We can close the original file descriptor now (unless it was one of 0-2) */ + + if (fd > 2) + { + close(fd); + } + + /* fdopen to get the stdin, stdout and stderr streams. The following logic depends + * on the fact that the library layer will allocate FILEs in order. And since + * we closed stdin, stdout, and stderr above, that is what we should get. + * + * fd = 0 is stdin (read-only) + * fd = 1 is stdout (write-only, append) + * fd = 2 is stderr (write-only, append) + */ + + (void)fdopen(0, "r"); + (void)fdopen(1, "a"); + (void)fdopen(2, "a"); + return OK; +} + +#endif /* HAVE_USB_CONSOLE */ + +/**************************************************************************** + * Name: nsh_usbtrace + ****************************************************************************/ + +#if defined(CONFIG_USBDEV_TRACE) && defined(HAVE_USB_CONSOLE) +void nsh_usbtrace(void) +{ + (void)usbtrace_enumerate(nsh_tracecallback, NULL); +} +#endif + +#endif /* CONFIG_USBDEV */ diff --git a/apps/nshlib/rcS.template b/apps/nshlib/rcS.template new file mode 100644 index 000000000..996f37fb1 --- /dev/null +++ b/apps/nshlib/rcS.template @@ -0,0 +1,5 @@ +# Create a RAMDISK and mount it at XXXRDMOUNTPOUNTXXX + +mkrd -m XXXMKRDMINORXXX -s XXMKRDSECTORSIZEXXX XXMKRDBLOCKSXXX +mkfatfs /dev/ramXXXMKRDMINORXXX +mount -t vfat /dev/ramXXXMKRDMINORXXX XXXRDMOUNTPOUNTXXX diff --git a/apps/position_estimator/.context b/apps/position_estimator/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/position_estimator/.context diff --git a/apps/position_estimator/Makefile b/apps/position_estimator/Makefile new file mode 100644 index 000000000..a766f1666 --- /dev/null +++ b/apps/position_estimator/Makefile @@ -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. +# +############################################################################ + +# +# Makefile to build the position estimator +# + +APPNAME = position_estimator +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +CSRCS = position_estimator_main.c \ + codegen/position_estimator.c \ + codegen/position_estimator_initialize.c \ + codegen/position_estimator_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator/codegen/position_estimator.c b/apps/position_estimator/codegen/position_estimator.c new file mode 100644 index 000000000..731ae03e3 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator.c @@ -0,0 +1,261 @@ +/* + * position_estimator.c + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "position_estimator.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T + xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T + predict_only, real32_T xapo1[6], real32_T Papo1[36]) +{ + real32_T fv0[6]; + real32_T fv1[6]; + real32_T I[36]; + real32_T xapri[6]; + int32_T i; + int32_T r1; + static const real32_T fv2[36] = { 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.004F, + 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.004F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.004F, 1.0F }; + + static const real32_T fv3[12] = { 0.0F, 0.0F, 0.1744F, 87.2F, 0.0F, 0.0F, + -0.1744F, -87.2F, 0.0F, 0.0F, 0.0F, 0.0F }; + + int32_T r2; + real32_T Papri[36]; + real32_T maxval; + static const real32_T fv4[36] = { 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.004F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }; + + static const real32_T fv5[36] = { 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0E-7F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, 0.0F, 1.0F }; + + real32_T K[18]; + static const int8_T iv0[18] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0 }; + + real32_T fv6[9]; + static const int8_T iv1[18] = { 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 1, 0 }; + + real32_T b_gps_covariance[9]; + real32_T A[9]; + real32_T B[18]; + int32_T r3; + real32_T a21; + real32_T Y[18]; + real32_T b_z[3]; + int8_T b_I[36]; + + /* if predit_onli == 1: no update step: use this when no new gps data is available */ + /* %initialization */ + /* use model F=m*a x''=F/m */ + /* 250Hz---> dT = 0.004s */ + /* u=[phi;theta] */ + /* x=[px;vx;py;vy]; */ + /* %------------------------------------------ */ + /* %------------------------------------------------ */ + /* R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; */ + /* process Covariance Matrix */ + /* measurement Covariance Matrix */ + /* %prediction */ + for (i = 0; i < 6; i++) { + fv0[i] = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + fv0[i] += fv2[i + 6 * r1] * xapo[r1]; + } + + fv1[i] = 0.0F; + for (r1 = 0; r1 < 2; r1++) { + fv1[i] += fv3[i + 6 * r1] * u[r1]; + } + + xapri[i] = fv0[i] + fv1[i]; + for (r1 = 0; r1 < 6; r1++) { + I[i + 6 * r1] = 0.0F; + for (r2 = 0; r2 < 6; r2++) { + I[i + 6 * r1] += fv2[i + 6 * r2] * Papo[r2 + 6 * r1]; + } + } + } + + for (i = 0; i < 6; i++) { + for (r1 = 0; r1 < 6; r1++) { + maxval = 0.0F; + for (r2 = 0; r2 < 6; r2++) { + maxval += I[i + 6 * r2] * fv4[r2 + 6 * r1]; + } + + Papri[i + 6 * r1] = maxval + fv5[i + 6 * r1]; + } + } + + if (1 != predict_only) { + /* update */ + for (i = 0; i < 3; i++) { + for (r1 = 0; r1 < 6; r1++) { + K[i + 3 * r1] = 0.0F; + for (r2 = 0; r2 < 6; r2++) { + K[i + 3 * r1] += (real32_T)iv0[i + 3 * r2] * Papri[r2 + 6 * r1]; + } + } + } + + for (i = 0; i < 3; i++) { + for (r1 = 0; r1 < 3; r1++) { + fv6[i + 3 * r1] = 0.0F; + for (r2 = 0; r2 < 6; r2++) { + fv6[i + 3 * r1] += K[r1 + 3 * r2] * (real32_T)iv1[r2 + 6 * i]; + } + } + } + + b_gps_covariance[0] = gps_covariance[0]; + b_gps_covariance[1] = 0.0F; + b_gps_covariance[2] = 0.0F; + b_gps_covariance[3] = 0.0F; + b_gps_covariance[4] = gps_covariance[1]; + b_gps_covariance[5] = 0.0F; + b_gps_covariance[6] = 0.0F; + b_gps_covariance[7] = 0.0F; + b_gps_covariance[8] = gps_covariance[2]; + for (i = 0; i < 3; i++) { + for (r1 = 0; r1 < 3; r1++) { + A[r1 + 3 * i] = fv6[r1 + 3 * i] + b_gps_covariance[r1 + 3 * i]; + } + + for (r1 = 0; r1 < 6; r1++) { + B[i + 3 * r1] = 0.0F; + for (r2 = 0; r2 < 6; r2++) { + B[i + 3 * r1] += Papri[r1 + 6 * r2] * (real32_T)iv1[r2 + 6 * i]; + } + } + } + + r1 = 0; + r2 = 1; + r3 = 2; + maxval = (real32_T)fabs(A[0]); + a21 = (real32_T)fabs(A[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if ((real32_T)fabs(A[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + A[r2] /= A[r1]; + A[r3] /= A[r1]; + A[3 + r2] -= A[r2] * A[3 + r1]; + A[3 + r3] -= A[r3] * A[3 + r1]; + A[6 + r2] -= A[r2] * A[6 + r1]; + A[6 + r3] -= A[r3] * A[6 + r1]; + if ((real32_T)fabs(A[3 + r3]) > (real32_T)fabs(A[3 + r2])) { + i = r2; + r2 = r3; + r3 = i; + } + + A[3 + r3] /= A[3 + r2]; + A[6 + r3] -= A[3 + r3] * A[6 + r2]; + for (i = 0; i < 6; i++) { + Y[3 * i] = B[r1 + 3 * i]; + Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * A[r2]; + Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * A[r3]) - Y[1 + 3 * i] * A[3 + + r3]; + Y[2 + 3 * i] /= A[6 + r3]; + Y[3 * i] -= Y[2 + 3 * i] * A[6 + r1]; + Y[1 + 3 * i] -= Y[2 + 3 * i] * A[6 + r2]; + Y[1 + 3 * i] /= A[3 + r2]; + Y[3 * i] -= Y[1 + 3 * i] * A[3 + r1]; + Y[3 * i] /= A[r1]; + } + + for (i = 0; i < 3; i++) { + for (r1 = 0; r1 < 6; r1++) { + K[r1 + 6 * i] = Y[i + 3 * r1]; + } + } + + for (i = 0; i < 3; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += (real32_T)iv0[i + 3 * r1] * xapri[r1]; + } + + b_z[i] = z[i] - maxval; + } + + for (i = 0; i < 6; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + maxval += K[i + 6 * r1] * b_z[r1]; + } + + xapo1[i] = xapri[i] + maxval; + } + + for (i = 0; i < 36; i++) { + b_I[i] = 0; + } + + for (i = 0; i < 6; i++) { + b_I[i + 6 * i] = 1; + } + + for (i = 0; i < 6; i++) { + for (r1 = 0; r1 < 6; r1++) { + maxval = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + maxval += K[i + 6 * r2] * (real32_T)iv0[r2 + 3 * r1]; + } + + I[i + 6 * r1] = (real32_T)b_I[i + 6 * r1] - maxval; + } + } + + for (i = 0; i < 6; i++) { + for (r1 = 0; r1 < 6; r1++) { + Papo1[i + 6 * r1] = 0.0F; + for (r2 = 0; r2 < 6; r2++) { + Papo1[i + 6 * r1] += I[i + 6 * r2] * Papri[r2 + 6 * r1]; + } + } + } + } else { + memcpy((void *)&Papo1[0], (void *)&Papri[0], 36U * sizeof(real32_T)); + for (i = 0; i < 6; i++) { + xapo1[i] = xapri[i]; + } + } +} + +/* End of code generation (position_estimator.c) */ diff --git a/apps/position_estimator/codegen/position_estimator.h b/apps/position_estimator/codegen/position_estimator.h new file mode 100644 index 000000000..5e2c9acd8 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator.h @@ -0,0 +1,32 @@ +/* + * position_estimator.h + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +#ifndef __POSITION_ESTIMATOR_H__ +#define __POSITION_ESTIMATOR_H__ +/* Include files */ +#include <math.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +#include "rtwtypes.h" +#include "position_estimator_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void position_estimator(const real32_T u[2], const real32_T z[3], const real32_T xapo[6], const real32_T Papo[36], const real32_T gps_covariance[3], uint8_T predict_only, real32_T xapo1[6], real32_T Papo1[36]); +#endif +/* End of code generation (position_estimator.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_initialize.c b/apps/position_estimator/codegen/position_estimator_initialize.c new file mode 100644 index 000000000..862381d3b --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_initialize.c @@ -0,0 +1,31 @@ +/* + * position_estimator_initialize.c + * + * Code generation for function 'position_estimator_initialize' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "position_estimator.h" +#include "position_estimator_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void position_estimator_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (position_estimator_initialize.c) */ diff --git a/apps/position_estimator/codegen/position_estimator_initialize.h b/apps/position_estimator/codegen/position_estimator_initialize.h new file mode 100644 index 000000000..71013b390 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_initialize.h @@ -0,0 +1,32 @@ +/* + * position_estimator_initialize.h + * + * Code generation for function 'position_estimator_initialize' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +#ifndef __POSITION_ESTIMATOR_INITIALIZE_H__ +#define __POSITION_ESTIMATOR_INITIALIZE_H__ +/* Include files */ +#include <math.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +#include "rtwtypes.h" +#include "position_estimator_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void position_estimator_initialize(void); +#endif +/* End of code generation (position_estimator_initialize.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_terminate.c b/apps/position_estimator/codegen/position_estimator_terminate.c new file mode 100644 index 000000000..b25aabf40 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_terminate.c @@ -0,0 +1,31 @@ +/* + * position_estimator_terminate.c + * + * Code generation for function 'position_estimator_terminate' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "position_estimator.h" +#include "position_estimator_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void position_estimator_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (position_estimator_terminate.c) */ diff --git a/apps/position_estimator/codegen/position_estimator_terminate.h b/apps/position_estimator/codegen/position_estimator_terminate.h new file mode 100644 index 000000000..d9fcf838b --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_terminate.h @@ -0,0 +1,32 @@ +/* + * position_estimator_terminate.h + * + * Code generation for function 'position_estimator_terminate' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +#ifndef __POSITION_ESTIMATOR_TERMINATE_H__ +#define __POSITION_ESTIMATOR_TERMINATE_H__ +/* Include files */ +#include <math.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +#include "rtwtypes.h" +#include "position_estimator_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void position_estimator_terminate(void); +#endif +/* End of code generation (position_estimator_terminate.h) */ diff --git a/apps/position_estimator/codegen/position_estimator_types.h b/apps/position_estimator/codegen/position_estimator_types.h new file mode 100644 index 000000000..cb01c7426 --- /dev/null +++ b/apps/position_estimator/codegen/position_estimator_types.h @@ -0,0 +1,16 @@ +/* + * position_estimator_types.h + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +#ifndef __POSITION_ESTIMATOR_TYPES_H__ +#define __POSITION_ESTIMATOR_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (position_estimator_types.h) */ diff --git a/apps/position_estimator/codegen/rtGetInf.c b/apps/position_estimator/codegen/rtGetInf.c new file mode 100644 index 000000000..20a64117c --- /dev/null +++ b/apps/position_estimator/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/* + * rtGetInf.c + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, Inf and MinusInf + */ +#include "rtGetInf.h" +#define NumBitsPerChar 8U + +/* Function: rtGetInf ================================================== + * Abstract: + * Initialize rtInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T inf = 0.0; + if (bitsPerReal == 32U) { + inf = rtGetInfF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + } + } + + return inf; +} + +/* Function: rtGetInfF ================================================== + * Abstract: + * Initialize rtInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetInfF(void) +{ + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; +} + +/* Function: rtGetMinusInf ================================================== + * Abstract: + * Initialize rtMinusInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetMinusInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T minf = 0.0; + if (bitsPerReal == 32U) { + minf = rtGetMinusInfF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + } + } + + return minf; +} + +/* Function: rtGetMinusInfF ================================================== + * Abstract: + * Initialize rtMinusInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetMinusInfF(void) +{ + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; +} + +/* End of code generation (rtGetInf.c) */ diff --git a/apps/position_estimator/codegen/rtGetInf.h b/apps/position_estimator/codegen/rtGetInf.h new file mode 100644 index 000000000..c74f1fc28 --- /dev/null +++ b/apps/position_estimator/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/* + * rtGetInf.h + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +#ifndef __RTGETINF_H__ +#define __RTGETINF_H__ + +#include <stddef.h> +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetInf(void); +extern real32_T rtGetInfF(void); +extern real_T rtGetMinusInf(void); +extern real32_T rtGetMinusInfF(void); + +#endif +/* End of code generation (rtGetInf.h) */ diff --git a/apps/position_estimator/codegen/rtGetNaN.c b/apps/position_estimator/codegen/rtGetNaN.c new file mode 100644 index 000000000..6aa2f4639 --- /dev/null +++ b/apps/position_estimator/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/* + * rtGetNaN.c + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, NaN + */ +#include "rtGetNaN.h" +#define NumBitsPerChar 8U + +/* Function: rtGetNaN ================================================== + * Abstract: + * Initialize rtNaN needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetNaN(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T nan = 0.0; + if (bitsPerReal == 32U) { + nan = rtGetNaNF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; + tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; + nan = tmpVal.fltVal; + break; + } + } + } + + return nan; +} + +/* Function: rtGetNaNF ================================================== + * Abstract: + * Initialize rtNaNF needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetNaNF(void) +{ + IEEESingle nanF = { { 0 } }; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + nanF.wordL.wordLuint = 0xFFC00000U; + break; + } + + case BigEndian: + { + nanF.wordL.wordLuint = 0x7FFFFFFFU; + break; + } + } + + return nanF.wordL.wordLreal; +} + +/* End of code generation (rtGetNaN.c) */ diff --git a/apps/position_estimator/codegen/rtGetNaN.h b/apps/position_estimator/codegen/rtGetNaN.h new file mode 100644 index 000000000..d3ec61c9e --- /dev/null +++ b/apps/position_estimator/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/* + * rtGetNaN.h + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +#ifndef __RTGETNAN_H__ +#define __RTGETNAN_H__ + +#include <stddef.h> +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetNaN(void); +extern real32_T rtGetNaNF(void); + +#endif +/* End of code generation (rtGetNaN.h) */ diff --git a/apps/position_estimator/codegen/rt_nonfinite.c b/apps/position_estimator/codegen/rt_nonfinite.c new file mode 100644 index 000000000..40e15fd22 --- /dev/null +++ b/apps/position_estimator/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/* + * rt_nonfinite.c + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finites, + * (Inf, NaN and -Inf). + */ +#include "rt_nonfinite.h" +#include "rtGetNaN.h" +#include "rtGetInf.h" + +real_T rtInf; +real_T rtMinusInf; +real_T rtNaN; +real32_T rtInfF; +real32_T rtMinusInfF; +real32_T rtNaNF; + +/* Function: rt_InitInfAndNaN ================================================== + * Abstract: + * Initialize the rtInf, rtMinusInf, and rtNaN needed by the + * generated code. NaN is initialized as non-signaling. Assumes IEEE. + */ +void rt_InitInfAndNaN(size_t realSize) +{ + (void) (realSize); + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); +} + +/* Function: rtIsInf ================================================== + * Abstract: + * Test if value is infinite + */ +boolean_T rtIsInf(real_T value) +{ + return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); +} + +/* Function: rtIsInfF ================================================= + * Abstract: + * Test if single-precision value is infinite + */ +boolean_T rtIsInfF(real32_T value) +{ + return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); +} + +/* Function: rtIsNaN ================================================== + * Abstract: + * Test if value is not a number + */ +boolean_T rtIsNaN(real_T value) +{ +#if defined(_MSC_VER) && (_MSC_VER <= 1200) + return _isnan(value)? TRUE:FALSE; +#else + return (value!=value)? 1U:0U; +#endif +} + +/* Function: rtIsNaNF ================================================= + * Abstract: + * Test if single-precision value is not a number + */ +boolean_T rtIsNaNF(real32_T value) +{ +#if defined(_MSC_VER) && (_MSC_VER <= 1200) + return _isnan((real_T)value)? true:false; +#else + return (value!=value)? 1U:0U; +#endif +} + + +/* End of code generation (rt_nonfinite.c) */ diff --git a/apps/position_estimator/codegen/rt_nonfinite.h b/apps/position_estimator/codegen/rt_nonfinite.h new file mode 100644 index 000000000..ac9660124 --- /dev/null +++ b/apps/position_estimator/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/* + * rt_nonfinite.h + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +#ifndef __RT_NONFINITE_H__ +#define __RT_NONFINITE_H__ + +#if defined(_MSC_VER) && (_MSC_VER <= 1200) +#include <float.h> +#endif +#include <stddef.h> +#include "rtwtypes.h" + +extern real_T rtInf; +extern real_T rtMinusInf; +extern real_T rtNaN; +extern real32_T rtInfF; +extern real32_T rtMinusInfF; +extern real32_T rtNaNF; +extern void rt_InitInfAndNaN(size_t realSize); +extern boolean_T rtIsInf(real_T value); +extern boolean_T rtIsInfF(real32_T value); +extern boolean_T rtIsNaN(real_T value); +extern boolean_T rtIsNaNF(real32_T value); + +typedef struct { + struct { + uint32_T wordH; + uint32_T wordL; + } words; +} BigEndianIEEEDouble; + +typedef struct { + struct { + uint32_T wordL; + uint32_T wordH; + } words; +} LittleEndianIEEEDouble; + +typedef struct { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; +} IEEESingle; + +#endif +/* End of code generation (rt_nonfinite.h) */ diff --git a/apps/position_estimator/codegen/rtwtypes.h b/apps/position_estimator/codegen/rtwtypes.h new file mode 100644 index 000000000..e87ae1fdc --- /dev/null +++ b/apps/position_estimator/codegen/rtwtypes.h @@ -0,0 +1,175 @@ +/* + * rtwtypes.h + * + * Code generation for function 'position_estimator' + * + * C source code generated on: Fri Jun 8 13:31:21 2012 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include <limits.h> + +/*=======================================================================* + * Target hardware information + * Device type: Generic->MATLAB Host Computer + * Number of bits: char: 8 short: 16 int: 32 + * long: 64 native word size: 64 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef long int64_T; +typedef unsigned long uint64_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ + +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + typedef struct { + real32_T re; + real32_T im; + } creal32_T; + + typedef struct { + real64_T re; + real64_T im; + } creal64_T; + + typedef struct { + real_T re; + real_T im; + } creal_T; + + typedef struct { + int8_T re; + int8_T im; + } cint8_T; + + typedef struct { + uint8_T re; + uint8_T im; + } cuint8_T; + + typedef struct { + int16_T re; + int16_T im; + } cint16_T; + + typedef struct { + uint16_T re; + uint16_T im; + } cuint16_T; + + typedef struct { + int32_T re; + int32_T im; + } cint32_T; + + typedef struct { + uint32_T re; + uint32_T im; + } cuint32_T; + + typedef struct { + int64_T re; + int64_T im; + } cint64_T; + + typedef struct { + uint64_T re; + uint64_T im; + } cuint64_T; + + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) +#define MAX_int64_T ((int64_T)(9223372036854775807L)) +#define MIN_int64_T ((int64_T)(-9223372036854775807L-1L)) +#define MAX_uint64_T ((uint64_T)(0xFFFFFFFFFFFFFFFFUL)) +#define MIN_uint64_T ((uint64_T)(0UL)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) +# ifndef false +# define false (0U) +# endif +# ifndef true +# define true (1U) +# endif +#endif + +/* + * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation + * for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 + +#endif +#endif +/* End of code generation (rtwtypes.h) */ diff --git a/apps/position_estimator/position_estimator.m b/apps/position_estimator/position_estimator.m new file mode 100644 index 000000000..2ef4d8b06 --- /dev/null +++ b/apps/position_estimator/position_estimator.m @@ -0,0 +1,62 @@ +function [xapo1,Papo1] = position_estimator(u,z,xapo,Papo,gps_covariance,predict_only) %if predit_onli == 1: no update step: use this when no new gps data is available +%#codegen +%%initialization +%use model F=m*a x''=F/m +% 250Hz---> dT = 0.004s +%u=[phi;theta] +%x=[px;vx;py;vy]; +%%------------------------------------------ +dT=0.004; +%%------------------------------------------------ + +%R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1]; + + +F=[ 1, 0.004, 0, 0, 0, 0; + 0, 1, 0, 0, 0, 0; + 0, 0, 1, 0.004, 0, 0; + 0, 0, 0, 1, 0, 0; + 0, 0, 0, 0, 1, 0.004; + 0, 0, 0, 0, 0, 1]; + +B=[ 0, -0.1744; + 0, -87.2; + 0.1744, 0; + 87.2, 0; + 0, 0; + 0, 0]; + +H=[1,0,0,0,0,0; + 0,0,1,0,0,0; + 0,0,0,0,1,0]; + + + + Q=[1e-007 ,0 ,0 ,0 ,0 ,0; + 0 ,1 ,0 ,0 ,0 ,0; + 0 ,0 ,1e-007 ,0 ,0 ,0; + 0 ,0 ,0 ,1 ,0 ,0 + 0 ,0 ,0 ,0 ,1e-007 ,0; + 0 ,0 ,0 ,0 ,0 ,1]; %process Covariance Matrix + + +R=[gps_covariance(1), 0, 0; + 0, gps_covariance(2), 0; + 0, 0, gps_covariance(3)]; %measurement Covariance Matrix + +%%prediction + +xapri=F*xapo+B*u; +Papri=F*Papo*F'+Q; + +if 1 ~= predict_only + %update + yr=z-H*xapri; + S=H*Papri*H'+R; + K=(Papri*H')/S; + xapo1=xapri+K*yr; + Papo1=(eye(6)-K*H)*Papri; +else + Papo1=Papri; + xapo1=xapri; +end
\ No newline at end of file diff --git a/apps/position_estimator/position_estimator.prj b/apps/position_estimator/position_estimator.prj new file mode 100644 index 000000000..d754f36cc --- /dev/null +++ b/apps/position_estimator/position_estimator.prj @@ -0,0 +1,269 @@ +<?xml version="1.0" encoding="UTF-8"?>
+<deployment-project>
+ <configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="position_estimator" location="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator" file="/home/thomasgubler/src/px4_nxbuilder/px4fmu/apps/position_estimator/position_estimator.prj" build-checksum="2425236060">
+ <param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>
+ <param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>
+ <param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>
+ <param.mex.general.EnableBLAS>false</param.mex.general.EnableBLAS>
+ <param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>
+ <param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>
+ <param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>
+ <param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>
+ <param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>
+ <param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>
+ <param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>
+ <param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>
+ <param.mex.paths.working>option.paths.working.project</param.mex.paths.working>
+ <param.mex.paths.working.specified />
+ <param.mex.paths.build>option.paths.build.project</param.mex.paths.build>
+ <param.mex.paths.build.specified />
+ <param.mex.paths.search />
+ <param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>
+ <param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>
+ <param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>
+ <param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>
+ <param.mex.symbols.ReservedNameArray />
+ <param.mex.customcode.CustomSourceCode />
+ <param.mex.customcode.CustomHeaderCode />
+ <param.mex.customcode.CustomInitializer />
+ <param.mex.customcode.CustomTerminator />
+ <param.mex.customcode.CustomInclude />
+ <param.mex.customcode.CustomSource />
+ <param.mex.customcode.CustomLibrary />
+ <param.mex.PostCodeGenCommand />
+ <param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>
+ <param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>
+ <param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>
+ <param.mex.InlineThreshold>10</param.mex.InlineThreshold>
+ <param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>
+ <param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>
+ <param.mex.StackUsageMax>200000</param.mex.StackUsageMax>
+ <param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>
+ <param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>
+ <param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>
+ <param.grt.CCompilerOptimization>option.CCompilerOptimization.Off</param.grt.CCompilerOptimization>
+ <param.grt.CCompilerCustomOptimizations />
+ <param.grt.general.GenerateMakefile>false</param.grt.general.GenerateMakefile>
+ <param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>
+ <param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>
+ <param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>
+ <param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>
+ <param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>
+ <param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>
+ <param.grt.paths.working>option.paths.working.project</param.grt.paths.working>
+ <param.grt.paths.working.specified />
+ <param.grt.paths.build>option.paths.build.specified</param.grt.paths.build>
+ <param.grt.paths.build.specified>./codegen</param.grt.paths.build.specified>
+ <param.grt.paths.search />
+ <param.grt.report.GenerateReport>false</param.grt.report.GenerateReport>
+ <param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
+ <param.grt.GenerateComments>true</param.grt.GenerateComments>
+ <param.grt.MATLABSourceComments>false</param.grt.MATLABSourceComments>
+ <param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>
+ <param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>
+ <param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>
+ <param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>
+ <param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>
+ <param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>
+ <param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>
+ <param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>
+ <param.grt.MaxIdLength>32</param.grt.MaxIdLength>
+ <param.grt.ReservedNameArray />
+ <param.grt.customcode.CustomSourceCode />
+ <param.grt.customcode.CustomHeaderCode />
+ <param.grt.customcode.CustomInitializer />
+ <param.grt.customcode.CustomTerminator />
+ <param.grt.customcode.CustomInclude />
+ <param.grt.customcode.CustomSource />
+ <param.grt.customcode.CustomLibrary />
+ <param.grt.PostCodeGenCommand />
+ <param.grt.Verbose>false</param.grt.Verbose>
+ <param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>
+ <param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>
+ <param.ert.TargetFunctionLibrary>C89/C90 (ANSI)</param.ert.TargetFunctionLibrary>
+ <param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>
+ <param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>
+ <param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>
+ <param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>
+ <param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>
+ <param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>
+ <param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>
+ <param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>
+ <param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>
+ <param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>
+ <param.grt.InlineThreshold>10</param.grt.InlineThreshold>
+ <param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>
+ <param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>
+ <param.grt.StackUsageMax>200000</param.grt.StackUsageMax>
+ <param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>
+ <param.UseECoderFeatures>true</param.UseECoderFeatures>
+ <param.mex.outputfile>position_estimator_mex</param.mex.outputfile>
+ <param.grt.outputfile>position_estimator</param.grt.outputfile>
+ <param.artifact>option.target.artifact.lib</param.artifact>
+ <param.mex.GenCodeOnly>true</param.mex.GenCodeOnly>
+ <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
+ <param.version>R2011a</param.version>
+ <param.HasECoderFeatures>true</param.HasECoderFeatures>
+ <param.mex.mainhtml>/home/thomasgubler/Dropbox/Semester Project Autonomous Landing PX4/position_estimator/codegen/mex/position_estimator/html/index.html</param.mex.mainhtml>
+ <param.grt.mainhtml />
+ <unset>
+ <param.mex.general.TargetLang />
+ <param.mex.general.IntegrityChecks />
+ <param.mex.general.ResponsivenessChecks />
+ <param.mex.general.ExtrinsicCalls />
+ <param.mex.general.EchoExpressions />
+ <param.mex.general.EnableDebugging />
+ <param.mex.general.SaturateOnIntegerOverflow />
+ <param.mex.general.FilePartitionMethod />
+ <param.mex.general.GlobalDataSyncMethod />
+ <param.mex.general.EnableVariableSizing />
+ <param.mex.general.DynamicMemoryAllocation />
+ <param.mex.paths.working />
+ <param.mex.paths.working.specified />
+ <param.mex.paths.build />
+ <param.mex.paths.build.specified />
+ <param.mex.paths.search />
+ <param.mex.report.GenerateReport />
+ <param.mex.report.LaunchReport />
+ <param.mex.comments.GenerateComments />
+ <param.mex.comments.MATLABSourceComments />
+ <param.mex.symbols.ReservedNameArray />
+ <param.mex.customcode.CustomInclude />
+ <param.mex.customcode.CustomSource />
+ <param.mex.customcode.CustomLibrary />
+ <param.mex.PostCodeGenCommand />
+ <param.mex.EnableMemcpy />
+ <param.mex.MemcpyThreshold />
+ <param.mex.InitFltsAndDblsToZero />
+ <param.mex.InlineThreshold />
+ <param.mex.InlineThresholdMax />
+ <param.mex.InlineStackLimit />
+ <param.mex.StackUsageMax />
+ <param.mex.ConstantFoldingTimeout />
+ <param.grt.general.TargetLang />
+ <param.grt.CCompilerOptimization />
+ <param.grt.CCompilerCustomOptimizations />
+ <param.grt.general.MakeCommand />
+ <param.grt.general.TemplateMakefile />
+ <param.grt.general.SaturateOnIntegerOverflow />
+ <param.grt.general.FilePartitionMethod />
+ <param.grt.general.EnableVariableSizing />
+ <param.grt.general.DynamicMemoryAllocation />
+ <param.grt.paths.working />
+ <param.grt.paths.working.specified />
+ <param.grt.paths.search />
+ <param.grt.report.LaunchReport />
+ <param.grt.GenerateComments />
+ <param.grt.MATLABSourceComments />
+ <param.ert.MATLABFcnDesc />
+ <param.ert.CustomSymbolStrGlobalVar />
+ <param.ert.CustomSymbolStrType />
+ <param.ert.CustomSymbolStrField />
+ <param.ert.CustomSymbolStrFcn />
+ <param.ert.CustomSymbolStrTmpVar />
+ <param.ert.CustomSymbolStrMacro />
+ <param.ert.CustomSymbolStrEMXArray />
+ <param.grt.MaxIdLength />
+ <param.grt.ReservedNameArray />
+ <param.grt.customcode.CustomHeaderCode />
+ <param.grt.customcode.CustomInitializer />
+ <param.grt.customcode.CustomTerminator />
+ <param.grt.customcode.CustomInclude />
+ <param.grt.customcode.CustomSource />
+ <param.grt.customcode.CustomLibrary />
+ <param.grt.PostCodeGenCommand />
+ <param.grt.Verbose />
+ <param.grt.SupportNonFinite />
+ <param.ert.PurelyIntegerCode />
+ <param.ert.SupportNonFinite />
+ <param.ert.IncludeTerminateFcn />
+ <param.ert.MultiInstanceCode />
+ <param.ert.ParenthesesLevel />
+ <param.ert.ConvertIfToSwitch />
+ <param.ert.PreserveExternInFcnDecls />
+ <param.grt.EnableMemcpy />
+ <param.grt.MemcpyThreshold />
+ <param.grt.InitFltsAndDblsToZero />
+ <param.grt.InlineThreshold />
+ <param.grt.InlineThresholdMax />
+ <param.grt.InlineStackLimit />
+ <param.grt.StackUsageMax />
+ <param.grt.ConstantFoldingTimeout />
+ <param.UseECoderFeatures />
+ <param.mex.outputfile />
+ <param.grt.outputfile />
+ <param.version />
+ <param.HasECoderFeatures />
+ </unset>
+ <fileset.entrypoints>
+ <file value="${PROJECT_ROOT}/position_estimator.m" custom-data-expanded="true">
+ <Inputs fileName="position_estimator.m" functionName="position_estimator">
+ <Input Name="u">
+ <Class>single</Class>
+ <Size>2 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="z">
+ <Class>single</Class>
+ <Size>3 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="xapo">
+ <Class>single</Class>
+ <Size>6 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="Papo">
+ <Class>single</Class>
+ <Size>6 x 6</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="gps_covariance">
+ <Class>single</Class>
+ <Size>3 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="predict_only">
+ <Class>uint8</Class>
+ <Size>1 x 1</Size>
+ <Value />
+ <InitialValue />
+ <Complex>false</Complex>
+ </Input>
+ </Inputs>
+ </file>
+ </fileset.entrypoints>
+ <fileset.package />
+ <build-deliverables />
+ <workflow />
+ <matlab>
+ <root>/home/thomasgubler/tools/matlab</root>
+ </matlab>
+ <platform>
+ <unix>true</unix>
+ <mac>false</mac>
+ <windows>false</windows>
+ <win2k>false</win2k>
+ <winxp>false</winxp>
+ <vista>false</vista>
+ <linux>true</linux>
+ <solaris>false</solaris>
+ <osver>3.2.0-25-generic</osver>
+ <os32>false</os32>
+ <os64>true</os64>
+ <arch>glnxa64</arch>
+ <matlab>true</matlab>
+ </platform>
+ </configuration>
+</deployment-project>
+
diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c new file mode 100644 index 000000000..773cd87ff --- /dev/null +++ b/apps/position_estimator/position_estimator_main.c @@ -0,0 +1,425 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Model-identification based position estimator for multirotors + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <v1.0/common/mavlink.h> +#include <float.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <poll.h> + +#include "codegen/position_estimator.h" + +#define N_STATES 6 +#define ERROR_COVARIANCE_INIT 3 +#define R_EARTH 6371000.0 + +#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 +#define REPROJECTION_COUNTER_LIMIT 125 + +__EXPORT int position_estimator_main(int argc, char *argv[]); + +static uint16_t position_estimator_counter_position_information; + +/* values for map projection */ +static double phi_1; +static double sin_phi_1; +static double cos_phi_1; +static double lambda_0; +static double scale; + +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + phi_1 = lat_0 / 180.0 * M_PI; + lambda_0 = lon_0 / 180.0 * M_PI; + + sin_phi_1 = sin(phi_1); + cos_phi_1 = cos(phi_1); + + /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale + + /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ + const double r_earth = 6371000; + + double lat1 = phi_1; + double lon1 = lambda_0; + + double lat2 = phi_1 + 0.5 / 180 * M_PI; + double lon2 = lambda_0 + 0.5 / 180 * M_PI; + double sin_lat_2 = sin(lat2); + double cos_lat_2 = cos(lat2); + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + + /* 2) calculate distance rho on plane */ + double k_bar = 0; + double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); + + if (0 != c) + k_bar = c / sin(c); + + double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane + double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); + double rho = sqrt(pow(x2, 2) + pow(y2, 2)); + + scale = d / rho; + +} + +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_project(double lat, double lon, float *x, float *y) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + double phi = lat / 180.0 * M_PI; + double lambda = lon / 180.0 * M_PI; + + double sin_phi = sin(phi); + double cos_phi = cos(phi); + + double k_bar = 0; + /* using small angle approximation (formula in comment is without aproximation) */ + double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + + if (0 != c) + k_bar = c / sin(c); + + /* using small angle approximation (formula in comment is without aproximation) */ + *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; + *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + +// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); +} + +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +static void map_projection_reproject(float x, float y, double *lat, double *lon) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + + double x_descaled = x / scale; + double y_descaled = y / scale; + + double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_sphere = 0; + + if (c != 0) + lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); + else + lat_sphere = asin(cos_c * sin_phi_1); + +// printf("lat_sphere = %.10f\n",lat_sphere); + + double lon_sphere = 0; + + if (phi_1 == M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + + } else if (phi_1 == -M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + + } else { + + lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); + //using small angle approximation +// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); +// if(denominator != 0) +// { +// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); +// } +// else +// { +// ... +// } + } + +// printf("lon_sphere = %.10f\n",lon_sphere); + + *lat = lat_sphere * 180.0 / M_PI; + *lon = lon_sphere * 180.0 / M_PI; + +} + +/**************************************************************************** + * main + ****************************************************************************/ + +int position_estimator_main(int argc, char *argv[]) +{ + + /* welcome user */ + printf("[multirotor position_estimator] started\n"); + + /* initialize values */ + static float u[2] = {0, 0}; + static float z[3] = {0, 0, 0}; + static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0}; + static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, + ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0 + }; + + static float xapo1[N_STATES]; + static float Papo1[36]; + + static float gps_covariance[3] = {0.0f, 0.0f, 0.0f}; + + static uint16_t counter = 0; + position_estimator_counter_position_information = 0; + + uint8_t predict_only = 1; + + bool gps_valid = false; + + bool new_initialization = true; + + static double lat_current = 0;//[°]] --> 47.0 + static double lon_current = 0; //[°]] -->8.5 + + + //TODO: handle flight without gps but with estimator + + /* subscribe to vehicle status, attitude, gps */ + struct vehicle_gps_position_s gps; + struct vehicle_status_s vstatus; + struct vehicle_attitude_s att; + + int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* subscribe to attitude at 100 Hz */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + /* wait until gps signal turns valid, only then can we initialize the projection */ + while (!gps_valid) { + struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} }; + + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds, 1, 5000)) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + /* Read wether the vehicle status changed */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + gps_valid = vstatus.gps_valid; + } + } + + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + lat_current = ((double)(gps.lat)) * 1e-7; + lon_current = ((double)(gps.lon)) * 1e-7; + + /* publish global position messages only after first GPS message */ + struct vehicle_global_position_s global_pos = { + .lat = lat_current * 1e7, + .lon = lon_current * 1e7, + .alt = gps.alt + }; + int global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + + printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + + while (1) { + + /*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */ + struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} }; + + if (poll(fds, 1, 5000) <= 0) { + /* error / timeout */ + } else { + + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + /* got attitude, updating pos as well */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + + /*copy attitude */ + u[0] = att.roll; + u[1] = att.pitch; + + /* initialize map projection with the last estimate (not at full rate) */ + if (counter % PROJECTION_INITIALIZE_COUNTER_LIMIT == 0) { + map_projection_init(lat_current, lon_current); + new_initialization = true; + + } else { + new_initialization = false; + } + + /*check if new gps values are available */ + gps_valid = vstatus.gps_valid; + + + if (gps_valid) { //we are safe to use the gps signal (it has good quality) + + predict_only = 0; + /* Project gps lat lon (Geographic coordinate system) to plane*/ + map_projection_project((double)(gps.lat) * 1e-7, (double)(gps.lon) * 1e-7, &(z[0]), &(z[1])); + + /* copy altitude */ + z[2] = (gps.alt) * 1e-3; + + gps_covariance[0] = gps.eph; //TODO: needs scaling + gps_covariance[1] = gps.eph; + gps_covariance[2] = gps.epv; + + } else { + /* we can not use the gps signal (it is of low quality) */ + predict_only = 1; + } + + // predict_only = 0; //TODO: only for testing, removeme, XXX + // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX + // usleep(100000); //TODO: only for testing, removeme, XXX + + + /*Get new estimation (this is calculated in the plane) */ + //TODO: if new_initialization == true: use 0,0,0, else use xapo + if (true == new_initialization) { //TODO,XXX: uncomment! + xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane + xapo[2] = 0; + xapo[4] = 0; + position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); + + } else { + position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); + } + + + + /* Copy values from xapo1 to xapo */ + int i; + + for (i = 0; i < N_STATES; i++) { + xapo[i] = xapo1[i]; + } + + if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) { + /* Reproject from plane to geographic coordinate system */ + // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment! + map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme + // //DEBUG + // if(counter%500 == 0) + // { + // printf("phi_1: %.10f\n", phi_1); + // printf("lambda_0: %.10f\n", lambda_0); + // printf("lat_estimated: %.10f\n", lat_current); + // printf("lon_estimated: %.10f\n", lon_current); + // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]); + // fflush(stdout); + // + // } + + // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5])) + // { + /* send out */ + + global_pos.lat = lat_current; + global_pos.lon = lon_current; + global_pos.alt = xapo1[4]; + global_pos.vx = xapo1[1]; + global_pos.vy = xapo1[3]; + global_pos.vz = xapo1[5]; + + /* publish current estimate */ + orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); + // } + // else + // { + // printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]); + // fflush(stdout); + // } + + } + + counter++; + } + + } + + return 0; +} + + diff --git a/apps/px4/attitude_estimator_bm/.context b/apps/px4/attitude_estimator_bm/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/attitude_estimator_bm/.context diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile new file mode 100644 index 000000000..358b062c0 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/Makefile @@ -0,0 +1,45 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the black magic attitude estimator +# + +APPNAME = attitude_estimator_bm +PRIORITY = SCHED_PRIORITY_MAX - 10 +STACKSIZE = 3000 + +# XXX this is *horribly* broken +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c new file mode 100644 index 000000000..41ef47918 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/attitude_bm.c @@ -0,0 +1,314 @@ + +/* + * attitude_bm.c + * + * Created on: 21.12.2010 + * Author: Laurens Mackay, Tobias Naegeli + */ + +#include <math.h> +#include "attitude_bm.h" +#include "kalman.h" + + +#define TIME_STEP (1.0f / 500.0f) + +static kalman_t attitude_blackmagic_kal; + +void vect_norm(float_vect3 *vect) +{ + float length = sqrtf( + vect->x * vect->x + vect->y * vect->y + vect->z * vect->z); + + if (length != 0) { + vect->x /= length; + vect->y /= length; + vect->z /= length; + } +} + + +void vect_cross_product(const float_vect3 *a, const float_vect3 *b, + float_vect3 *c) +{ + c->x = a->y * b->z - a->z * b->y; + c->y = a->z * b->x - a->x * b->z; + c->z = a->x * b->y - a->y * b->x; +} + +void attitude_blackmagic_update_a(void) +{ + // for acc + // Idendity matrix already in A. + M(attitude_blackmagic_kal.a, 0, 1) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 0, 2) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + + M(attitude_blackmagic_kal.a, 1, 0) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 1, 2) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + M(attitude_blackmagic_kal.a, 2, 0) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + M(attitude_blackmagic_kal.a, 2, 1) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + // for mag + // Idendity matrix already in A. + M(attitude_blackmagic_kal.a, 3, 4) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 3, 5) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + + M(attitude_blackmagic_kal.a, 4, 3) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 4, 5) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + M(attitude_blackmagic_kal.a, 5, 3) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + M(attitude_blackmagic_kal.a, 5, 4) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + +} + +void attitude_blackmagic_init(void) +{ + //X Kalmanfilter + //initalize matrices + + static m_elem kal_a[12 * 12] = { + 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f + }; + + static m_elem kal_c[9 * 12] = { + 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f + }; + + + +#define FACTOR 0.5 +#define FACTORstart 1 + + +// static m_elem kal_gain[12 * 9] = +// { 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0, +// 0.0000 , +0.00002,0 , 0 , 0, 0, 0, 0 , 0, +// -0.00002,0 , 0 , 0 , 0, 0, 0, 0, 0, +// 0, 0 , 0 , 0, 0, 0, 0, 0, 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0 , 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 +// }; + + static m_elem kal_gain[12 * 9] = { + 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0, + 0.0000f , +0.00002f, 0 , 0 , 0, 0, 0, 0 , 0, + -0.00002f, 0 , 0 , 0 , 0, 0, 0, 0, 0, + 0, 0 , 0 , 0, 0, 0, 0, 0, 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0 , 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f + }; + //offset update only correct if not upside down. + +#define K (10.0f*TIME_STEP) + + static m_elem kal_gain_start[12 * 9] = { + K, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, K, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, K, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, K, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, K, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, K, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, K, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, K, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, K, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0 + }; + + + + static m_elem kal_x_apriori[12 * 1] = + { }; + + + //---> initial states sind aposteriori!? ---> fehler + static m_elem kal_x_aposteriori[12 * 1] = + { 0.0f, 0.0f, -1.0f, 0.6f, 0.0f, 0.8f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; + + kalman_init(&attitude_blackmagic_kal, 12, 9, kal_a, kal_c, + kal_gain_start, kal_gain, kal_x_apriori, kal_x_aposteriori, 1000); + +} + +void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro) +{ + //Transform accelerometer used in all directions + // float_vect3 acc_nav; + //body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav); + + // Kalman Filter + + //Calculate new linearized A matrix + attitude_blackmagic_update_a(); + + kalman_predict(&attitude_blackmagic_kal); + + //correction update + + m_elem measurement[9] = + { }; + m_elem mask[9] = + { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }; + + measurement[0] = accel->x; + measurement[1] = accel->y; + measurement[2] = accel->z; + + measurement[3] = mag->x; + measurement[4] = mag->y; + measurement[5] = mag->z; + + measurement[6] = gyro->x; + measurement[7] = gyro->y; + measurement[8] = gyro->z; + + //Put measurements into filter + + +// static int j = 0; +// if (j >= 3) +// { +// j = 0; +// +// mask[3]=1; +// mask[4]=1; +// mask[5]=1; +// j=0; +// +// }else{ +// j++;} + + kalman_correct(&attitude_blackmagic_kal, measurement, mask); + +} +void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) +{ + //debug + + // save outputs + float_vect3 kal_acc; + float_vect3 kal_mag; +// float_vect3 kal_w0, kal_w; + + kal_acc.x = kalman_get_state(&attitude_blackmagic_kal, 0); + kal_acc.y = kalman_get_state(&attitude_blackmagic_kal, 1); + kal_acc.z = kalman_get_state(&attitude_blackmagic_kal, 2); + + kal_mag.x = kalman_get_state(&attitude_blackmagic_kal, 3); + kal_mag.y = kalman_get_state(&attitude_blackmagic_kal, 4); + kal_mag.z = kalman_get_state(&attitude_blackmagic_kal, 5); + +// kal_w0.x = kalman_get_state(&attitude_blackmagic_kal, 6); +// kal_w0.y = kalman_get_state(&attitude_blackmagic_kal, 7); +// kal_w0.z = kalman_get_state(&attitude_blackmagic_kal, 8); +// +// kal_w.x = kalman_get_state(&attitude_blackmagic_kal, 9); +// kal_w.y = kalman_get_state(&attitude_blackmagic_kal, 10); +// kal_w.z = kalman_get_state(&attitude_blackmagic_kal, 11); + + rates->x = kalman_get_state(&attitude_blackmagic_kal, 9); + rates->y = kalman_get_state(&attitude_blackmagic_kal, 10); + rates->z = kalman_get_state(&attitude_blackmagic_kal, 11); + + + +// kal_w = kal_w; // XXX hack to silence compiler warning +// kal_w0 = kal_w0; // XXX hack to silence compiler warning + + + + //debug_vect("magn", mag); + + //float_vect3 x_n_b, y_n_b, z_n_b; + z_n_b->x = -kal_acc.x; + z_n_b->y = -kal_acc.y; + z_n_b->z = -kal_acc.z; + vect_norm(z_n_b); + vect_cross_product(z_n_b, &kal_mag, y_n_b); + vect_norm(y_n_b); + + vect_cross_product(y_n_b, z_n_b, x_n_b); + + + + //save euler angles + euler->x = atan2f(z_n_b->y, z_n_b->z); + euler->y = -asinf(z_n_b->x); + euler->z = atan2f(y_n_b->x, x_n_b->x); + +} diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.h b/apps/px4/attitude_estimator_bm/attitude_bm.h new file mode 100644 index 000000000..c21b3d6f1 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/attitude_bm.h @@ -0,0 +1,24 @@ +/* + * attitude_blackmagic.h + * + * Created on: May 31, 2011 + * Author: pixhawk + */ + +#ifndef attitude_blackmagic_H_ +#define attitude_blackmagic_H_ + +#include "matrix.h" + +void vect_norm(float_vect3 *vect); + +void vect_cross_product(const float_vect3 *a, const float_vect3 *b, float_vect3 *c); + +void attitude_blackmagic_update_a(void); + +void attitude_blackmagic_init(void); + +void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro); + +void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); +#endif /* attitude_blackmagic_H_ */ diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c new file mode 100644 index 000000000..3c6c8eec3 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file Black Magic Attitude Estimator */ + + + +#include <nuttx/config.h> +#include <stdio.h> +#include <unistd.h> +#include <sys/time.h> +#include <stdbool.h> +#include <fcntl.h> +#include <arch/board/up_hrt.h> +#include <string.h> +#include <poll.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <math.h> +#include <errno.h> + +#include "attitude_bm.h" + +static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds + +__EXPORT int attitude_estimator_bm_main(int argc, char *argv[]); + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * user_start + ****************************************************************************/ +int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); + +int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) +{ + float_vect3 gyro_values; + gyro_values.x = raw->gyro_rad_s[0]; + gyro_values.y = raw->gyro_rad_s[1]; + gyro_values.z = raw->gyro_rad_s[2]; + + float_vect3 accel_values; + accel_values.x = raw->accelerometer_m_s2[0]; + accel_values.y = raw->accelerometer_m_s2[1]; + accel_values.z = raw->accelerometer_m_s2[2]; + + float_vect3 mag_values; + mag_values.x = raw->magnetometer_ga[0]; + mag_values.y = raw->magnetometer_ga[1]; + mag_values.z = raw->magnetometer_ga[2]; + + attitude_blackmagic(&accel_values, &mag_values, &gyro_values); + + /* read out values */ + attitude_blackmagic_get_all(euler, rates, x_n_b, y_n_b, z_n_b); + + return OK; +} + + +int attitude_estimator_bm_main(int argc, char *argv[]) +{ + // print text + printf("Black Magic Attitude Estimator initialized..\n\n"); + fflush(stdout); + + /* data structures to read euler angles and rotation matrix back */ + float_vect3 euler = {.x = 0, .y = 0, .z = 0}; + float_vect3 rates; + float_vect3 x_n_b; + float_vect3 y_n_b; + float_vect3 z_n_b; + + int overloadcounter = 19; + + /* initialize */ + attitude_blackmagic_init(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s sensor_combined_s_local = { .gyro_raw = {0}}; + struct vehicle_attitude_s att = {.roll = 0.0f, .pitch = 0.0f, .yaw = 0.0f, + .rollspeed = 0.0f, .pitchspeed = 0.0f, .yawspeed = 0.0f, + .R = {0}, .timestamp = 0}; + + uint64_t last_data = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + + /* rate-limit raw data updates to 200Hz */ + //orb_set_interval(sub_raw, 5); + + /* advertise attitude */ + int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + struct pollfd fds[] = { + { .fd = sub_raw, .events = POLLIN }, + }; + + // int paramcounter = 100; + + /* Main loop*/ + while (true) { + + /* wait for sensor update */ + int ret = poll(fds, 1, 5000); + + 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("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); + continue; + } + + orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local); + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + +//#if 0 + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + if (overloadcounter == 20) { + printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + overloadcounter = 0; + } + + overloadcounter++; + } + +//#endif +// now = hrt_absolute_time(); + /* filter values */ + attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b); + +// time_elapsed = hrt_absolute_time() - now; +// if (blubb == 20) +// { +// printf("Estimator: %lu\n", time_elapsed); +// blubb = 0; +// } +// blubb++; + +// if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data); + +// printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data); + // last_data = sensor_combined_s_local.timestamp; + + /*correct yaw */ +// euler.z = euler.z + M_PI; + + /* send out */ + + att.timestamp = sensor_combined_s_local.timestamp; + att.roll = euler.x; + att.pitch = euler.y; + att.yaw = euler.z + M_PI; + + if (att.yaw > 2.0f * ((float)M_PI)) { + att.yaw -= 2.0f * ((float)M_PI); + } + + att.rollspeed = rates.x; + att.pitchspeed = rates.y; + att.yawspeed = rates.z; + + att.R[0][0] = x_n_b.x; + att.R[0][1] = x_n_b.y; + att.R[0][2] = x_n_b.z; + // XXX add remaining entries + + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + } + + /* Should never reach here */ + return 0; +} + + diff --git a/apps/px4/attitude_estimator_bm/kalman.c b/apps/px4/attitude_estimator_bm/kalman.c new file mode 100644 index 000000000..e4ea7a417 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/kalman.c @@ -0,0 +1,115 @@ +/* + * kalman.c + * + * Created on: 01.12.2010 + * Author: Laurens Mackay + */ +#include "kalman.h" +//#include "mavlink_debug.h" + +void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], + m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], + m_elem x_aposteriori[], int gainfactorsteps) +{ + kalman->states = states; + kalman->measurements = measurements; + kalman->gainfactorsteps = gainfactorsteps; + kalman->gainfactor = 0; + + //Create all matrices that are persistent + kalman->a = matrix_create(states, states, a); + kalman->c = matrix_create(measurements, states, c); + kalman->gain_start = matrix_create(states, measurements, gain_start); + kalman->gain = matrix_create(states, measurements, gain); + kalman->x_apriori = matrix_create(states, 1, x_apriori); + kalman->x_aposteriori = matrix_create(states, 1, x_aposteriori); +} + +void kalman_predict(kalman_t *kalman) +{ + matrix_mult(kalman->a, kalman->x_aposteriori, kalman->x_apriori); +} + +void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]) +{ + //create matrices from inputs + matrix_t measurement = + matrix_create(kalman->measurements, 1, measurement_a); + matrix_t mask = matrix_create(kalman->measurements, 1, mask_a); + + //create temporary matrices + m_elem gain_start_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = + { }; + matrix_t gain_start_part = matrix_create(kalman->states, + kalman->measurements, gain_start_part_a); + + m_elem gain_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = + { }; + matrix_t gain_part = matrix_create(kalman->states, kalman->measurements, + gain_part_a); + + m_elem gain_sum_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = + { }; + matrix_t gain_sum = matrix_create(kalman->states, kalman->measurements, + gain_sum_a); + + m_elem error_a[KALMAN_MAX_MEASUREMENTS * 1] = + { }; + matrix_t error = matrix_create(kalman->measurements, 1, error_a); + + m_elem measurement_estimate_a[KALMAN_MAX_MEASUREMENTS * 1] = + { }; + matrix_t measurement_estimate = matrix_create(kalman->measurements, 1, + measurement_estimate_a); + + m_elem x_update_a[KALMAN_MAX_STATES * 1] = + { }; + matrix_t x_update = matrix_create(kalman->states, 1, x_update_a); + + + + //x(:,i+1)=xapriori+(gainfactor*[M_50(:,1) M(:,2)]+(1-gainfactor)*M_start)*(z-C*xapriori); + + + //est=C*xapriori; + matrix_mult(kalman->c, kalman->x_apriori, measurement_estimate); + //error=(z-C*xapriori) = measurement-estimate + matrix_sub(measurement, measurement_estimate, error); + matrix_mult_element(error, mask, error); + + kalman->gainfactor = kalman->gainfactor * (1.0f - 1.0f + / kalman->gainfactorsteps) + 1.0f * 1.0f / kalman->gainfactorsteps; + + + + matrix_mult_scalar(kalman->gainfactor, kalman->gain, gain_part); + + matrix_mult_scalar(1.0f - kalman->gainfactor, kalman->gain_start, + gain_start_part); + + matrix_add(gain_start_part, gain_part, gain_sum); + + //gain*(z-C*xapriori) + matrix_mult(gain_sum, error, x_update); + + //xaposteriori = xapriori + update + + matrix_add(kalman->x_apriori, x_update, kalman->x_aposteriori); + + +// static int i=0; +// if(i++==4){ +// i=0; +// float_vect3 out_kal; +// out_kal.x = M(gain_sum,0,1); +//// out_kal_z.x = z_measurement[1]; +// out_kal.y = M(gain_sum,1,1); +// out_kal.z = M(gain_sum,2,1); +// debug_vect("out_kal", out_kal); +// } +} + +m_elem kalman_get_state(kalman_t *kalman, int state) +{ + return M(kalman->x_aposteriori, state, 0); +} diff --git a/apps/px4/attitude_estimator_bm/kalman.h b/apps/px4/attitude_estimator_bm/kalman.h new file mode 100644 index 000000000..0a6a18505 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/kalman.h @@ -0,0 +1,35 @@ +/* + * kalman.h + * + * Created on: 01.12.2010 + * Author: Laurens Mackay + */ + +#ifndef KALMAN_H_ +#define KALMAN_H_ + +#include "matrix.h" + +#define KALMAN_MAX_STATES 12 +#define KALMAN_MAX_MEASUREMENTS 9 +typedef struct { + int states; + int measurements; + matrix_t a; + matrix_t c; + matrix_t gain_start; + matrix_t gain; + matrix_t x_apriori; + matrix_t x_aposteriori; + float gainfactor; + int gainfactorsteps; +} kalman_t; + +void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], + m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], + m_elem x_aposteriori[], int gainfactorsteps); +void kalman_predict(kalman_t *kalman); +void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]); +m_elem kalman_get_state(kalman_t *kalman, int state); + +#endif /* KALMAN_H_ */ diff --git a/apps/px4/attitude_estimator_bm/matrix.h b/apps/px4/attitude_estimator_bm/matrix.h new file mode 100644 index 000000000..613a2d081 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/matrix.h @@ -0,0 +1,156 @@ +/* + * matrix.h + * + * Created on: 18.11.2010 + * Author: Laurens Mackay + */ + +#ifndef MATRIX_H_ +#define MATRIX_H_ + +typedef float m_elem; + +typedef struct { + int rows; + int cols; + m_elem *a; +} matrix_t; + +typedef struct { + float x; + float y; + float z; +} float_vect3; + +#define M(m,i,j) m.a[m.cols*i+j] + +///* This is the datatype used for the math and non-type specific ops. */ +// +//matrix_t matrix_create(const int rows, const int cols, m_elem * a); +///* matrix C = matrix A + matrix B , both of size m x n */ +//void matrix_add(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = matrix A - matrix B , all of size m x n */ +//void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = matrix A x matrix B , A(a_rows x a_cols), B(a_cols x b_cols) */ +//void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c); +// +//void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c); +// +//void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c); +// +///* matrix C = A*B'*/ +//void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c); + + +static inline matrix_t matrix_create(const int rows, const int cols, m_elem *a) +{ + matrix_t ret; + ret.rows = rows; + ret.cols = cols; + ret.a = a; + return ret; +} + +static inline void matrix_add(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_add: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) + M(b, i, j); + } + + } +} + +static inline void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_sub: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) - M(b, i, j); + } + + } +} + +static inline void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || b.cols != c.cols || a.cols != b.rows) { + //debug_message_buffer("matrix_mult: Dimension mismatch"); + } + + for (int i = 0; i < a.rows; i++) { + for (int j = 0; j < b.cols; j++) { + M(c, i, j) = 0; + + for (int k = 0; k < a.cols; k++) { + M(c, i, j) += M(a, i, k) * M(b, k, j); + } + } + + } +} + +static inline void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c) +{ + + if (a.rows != c.rows || b.rows != c.cols || a.cols != b.cols) { + //debug_message_buffer("matrix_mult: Dimension mismatch"); + } + + for (int i = 0; i < a.rows; i++) { + for (int j = 0; j < b.cols; j++) { + M(c, i, j) = 0; + + for (int k = 0; k < a.cols; k++) { + M(c, i, j) += M(a, i, k) * M(b, j, k); + } + } + + } + +} + +static inline void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols) { + //debug_message_buffer("matrix_mult_scalar: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = f * M(a, i, j); + } + + } +} + + +static inline void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c) +{ + if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols + != c.cols) { + //debug_message_buffer("matrix_mult_element: Dimension mismatch"); + } + + for (int i = 0; i < c.rows; i++) { + for (int j = 0; j < c.cols; j++) { + M(c, i, j) = M(a, i, j) * M(b, i, j); + } + + } +} + + + +#endif /* MATRIX_H_ */ diff --git a/apps/px4/fmu/Makefile b/apps/px4/fmu/Makefile new file mode 100644 index 000000000..7f1f836e3 --- /dev/null +++ b/apps/px4/fmu/Makefile @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Interface driver for the PX4FMU board +# + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp new file mode 100644 index 000000000..6c7d9f742 --- /dev/null +++ b/apps/px4/fmu/fmu.cpp @@ -0,0 +1,452 @@ +/**************************************************************************** + * + * 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 Driver/configurator for the PX4 FMU multi-purpose port. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> + +#include <drivers/device/device.h> +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_gpio.h> + +#include <uORB/topics/actuator_controls.h> +#include <systemlib/mixer.h> + +#include <arch/board/up_pwm_servo.h> + +class FMUServo : public device::CDev +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_NONE + }; + FMUServo(Mode mode); + ~FMUServo(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int init(); + +private: + Mode _mode; + int _task; + int _t_actuators; + int _t_armed; + unsigned _num_outputs; + + volatile bool _task_should_exit; + bool _armed; + + MixMixer *_mixer[4]; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main(); +}; + +namespace { + +FMUServo *g_servo; + +} // namespace + +FMUServo::FMUServo(Mode mode) : + CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH), + _mode(mode), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _task_should_exit(false), + _armed(false) +{ + for (unsigned i = 0; i < 4; i++) + _mixer[i] = nullptr; +} + +FMUServo::~FMUServo() +{ + if (_task != -1) { + /* task should wake up every 100ms or so at least */ + _task_should_exit = true; + + unsigned i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 10) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + g_servo = nullptr; +} + +int +FMUServo::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; + + /* start the IO interface task */ + _task = task_create("fmuservo", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&FMUServo::task_main_trampoline, nullptr); + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +FMUServo::task_main_trampoline(int argc, char *argv[]) +{ + g_servo->task_main(); +} + +void +FMUServo::task_main() +{ + log("ready"); + + /* configure for PWM output */ + switch (_mode) { + case MODE_2PWM: + /* multi-port with flow control lines as PWM */ + /* XXX magic numbers */ + up_pwm_servo_init(0x3); + break; + case MODE_4PWM: + /* multi-port as 4 PWM outs */ + /* XXX magic numbers */ + up_pwm_servo_init(0xf); + break; + case MODE_NONE: + /* we should never get here... */ + break; + } + + /* subscribe to objects that we are interested in watching */ + _t_actuators = orb_subscribe(ORB_ID(actuator_controls)); + orb_set_interval(_t_actuators, 20); /* 50Hz update rate */ + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 100); /* 10Hz update rate */ + + struct pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + + /* loop until killed */ + while (!_task_should_exit) { + + /* sleep waiting for data, but no more than 100ms */ + int ret = ::poll(&fds[0], 2, 100); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + struct actuator_controls ac; + + /* get controls */ + orb_copy(ORB_ID(actuator_controls), _t_actuators, &ac); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* if the actuator is configured */ + if (_mixer[i] != nullptr) { + + /* mix controls to the actuator */ + float output = mixer_mix(_mixer[i], &ac.control[0]); + + /* scale for PWM output 900 - 2100us */ + up_pwm_servo_set(i, 1500 + (600 * output)); + } + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + struct actuator_armed aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PMW servo armed status */ + up_pwm_servo_arm(aa.armed); + } + } + + ::close(_t_actuators); + ::close(_t_armed); + + /* make sure servos are off */ + up_pwm_servo_deinit(); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case PWM_SERVO_ARM: + up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + int channel = cmd - PWM_SERVO_SET(0); + up_pwm_servo_set(channel, arg); + } else { + ret = -EINVAL; + } + break; + + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + /* FALLTHROUGH */ + case PWM_SERVO_GET(0): + case PWM_SERVO_GET(1): { + int channel = cmd - PWM_SERVO_SET(0); + *(servo_position_t *)arg = up_pwm_servo_get(channel); + break; + } + + default: + ret = -ENOTTY; + break; + } + return ret; +} + +namespace { + +enum PortMode { + PORT_FULL_GPIO, + PORT_FULL_SERIAL, + PORT_FULL_PWM, + PORT_GPIO_AND_SERIAL, + PORT_PWM_AND_SERIAL, + PORT_PWM_AND_GPIO, + PORT_MODE_UNSET +}; + +PortMode g_port_mode; + +int +fmu_new_mode(PortMode new_mode) +{ + int fd; + int ret = OK; + uint32_t gpio_bits; + FMUServo::Mode servo_mode; + + /* get hold of the GPIO configuration descriptor */ + fd = open(GPIO_DEVICE_PATH, 0); + if (fd < 0) + return -errno; + + /* start by tearing down any existing state and revert to all-GPIO-inputs */ + if (g_servo != nullptr) { + delete g_servo; + g_servo = nullptr; + } + + /* reset to all-inputs */ + ioctl(fd, GPIO_RESET, 0); + + gpio_bits = 0; + servo_mode = FMUServo::MODE_NONE; + + switch (new_mode) { + case PORT_FULL_GPIO: + case PORT_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT_FULL_SERIAL: + /* set all multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = FMUServo::MODE_4PWM; + break; + + case PORT_GPIO_AND_SERIAL: + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = FMUServo::MODE_2PWM; + /* set RX/TX multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = FMUServo::MODE_2PWM; + break; + } + + /* adjust GPIO config for serial mode(s) */ + if (gpio_bits != 0) + ioctl(fd, GPIO_SET_ALT_1, gpio_bits); + close(fd); + + /* create new PWM driver if required */ + if (servo_mode != FMUServo::MODE_NONE) { + g_servo = new FMUServo(servo_mode); + if (g_servo == nullptr) { + ret = -ENOMEM; + } else { + ret = g_servo->init(); + if (ret != OK) { + delete g_servo; + g_servo = nullptr; + } + } + } + + return ret; +} + +} // namespace + +extern "C" int fmu_main(int argc, char *argv[]); + +int +fmu_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNSET; + + /* + * Mode switches. + * + * XXX use getopt + */ + if (!strcmp(argv[1], "mode_gpio")) { + new_mode = PORT_FULL_GPIO; + } else if (!strcmp(argv[1], "mode_serial")) { + new_mode = PORT_FULL_SERIAL; + } else if (!strcmp(argv[1], "mode_pwm")) { + new_mode = PORT_FULL_PWM; + } else if (!strcmp(argv[1], "mode_gpio_serial")) { + new_mode = PORT_GPIO_AND_SERIAL; + } else if (!strcmp(argv[1], "mode_pwm_serial")) { + new_mode = PORT_PWM_AND_SERIAL; + } else if (!strcmp(argv[1], "mode_pwm_gpio")) { + new_mode = PORT_PWM_AND_GPIO; + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNSET) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return OK; + + /* switch modes */ + return fmu_new_mode(new_mode); + } + + /* test, etc. here */ + + fprintf(stderr, "FMU: unrecognised command, try:\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + return -EINVAL; +} diff --git a/apps/px4/px4io/driver/.context b/apps/px4/px4io/driver/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/px4io/driver/.context diff --git a/apps/px4/px4io/driver/Makefile b/apps/px4/px4io/driver/Makefile new file mode 100644 index 000000000..cbd942546 --- /dev/null +++ b/apps/px4/px4io/driver/Makefile @@ -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. +# +############################################################################ + +# +# Interface driver for the PX4IO board. +# + +APPNAME = px4io +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp new file mode 100644 index 000000000..c3f14e3a4 --- /dev/null +++ b/apps/px4/px4io/driver/px4io.cpp @@ -0,0 +1,560 @@ +/**************************************************************************** + * + * 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 Driver for the PX4IO board. + * + * PX4IO is connected via serial (or possibly some other interface at a later + * point). + * + * XXX current design is racy as all hell; need a locking strategy. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> +#include <unistd.h> +#include <fcntl.h> + +#include <arch/board/board.h> + +#include <drivers/device/device.h> +#include <drivers/drv_rc_input.h> +#include <drivers/drv_pwm_output.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/hx_stream.h> + +#include "../protocol.h" +#include "uploader.h" + +class PX4IO; + + +namespace +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +PX4IO *g_dev; + +} + + +class PX4IO_RC : public device::CDev +{ +public: + PX4IO_RC(); + ~PX4IO_RC(); + + virtual int init(); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + + friend class PX4IO; +protected: + void set_channels(unsigned count, const servo_position_t *data); + +private: + int _publication; + struct rc_input_values _input; +}; + +/* XXX this may conflict with the onboard PPM input */ +PX4IO_RC::PX4IO_RC() : + CDev("px4io_rc", RC_INPUT_DEVICE_PATH), + _publication(-1) +{ + for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { + _input.values[i] = 0; + } + _input.channel_count = 0; +} + +PX4IO_RC::~PX4IO_RC() +{ + if (_publication != -1) + ::close(_publication); +} + +int +PX4IO_RC::init() +{ + int ret; + + ret = CDev::init(); + + /* advertise ourselves as the RC input controller */ + if (ret == OK) { + _publication = orb_advertise(ORB_ID(input_rc), &_input); + if (_publication < 0) + ret = -errno; + } + + return ret; +} + +ssize_t +PX4IO_RC::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned channels = buflen / sizeof(rc_input_t); + rc_input_t *pdata = (rc_input_t *)buffer; + unsigned i; + + if (channels > PX4IO_INPUT_CHANNELS) + return -EIO; + + lock(); + for (i = 0; i < channels; i++) + pdata[i] = _input.values[i]; + unlock(); + + return i * sizeof(servo_position_t); +} + +void +PX4IO_RC::set_channels(unsigned count, const servo_position_t *data) +{ + + ASSERT(count <= PX4IO_INPUT_CHANNELS); + + /* convert incoming servo position values into 0-100 range */ + lock(); + for (unsigned i = 0; i < count; i++) { + rc_input_t chn; + + if (data[i] < 1000) { + chn = 0; + } else if (data[i] > 2000) { + chn = 100; + } else { + chn = (data[i] - 1000) / 10; + } + + _input.values[i] = chn; + } + _input.channel_count = count; + unlock(); + + /* publish to anyone that might be listening */ + if (_publication != -1) + orb_publish(ORB_ID(input_rc), _publication, &_input); + +} + +class PX4IO : public device::CDev +{ +public: + PX4IO(); + ~PX4IO(); + + virtual int init(); + + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + int _fd; + int _task; + PX4IO_RC *_rc; + + /** command to be sent to IO */ + struct px4io_command _next_command; + + /** RC channel input from IO */ + servo_position_t _rc_channel[PX4IO_INPUT_CHANNELS]; + int _rc_channel_count; + + volatile bool _armed; + volatile bool _task_should_exit; + + bool _send_needed; + + hx_stream_t _io_stream; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main(); + void io_recv(); + + static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received); + void rx_callback(const uint8_t *buffer, size_t bytes_received); + + void io_send(); +}; + +PX4IO::PX4IO() : + CDev("px4io", "/dev/px4io"), + _fd(-1), + _task(-1), + _rc(new PX4IO_RC), + _rc_channel_count(0), + _armed(false), + _task_should_exit(false), + _send_needed(false), + _io_stream(nullptr) +{ + /* set up the command we will use */ + _next_command.f2i_magic = F2I_MAGIC; + + /* we need this potentially before it could be set in px4io_main */ + g_dev = this; + + _debug_enabled = true; +} + +PX4IO::~PX4IO() +{ + if (_rc != nullptr) + delete _rc; + if (_task != -1) { + /* task should wake up every 100ms or so at least */ + _task_should_exit = true; + + unsigned i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 10) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + g_dev = nullptr; +} + +int +PX4IO::init() +{ + int ret; + + ASSERT(_task == -1); + + /* XXX send a who-are-you request */ + + /* XXX verify firmware/protocol version */ + + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) + return ret; + + /* start the IO interface task */ + _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +PX4IO::task_main_trampoline(int argc, char *argv[]) +{ + g_dev->task_main(); +} + +void +PX4IO::task_main() +{ + ASSERT(_fd == -1); + + log("ready"); + + /* open the serial port */ + _fd = ::open("/dev/ttyS2", O_RDWR | O_NONBLOCK); + if (_fd < 0) { + debug("failed to open serial port for IO: %d", errno); + _task = -1; + _exit(errno); + } + + /* protocol stream */ + _io_stream = hx_stream_init(_fd, &PX4IO::rx_callback_trampoline, this); + + perf_counter_t pc_tx_bytes = perf_alloc(PC_COUNT, "PX4IO frames transmitted"); + perf_counter_t pc_rx_bytes = perf_alloc(PC_COUNT, "PX4IO frames received"); + perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors"); + hx_stream_set_counters(_io_stream, pc_tx_bytes, pc_rx_bytes, pc_rx_errors); + + /* poll descriptor(s) */ + struct pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + /* loop handling received serial bytes */ + while (!_task_should_exit) { + + /* sleep waiting for data, but no more than 100ms */ + int ret = ::poll(&fds[0], 1, 100); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* if we timed out waiting, we should send an update */ + if (ret == 0) + _send_needed = true; + + /* if we have new data from IO, go handle it */ + if ((ret > 0) && (fds[0].revents & POLLIN)) + io_recv(); + + /* send an update to IO if required */ + if (_send_needed) { + _send_needed = false; + io_send(); + } + } + if (_io_stream != nullptr) + hx_stream_free(_io_stream); + ::close(_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +void +PX4IO::io_recv() +{ + uint8_t c; + + /* handle bytes from IO */ + while (::read(_fd, &c, 1) == 1) + hx_stream_rx(_io_stream, c); +} + +void +PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received) +{ + g_dev->rx_callback((const uint8_t *)buffer, bytes_received); +} + +void +PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) +{ + const struct px4io_report *rep = (const struct px4io_report *)buffer; + + /* sanity-check the received frame size */ + if (bytes_received != sizeof(struct px4io_report)) + return; + + lock(); + + /* pass RC input data to the driver */ + if (_rc != nullptr) + _rc->set_channels(rep->channel_count, &rep->rc_channel[0]); + _armed = rep->armed; + + /* send an update frame */ + _send_needed = true; + + unlock(); + +} + +void +PX4IO::io_send() +{ + lock(); + + /* send packet to IO while we're guaranteed it won't change */ + hx_stream_send(_io_stream, &_next_command, sizeof(_next_command)); + + unlock(); +} + +ssize_t +PX4IO::write(struct file *filp, const char *buffer, size_t len) +{ + unsigned channels = len / sizeof(servo_position_t); + servo_position_t *pdata = (servo_position_t *)buffer; + unsigned i; + + if (channels > PX4IO_OUTPUT_CHANNELS) + return -EIO; + + lock(); + for (i = 0; i < channels; i++) + _next_command.servo_command[i] = pdata[i]; + unlock(); + + return i * sizeof(servo_position_t); +} + +int +PX4IO::ioctl(struct file *filep, int cmd, unsigned long arg) +{ + int ret = -ENOTTY; + + lock(); + + /* regular ioctl? */ + switch (cmd) { + case PWM_SERVO_ARM: + _next_command.arm_ok = true; + ret = 0; + break; + + case PWM_SERVO_DISARM: + _next_command.arm_ok = false; + ret = 0; + break; + + default: + /* channel set? */ + if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PX4IO_OUTPUT_CHANNELS))) { + /* XXX sanity-check value? */ + _next_command.servo_command[cmd - PWM_SERVO_SET(0)] = arg; + ret = 0; + break; + } + + /* channel get? */ + if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PX4IO_INPUT_CHANNELS))) { + int channel = cmd - PWM_SERVO_GET(0); + + /* currently no data for this channel */ + if (channel >= _rc_channel_count) { + ret = -ERANGE; + break; + } + + *(servo_position_t *)arg = _rc_channel[channel]; + ret = 0; + break; + } + + /* not a recognised value */ + ret = -ENOTTY; + } + unlock(); + + return ret; +} + +extern "C" __EXPORT int px4io_main(int argc, char *argv[]); + +int +px4io_main(int argc, char *argv[]) +{ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + fprintf(stderr, "PX4IO: already loaded\n"); + return -EBUSY; + } + + /* create the driver - it will set g_dev */ + (void)new PX4IO; + + if (g_dev == nullptr) { + fprintf(stderr, "PX4IO: driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "PX4IO: driver init failed\n"); + delete g_dev; + return -EIO; + } + + return OK; + } + + if (!strcmp(argv[1], "update")) { + PX4IO_Uploader *up; + const char *fn[3]; + + /* work out what we're uploading... */ + if (argc > 2) { + fn[0] = argv[2]; + fn[1] = nullptr; + } else { + fn[0] = "/fs/microsd/px4io.bin"; + fn[1] = "/etc/px4io.bin"; + fn[2] = nullptr; + } + + up = new PX4IO_Uploader; + int ret = up->upload(&fn[0]); + delete up; + + switch (ret) { + case OK: + break; + case -ENOENT: + fprintf(stderr, "PX4IO firmware file '%s' not found\n", fn); + break; + case -EEXIST: + case -EIO: + fprintf(stderr, "error updating PX4IO - check that bootloader mode is enabled\n"); + break; + case -EINVAL: + fprintf(stderr, "verify failed - retry the update\n"); + break; + case -ETIMEDOUT: + fprintf(stderr, "timed out waiting for bootloader - power-cycle and try again\n"); + default: + fprintf(stderr, "unexpected error %d\n", ret); + break; + } + return ret; + } + + + + printf("need a verb, only support 'start'\n"); + return ERROR; +} diff --git a/apps/px4/px4io/driver/uploader.cpp b/apps/px4/px4io/driver/uploader.cpp new file mode 100644 index 000000000..7d6a9e171 --- /dev/null +++ b/apps/px4/px4io/driver/uploader.cpp @@ -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 Firmware uploader for PX4IO + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> +#include <stdarg.h> +#include <unistd.h> +#include <fcntl.h> +#include <poll.h> + +#include "uploader.h" + +PX4IO_Uploader::PX4IO_Uploader() : + _io_fd(-1), + _fw_fd(-1) +{ +} + +PX4IO_Uploader::~PX4IO_Uploader() +{ +} + +int +PX4IO_Uploader::upload(const char *filenames[]) +{ + int ret; + + _io_fd = open("/dev/ttyS2", O_RDWR); + if (_io_fd < 0) { + log("could not open interface"); + return -errno; + } + + /* look for the bootloader */ + ret = sync(); + if (ret != OK) { + /* this is immediately fatal */ + log("bootloader not responding"); + return -EIO; + } + + for (unsigned i = 0; filenames[i] != nullptr; i++) { + _fw_fd = open(filenames[i], O_RDONLY); + + if (_fw_fd < 0) { + log("failed to open %s", filenames[i]); + continue; + } + log("using firmware from %s", filenames[i]); + break; + } + if (_fw_fd == -1) + return -ENOENT; + + /* do the usual program thing - allow for failure */ + for (unsigned retries = 0; retries < 1; retries++) { + if (retries > 0) { + log("retrying update..."); + ret = sync(); + if (ret != OK) { + /* this is immediately fatal */ + log("bootloader not responding"); + return -EIO; + } + } + + ret = erase(); + if (ret != OK) { + log("erase failed"); + continue; + } + ret = program(); + if (ret != OK) { + log("program failed"); + continue; + } + ret = verify(); + if (ret != OK) { + log("verify failed"); + continue; + } + ret = reboot(); + if (ret != OK) { + log("reboot failed"); + return ret; + } + log("update complete"); + + ret = OK; + break; + } + + close(_fw_fd); + return ret; +} + +int +PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) +{ + struct pollfd fds[1]; + + fds[0].fd = _io_fd; + fds[0].events = POLLIN; + + /* wait 100 ms for a character */ + int ret = ::poll(&fds[0], 1, timeout); + if (ret < 1) { + //log("poll timeout %d", ret); + return -ETIMEDOUT; + } + + read(_io_fd, &c, 1); + //log("recv 0x%02x", c); + return OK; +} + +int +PX4IO_Uploader::recv(uint8_t *p, unsigned count) +{ + while (count--) { + int ret = recv(*p++); + if (ret != OK) + return ret; + } + return OK; +} + +void +PX4IO_Uploader::drain() +{ + uint8_t c; + int ret; + + do { + ret = recv(c, 10); + //log("discard 0x%02x", c); + } while(ret == OK); +} + +int +PX4IO_Uploader::send(uint8_t c) +{ + //log("send 0x%02x", c); + if (write(_io_fd, &c, 1) != 1) + return -errno; + return OK; +} + +int +PX4IO_Uploader::send(uint8_t *p, unsigned count) +{ + while (count--) { + int ret = send(*p++); + if (ret != OK) + return ret; + } + return OK; +} + +int +PX4IO_Uploader::get_sync(unsigned timeout) +{ + uint8_t c[2]; + int ret; + + ret = recv(c[0], timeout); + if (ret != OK) + return ret; + ret = recv(c[1], timeout); + if (ret != OK) + return ret; + if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { + log("bad sync 0x%02x,0x%02x", c[0], c[1]); + return -EIO; + } + return OK; +} + +int +PX4IO_Uploader::sync() +{ + drain(); + /* complete any pending program operation */ + for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) + send(0); + send(PROTO_GET_SYNC); + send(PROTO_EOC); + return get_sync(); +} + +int +PX4IO_Uploader::get_info(int param, uint32_t &val) +{ + int ret; + + send(PROTO_GET_DEVICE); + send(param); + send(PROTO_EOC); + + ret = recv((uint8_t *)&val, sizeof(val)); + if (ret != OK) + return ret; + return get_sync(); +} + +int +PX4IO_Uploader::erase() +{ + log("erase..."); + send(PROTO_CHIP_ERASE); + send(PROTO_EOC); + return get_sync(10000); /* allow 10s timeout */ +} + +int +PX4IO_Uploader::program() +{ + uint8_t file_buf[PROG_MULTI_MAX]; + ssize_t count; + int ret; + + log("program..."); + lseek(_fw_fd, 0, SEEK_SET); + + while (true) { + /* get more bytes to program */ + //log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR)); + count = read(_fw_fd, file_buf, sizeof(file_buf)); + if (count == 0) + return OK; + if (count < 0) + return -errno; + ASSERT((count % 4) == 0); + + send(PROTO_PROG_MULTI); + send(count); + send(&file_buf[0], count); + send(PROTO_EOC); + + ret = get_sync(1000); + if (ret != OK) + return ret; + } +} + +int +PX4IO_Uploader::verify() +{ + uint8_t file_buf[PROG_MULTI_MAX]; + ssize_t count; + int ret; + + log("verify..."); + lseek(_fw_fd, 0, SEEK_SET); + + send(PROTO_CHIP_VERIFY); + send(PROTO_EOC); + ret = get_sync(); + if (ret != OK) + return ret; + + while (true) { + /* get more bytes to verify */ + int base = (int)lseek(_fw_fd, 0, SEEK_CUR); + count = read(_fw_fd, file_buf, sizeof(file_buf)); + if (count == 0) + break; + if (count < 0) + return -errno; + ASSERT((count % 4) == 0); + + send(PROTO_READ_MULTI); + send(count); + send(PROTO_EOC); + for (ssize_t i = 0; i < count; i++) { + uint8_t c; + + ret = recv(c); + if (ret != OK) { + log("%d: got %d waiting for bytes", ret); + return ret; + } + + if (c != file_buf[i]) { + log("%d: got 0x%02x expected 0x%02x", base + i, c, file_buf[i]); + return -EINVAL; + } + } + ret = get_sync(); + if (ret != OK) { + log("timeout waiting for post-verify sync"); + return ret; + } + } + return OK; +} + +int +PX4IO_Uploader::reboot() +{ + send(PROTO_REBOOT); + send(PROTO_EOC); + + return OK; +} + +int +PX4IO_Uploader::compare(bool &identical) +{ + uint32_t file_vectors[15]; + uint32_t fw_vectors[15]; + int ret; + + lseek(_fw_fd, 0, SEEK_SET); + ret = read(_fw_fd, &file_vectors[0], sizeof(file_vectors)); + + send(PROTO_CHIP_VERIFY); + send(PROTO_EOC); + ret = get_sync(); + if (ret != OK) + return ret; + + send(PROTO_READ_MULTI); + send(sizeof(fw_vectors)); + send(PROTO_EOC); + ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors)); + if (ret != OK) + return ret; + + identical = (memcmp(&file_vectors[0], &fw_vectors[0], sizeof(file_vectors))) ? true : false; + + return OK; +} + +void +PX4IO_Uploader::log(const char *fmt, ...) +{ + va_list ap; + + printf("[PX4IO] "); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +}
\ No newline at end of file diff --git a/apps/px4/px4io/driver/uploader.h b/apps/px4/px4io/driver/uploader.h new file mode 100644 index 000000000..8d41992f8 --- /dev/null +++ b/apps/px4/px4io/driver/uploader.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 Firmware uploader for PX4IO + */ + +#ifndef _PX4IO_UPLOADER_H +#define _PX4IO_UPLOADER_H value + +#include <stdint.h> +#include <stdbool.h> + + +class PX4IO_Uploader +{ +public: + PX4IO_Uploader(); + ~PX4IO_Uploader(); + + int upload(const char *filenames[]); + +private: + enum { + + PROTO_NOP = 0x00, + PROTO_OK = 0x10, + PROTO_FAILED = 0x11, + PROTO_INSYNC = 0x12, + PROTO_EOC = 0x20, + PROTO_GET_SYNC = 0x21, + PROTO_GET_DEVICE = 0x22, + PROTO_CHIP_ERASE = 0x23, + PROTO_CHIP_VERIFY = 0x24, + PROTO_PROG_MULTI = 0x27, + PROTO_READ_MULTI = 0x28, + PROTO_REBOOT = 0x30, + + INFO_BL_REV = 1, /**< bootloader protocol revision */ + BL_REV = 2, /**< 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; + + void log(const char *fmt, ...); + + int recv(uint8_t &c, unsigned timeout = 1000); + int recv(uint8_t *p, unsigned count); + void drain(); + int send(uint8_t c); + int send(uint8_t *p, unsigned count); + int get_sync(unsigned timeout = 1000); + int sync(); + int get_info(int param, uint32_t &val); + int erase(); + int program(); + int verify(); + int reboot(); + int compare(bool &identical); +}; + +#endif
\ No newline at end of file diff --git a/apps/px4/px4io/protocol.h b/apps/px4/px4io/protocol.h new file mode 100644 index 000000000..c186c5b86 --- /dev/null +++ b/apps/px4/px4io/protocol.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 PX4FMU <-> PX4IO messaging protocol. + * + * This initial version of the protocol is very simple; each side transmits a + * complete update with each frame. This avoids the sending of many small + * messages and the corresponding complexity involved. + */ + +/* + * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL + * TREES ARE MERGED. + */ + +#define PX4IO_OUTPUT_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_RELAY_CHANNELS 2 + +#pragma pack(push,1) + +/* command from FMU to IO */ +struct px4io_command { + uint16_t f2i_magic; +#define F2I_MAGIC 0x636d + + uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; + bool relay_state[PX4IO_RELAY_CHANNELS]; + bool arm_ok; +}; + +/* report from IO to FMU */ +struct px4io_report { + uint16_t i2f_magic; +#define I2F_MAGIC 0x7570 + + uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; + bool armed; + uint8_t channel_count; +} __attribute__((packed)); + +#pragma pack(pop) diff --git a/apps/px4/sensors_bringup/.context b/apps/px4/sensors_bringup/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/sensors_bringup/.context diff --git a/apps/px4/sensors_bringup/Makefile b/apps/px4/sensors_bringup/Makefile new file mode 100644 index 000000000..8867653a0 --- /dev/null +++ b/apps/px4/sensors_bringup/Makefile @@ -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. +# +############################################################################ + +# +# Makefile to build the sensor bringup tests +# + +APPNAME = sensors_bringup +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/sensors_bringup/bma180.c b/apps/px4/sensors_bringup/bma180.c new file mode 100644 index 000000000..6c4b9d483 --- /dev/null +++ b/apps/px4/sensors_bringup/bma180.c @@ -0,0 +1,209 @@ +/* + * Operations for the Bosch BMA180 3D Accelerometer + */ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <nuttx/spi.h> + +#include "sensors.h" + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define ADDR_CHIP_ID 0x00 +#define CHIP_ID 0x03 +#define ADDR_VERSION 0x01 + +#define ADDR_CTRL_REG0 0x0D +#define ADDR_CTRL_REG1 0x0E +#define ADDR_CTRL_REG2 0x0F +#define ADDR_BWTCS 0x20 +#define ADDR_CTRL_REG3 0x21 +#define ADDR_CTRL_REG4 0x22 +#define ADDR_OLSB1 0x35 + +#define ADDR_ACC_X_LSB 0x02 +#define ADDR_ACC_Z_MSB 0x07 +#define ADDR_TEMPERATURE 0x08 + +#define ADDR_STATUS_REG1 0x09 +#define ADDR_STATUS_REG2 0x0A +#define ADDR_STATUS_REG3 0x0B +#define ADDR_STATUS_REG4 0x0C + +#define ADDR_RESET 0x10 +#define SOFT_RESET 0xB6 + +#define ADDR_DIS_I2C 0x27 + +#define REG0_WRITE_ENABLE 0x10 + +#define BWTCS_LP_10HZ (0<<4) +#define BWTCS_LP_20HZ (1<<4) +#define BWTCS_LP_40HZ (2<<4) +#define BWTCS_LP_75HZ (3<<4) +#define BWTCS_LP_150HZ (4<<4) +#define BWTCS_LP_300HZ (5<<4) +#define BWTCS_LP_600HZ (6<<4) +#define BWTCS_LP_1200HZ (7<<4) + +#define RANGE_1G (0<<1) +#define RANGE_1_5G (1<<1) +#define RANGE_2G (2<<1) +#define RANGE_3G (3<<1) +#define RANGE_4G (4<<1) +#define RANGE_8G (5<<1) +#define RANGE_16G (6<<1) + +#define RANGEMASK 0x0E +#define BWMASK 0xF0 + + +static void +write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data) +{ + uint8_t cmd[2] = { address | DIR_WRITE, data }; + + SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true); + SPI_SNDBLOCK(spi, &cmd, sizeof(cmd)); + SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false); +} + +static uint8_t +read_reg(struct spi_dev_s *spi, uint8_t address) +{ + uint8_t cmd[2] = {address | DIR_READ, 0}; + uint8_t data[2]; + + SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true); + SPI_EXCHANGE(spi, cmd, data, sizeof(cmd)); + SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false); + + return data[1]; +} + +int +bma180_test_configure(struct spi_dev_s *spi) +{ + uint8_t id; + + id = read_reg(spi, ADDR_CHIP_ID); + uint8_t version = read_reg(spi, 0x01); + + if (id == CHIP_ID) + { + message("BMA180 SUCCESS: 0x%02x, version: %d\n", id, version); + } + else + { + message("BMA180 FAIL: 0x%02x\n", id); + } + //message("got id 0x%02x, expected ID 0x03\n", id); + + write_reg(spi, ADDR_RESET, SOFT_RESET); // page 48 + usleep(12000); // wait 10 ms, see page 49 + + // Configuring the BMA180 + + /* enable writing to chip config */ + uint8_t ctrl0 = read_reg(spi, ADDR_CTRL_REG0); + ctrl0 |= REG0_WRITE_ENABLE; + write_reg(spi, ADDR_CTRL_REG0, ctrl0); + + uint8_t disi2c = read_reg(spi, ADDR_DIS_I2C); // read + disi2c |= 0x01; // set bit0 to 1, SPI only + write_reg(spi, ADDR_DIS_I2C, disi2c); // Set spi, disable i2c, page 31 + + /* set bandwidth */ + uint8_t bwtcs = read_reg(spi, ADDR_BWTCS); + printf("bwtcs: %d\n", bwtcs); + bwtcs &= (~BWMASK); + bwtcs |= (BWTCS_LP_600HZ);// & BWMASK); + write_reg(spi, ADDR_BWTCS, bwtcs); + + /* set range */ + uint8_t olsb1 = read_reg(spi, ADDR_OLSB1); + printf("olsb1: %d\n", olsb1); + olsb1 &= (~RANGEMASK); + olsb1 |= (RANGE_4G);// & RANGEMASK); + write_reg(spi, ADDR_OLSB1, olsb1); + +// uint8_t reg3 = read_reg(spi, ADDR_CTRL_REG3); +// //reg3 &= 0xFD; // REset bit 1 enable interrupt +// //reg3 |= 0x02; // enable +// write_reg(spi, ADDR_CTRL_REG3, reg3); // + + /* block writing to chip config */ + ctrl0 = read_reg(spi, ADDR_CTRL_REG0); + ctrl0 &= (~REG0_WRITE_ENABLE); + printf("ctrl0: %d\n", ctrl0); + write_reg(spi, ADDR_CTRL_REG0, ctrl0); + + return 0; +} + +int +bma180_test_read(struct spi_dev_s *spi) +{ + + + + struct { /* status register and data as read back from the device */ + uint8_t cmd; + int16_t x; + int16_t y; + int16_t z; + uint8_t temp; + } __attribute__((packed)) report; + + report.x = 0; + report.y = 0; + report.z = 0; + +// uint8_t temp; +// uint8_t status1; +// uint8_t status2; +// uint8_t status3; +// uint8_t status4; + + report.cmd = ADDR_ACC_X_LSB | DIR_READ | ADDR_INCREMENT; + + //SPI_LOCK(spi, true); + //SPI_SELECT(spi, PX4_SPIDEV_ACCEL, true); + //SPI_EXCHANGE(spi, &report, &report, sizeof(report)); + //SPI_SELECT(spi, PX4_SPIDEV_ACCEL, false); + //SPI_LOCK(spi, false); + + report.x = read_reg(spi, ADDR_ACC_X_LSB); + report.x |= (read_reg(spi, ADDR_ACC_X_LSB+1) << 8); + report.y = read_reg(spi, ADDR_ACC_X_LSB+2); + report.y |= (read_reg(spi, ADDR_ACC_X_LSB+3) << 8); + report.z = read_reg(spi, ADDR_ACC_X_LSB+4); + report.z |= (read_reg(spi, ADDR_ACC_X_LSB+5) << 8); + report.temp = read_reg(spi, ADDR_ACC_X_LSB+6); + + // Collect status and remove two top bits + + uint8_t new_data = (report.x & 0x01) + (report.x & 0x01) + (report.x & 0x01); + report.x = (report.x >> 2); + report.y = (report.y >> 2); + report.z = (report.z >> 2); + + message("ACC: x: %d\ty: %d\tz: %d\ttemp: %d new: %d\n", report.x, report.y, report.z, report.temp, new_data); + usleep(2000); + + return 0; +} diff --git a/apps/px4/sensors_bringup/l3gd20.c b/apps/px4/sensors_bringup/l3gd20.c new file mode 100644 index 000000000..5bdc10e06 --- /dev/null +++ b/apps/px4/sensors_bringup/l3gd20.c @@ -0,0 +1,184 @@ +/* + * Operations for the l3g4200 + */ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <nuttx/spi.h> + +#include "sensors.h" + +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define ADDR_WHO_AM_I 0x0f +#define WHO_I_AM 0xd4 + +#define ADDR_CTRL_REG1 0x20 /* sample rate constants are in the public header */ +#define REG1_POWER_NORMAL (1<<3) +#define REG1_Z_ENABLE (1<<2) +#define REG1_Y_ENABLE (1<<1) +#define REG1_X_ENABLE (1<<0) + +#define ADDR_CTRL_REG2 0x21 +/* high-pass filter - usefulness TBD */ + +#define ADDR_CTRL_REG3 0x22 + +#define ADDR_CTRL_REG4 0x23 +#define REG4_BDU (1<<7) +#define REG4_BIG_ENDIAN (1<<6) +#define REG4_SPI_3WIRE (1<<0) + +#define ADDR_CTRL_REG5 0x24 +#define REG5_BOOT (1<<7) +#define REG5_FIFO_EN (1<<6) +#define REG5_HIGHPASS_ENABLE (1<<4) + +#define ADDR_REFERENCE 0x25 +#define ADDR_TEMPERATURE 0x26 + +#define ADDR_STATUS_REG 0x27 +#define STATUS_ZYXOR (1<<7) +#define SATAUS_ZOR (1<<6) +#define STATUS_YOR (1<<5) +#define STATUS_XOR (1<<4) +#define STATUS_ZYXDA (1<<3) +#define STATUS_ZDA (1<<2) +#define STATUS_YDA (1<<1) +#define STATUS_XDA (1<<0) + +#define ADDR_OUT_X 0x28 /* 16 bits */ +#define ADDR_OUT_Y 0x2A /* 16 bits */ +#define ADDR_OUT_Z 0x2C /* 16 bits */ + +#define ADDR_FIFO_CTRL 0x2e +#define FIFO_MODE_BYPASS (0<<5) +#define FIFO_MODE_FIFO (1<<5) +#define FIFO_MODE_STREAM (2<<5) +#define FIFO_MODE_STREAM_TO_FIFO (3<<5) +#define FIFO_MODE_BYPASS_TO_STREAM (4<<5) +#define FIFO_THRESHOLD_MASK 0x1f + +#define ADDR_FIFO_SRC 0x2f +#define FIFO_THREHSHOLD_OVER (1<<7) +#define FIFO_OVERRUN (1<<6) +#define FIFO_EMPTY (1<<5) + +#define L3G4200_RATE_100Hz ((0<<6) | (0<<4)) +#define L3G4200_RATE_200Hz ((1<<6) | (0<<4)) +#define L3G4200_RATE_400Hz ((2<<6) | (1<<4)) +#define L3G4200_RATE_800Hz ((3<<6) | (2<<4)) + +#define L3G4200_RANGE_250dps (0<<4) +#define L3G4200_RANGE_500dps (1<<4) +#define L3G4200_RANGE_2000dps (3<<4) + +static void +write_reg(struct spi_dev_s *spi, uint8_t address, uint8_t data) +{ + uint8_t cmd[2] = { address | DIR_WRITE, data }; + + SPI_SELECT(spi, PX4_SPIDEV_GYRO, true); + SPI_SNDBLOCK(spi, &cmd, sizeof(cmd)); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, false); +} + +static uint8_t +read_reg(struct spi_dev_s *spi, uint8_t address) +{ + uint8_t cmd[2] = {address | DIR_READ, 0}; + uint8_t data[2]; + + SPI_SELECT(spi, PX4_SPIDEV_GYRO, true); + SPI_EXCHANGE(spi, cmd, data, sizeof(cmd)); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, false); + + return data[1]; +} + +int +l3gd20_test_configure(struct spi_dev_s *spi) +{ + uint8_t id; + + id = read_reg(spi, ADDR_WHO_AM_I); + + if (id == WHO_I_AM) + { + message("L3GD20 SUCCESS: 0x%02x\n", id); + } + else + { + message("L3GD20 FAIL: 0x%02x\n", id); + } + + struct { /* status register and data as read back from the device */ + uint8_t cmd; + uint8_t temp; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } __attribute__((packed)) report; + + report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT; + + write_reg(spi, ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_reg(spi, ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(spi, ADDR_CTRL_REG5, 0); /* turn off FIFO mode */ + + write_reg(spi, ADDR_CTRL_REG4, ((3<<4) & 0x30) | REG4_BDU); + + + write_reg(spi, ADDR_CTRL_REG1, + (((2<<6) | (1<<4)) & 0xf0) | REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + + SPI_SELECT(spi, PX4_SPIDEV_GYRO, true); + SPI_EXCHANGE(spi, &report, &report, sizeof(report)); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, false); + + message("Init-read: gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z); + usleep(1000); + + //message("got id 0x%02x, expected ID 0xd4\n", id); + + return 0; +} + +int +l3gd20_test_read(struct spi_dev_s *spi) +{ + struct { /* status register and data as read back from the device */ + uint8_t cmd; + uint8_t temp; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } __attribute__((packed)) report; + + report.cmd = 0x26 | DIR_READ | ADDR_INCREMENT; + + SPI_LOCK(spi, true); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, true); + SPI_EXCHANGE(spi, &report, &report, sizeof(report)); + SPI_SELECT(spi, PX4_SPIDEV_GYRO, false); + SPI_LOCK(spi, false); + + message("gyro: x: %d\ty: %d\tz: %d\n", report.x, report.y, report.z); + usleep(1000); + return 0; +} diff --git a/apps/px4/sensors_bringup/sensors.h b/apps/px4/sensors_bringup/sensors.h new file mode 100644 index 000000000..5fc2fbb08 --- /dev/null +++ b/apps/px4/sensors_bringup/sensors.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef __APPS_PX4_SENSORS_H +#define __APPS_PX4_SENSORS_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/spi.h> +#include <nuttx/i2c.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +int l3gd20_test_configure(struct spi_dev_s *spi); +int l3gd20_test_read(struct spi_dev_s *spi); +int bma180_test_configure(struct spi_dev_s *spi); +int bma180_test_read(struct spi_dev_s *spi); +int bma180_test(struct spi_dev_s *spi); +int hmc5883l_test(struct i2c_dev_s *i2c); + +#endif /* __APPS_PX4_SENSORS_H */ diff --git a/apps/px4/sensors_bringup/sensors_main.c b/apps/px4/sensors_bringup/sensors_main.c new file mode 100644 index 000000000..3aa21ae14 --- /dev/null +++ b/apps/px4/sensors_bringup/sensors_main.c @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <nuttx/spi.h> +#include <nuttx/i2c.h> + +#include "sensors.h" + +__EXPORT int sensors_bringup_main(int argc, char *argv[]); + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: user_start/adc_main + ****************************************************************************/ + +int sensors_bringup_main(int argc, char *argv[]) +{ + struct spi_dev_s *spi; + int result = -1; + + spi = up_spiinitialize(1); + if (!spi) { + message("Failed to initialize SPI port 1\n"); + goto out; + } + + struct i2c_dev_s *i2c; + i2c = up_i2cinitialize(2); + if (!i2c) { + message("Failed to initialize I2C bus 2\n"); + goto out; + } + + int ret; + +#define EEPROM_ADDRESS 0x50 +#define HMC5883L_ADDRESS 0x1E + + //uint8_t devaddr = EEPROM_ADDRESS; + + I2C_SETFREQUENCY(i2c, 100000); +// +// uint8_t subaddr = 0x00; +// int ret = 0; +// +// // ATTEMPT HMC5883L CONFIG +// I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7); +// subaddr = 0x02; // mode register +// ret = I2C_WRITE(i2c, &subaddr, 0); +// if (ret < 0) +// { +// message("I2C_WRITE failed: %d\n", ret); +// } +// else +// { +// message("I2C_WRITE SUCCEEDED: %d\n", ret); +// } + + //fflush(stdout); +// +// +// + + +#define STATUS_REGISTER 0x09 // Of HMC5883L + + // ATTEMPT HMC5883L WRITE + I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7); + uint8_t cmd = 0x09; + uint8_t status_id[4] = {0, 0, 0, 0}; + + + ret = I2C_WRITEREAD(i2c, &cmd, 1, status_id, 4); + + if (ret >= 0 && status_id[1] == 'H' && status_id[2] == '4' && status_id[3] == '3') + { + message("HMC5883L identified, device status: %d\n", status_id[0]); + } else { + message("HMC5883L identification failed: %d\n", ret); + } + +#define HMC5883L_ADDR_CONF_A 0x00 +#define HMC5883L_ADDR_CONF_B 0x01 +#define HMC5883L_ADDR_MODE 0x02 + +#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */ +#define HMC5883L_AVERAGING_2 (1 << 5) +#define HMC5883L_AVERAGING_4 (2 << 5) +#define HMC5883L_AVERAGING_8 (3 << 5) + +#define HMC5883L_RATE_75HZ (6 << 2) /* 75 Hz */ + +#define HMC5883L_RANGE_0_88GA (0 << 5) + + uint8_t rate_cmd[] = {HMC5883L_ADDR_CONF_A, HMC5883L_RATE_75HZ | HMC5883L_AVERAGING_8}; + ret = I2C_WRITE(i2c, rate_cmd, sizeof(rate_cmd)); + message("Wrote %d into register 0x00 of HMC, result: %d (0 = success)\n", HMC5883L_RATE_75HZ | HMC5883L_AVERAGING_8, ret); + + uint8_t range_cmd[] = {HMC5883L_ADDR_CONF_B, HMC5883L_RANGE_0_88GA}; + ret = I2C_WRITE(i2c, range_cmd, sizeof(range_cmd)); + message("Wrote %d into register 0x01 of HMC, result: %d (0 = success)\n", HMC5883L_RANGE_0_88GA, ret); + + // Set HMC into continous mode + // First write address, then value + uint8_t cont_address[] = {HMC5883L_ADDR_MODE, 0x00}; + ret = I2C_WRITE(i2c, cont_address, sizeof(cont_address)); + + message("Wrote 0x00 into register 0x02 of HMC, result: %d (0 = success)\n", ret); + + + // ATTEMPT HMC5883L READ + int h = 0; + + I2C_SETADDRESS(i2c, HMC5883L_ADDRESS, 7); + for (h = 0; h < 5; h++) + { + + cont_address[0] = HMC5883L_ADDR_MODE; + cont_address[1] = 0x01; + ret = I2C_WRITE(i2c, cont_address, sizeof(cont_address)); + + message("Wrote 0x01 into register 0x02 of HMC, result: %d (0 = success)\n", ret); + + usleep(100000); + + cont_address[1] = 0x00; + uint8_t dummy; + ret = I2C_WRITEREAD(i2c, cont_address, sizeof(cont_address), &dummy, 1); + + message("Wrote 0x00 into register 0x02 of HMC, result: %d (0 = success)\n", ret); + + usleep(100000); + + + int16_t hmc5883l_data[3] = {0, 0, 0}; + uint8_t data_address = 0x03; + uint8_t* data_ptr = (uint8_t*)hmc5883l_data; + ret = I2C_WRITEREAD(i2c, &data_address, 1, data_ptr, 6); + if (ret < 0) + { + message("HMC5883L READ failed: %d\n", ret); + } + else + { + // mask out top four bits as only 12 bits are valid + hmc5883l_data[0] &= 0xFFF; + hmc5883l_data[1] &= 0xFFF; + hmc5883l_data[2] &= 0xFFF; + + message("HMC5883L READ SUCCEEDED: %d, val: %d %d %d\n", ret, hmc5883l_data[0], hmc5883l_data[1], hmc5883l_data[2]); + uint8_t hmc_status; + ret = I2C_WRITEREAD(i2c, &cmd, 1, &hmc_status, 1); + + message("\t status: %d\n", hmc_status); + } + } + + + // Possible addresses: 0x77 or 0x76 +#define MS5611_ADDRESS_1 0x76 +#define MS5611_ADDRESS_2 0x77 + I2C_SETADDRESS(i2c, MS5611_ADDRESS_1, 7); + // Reset cmd + uint8_t ms5611_cmd[2] = {0x00, 0x1E}; + ret = I2C_WRITE(i2c, ms5611_cmd, 2); + if (ret < 0) + { + message("MS5611 #1 WRITE failed: %d\n", ret); + } + else + { + message("MS5611 #1 WRITE SUCCEEDED: %d\n", ret); + } + + fflush(stdout); + + I2C_SETADDRESS(i2c, MS5611_ADDRESS_2, 7); + ret = I2C_WRITE(i2c, ms5611_cmd, 2); + if (ret < 0) + { + message("MS5611 #2 WRITE failed: %d\n", ret); + } + else + { + message("MS5611 #2 WRITE SUCCEEDED: %d\n", ret); + } + + fflush(stdout); + + + // Wait for reset to complete (10 ms nominal, wait: 100 ms) + usleep(100000); + + // Read PROM data + uint8_t prom_buf[2] = {0,1}; + + uint16_t calibration[6]; + + int i = 0; + + prom_buf[0] = 0xA2 + (i*2); + + struct i2c_msg_s msgv[2] = { + { + .addr = MS5611_ADDRESS_2, + .flags = 0, + .buffer = prom_buf, + .length = 1 + }, + { + .addr = MS5611_ADDRESS_2, + .flags = I2C_M_READ, + .buffer = prom_buf, + .length = 1 + } + }; + + calibration[i] = prom_buf[0]*256; + calibration[i]+= prom_buf[1]; + + int retval; + + if ( (retval = I2C_TRANSFER(i2c, msgv, 2)) == OK ) + { + printf("SUCCESS ACCESSING PROM OF MS5611: %d, value C1: %d\n", retval, (int)calibration[0]); + } + else + { + printf("FAIL ACCESSING PROM OF MS5611\n"); + } + + + + + // TESTING CODE, EEPROM READ/WRITE + uint8_t val[1] = {10}; + int retval_eeprom; + uint8_t eeprom_subaddr[2] = {0, 0}; + + struct i2c_msg_s msgv_eeprom[2] = { + { + .addr = EEPROM_ADDRESS, + .flags = 0, + .buffer = eeprom_subaddr, + .length = 2 + }, + { + .addr = EEPROM_ADDRESS, + .flags = I2C_M_READ, + .buffer = val, + .length = 1 + } + }; + + val[0] = 5; + + if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom, 2)) == OK ) + { + printf("SUCCESS READING EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]); + } + else + { + printf("FAIL READING EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]); + } + + // Increment val + val[0] = val[0] + 1; + + struct i2c_msg_s msgv_eeprom_write[2] = { + { + .addr = EEPROM_ADDRESS, + .flags = I2C_M_NORESTART, + .buffer = eeprom_subaddr, + .length = 2 + }, + { + .addr = EEPROM_ADDRESS, + .flags = I2C_M_NORESTART, + .buffer = val, + .length = 1 + } + }; + + + if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom_write, 2)) == OK ) + { + printf("SUCCESS WRITING EEPROM: %d\n", retval_eeprom); + } + + usleep(10000); + + struct i2c_msg_s msgv_eeprom2[2] = { + { + .addr = EEPROM_ADDRESS, + .flags = 0, + .buffer = eeprom_subaddr, + .length = 2 + }, + { + .addr = EEPROM_ADDRESS, + .flags = I2C_M_READ, + .buffer = val, + .length = 1 + } + }; + + val[0] = 5; + + + if ( (retval_eeprom = I2C_TRANSFER(i2c, msgv_eeprom2, 2)) == OK ) + { + printf("SUCCESS READING WRITE RESULT EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]); + } + else + { + printf("FAIL READING WRITE RESULT EEPROM: %d, value: %d\n", retval_eeprom, (int)val[0]); + } + + // Configure sensors + l3gd20_test_configure(spi); + bma180_test_configure(spi); + + for (int i = 0; i < 3; i++) + { + l3gd20_test_read(spi); + bma180_test_read(spi); + printf("# %d of 10\n", i+1); + usleep(50000); + } + + + out: + msgflush(); + return result; +} diff --git a/apps/px4/tests/.context b/apps/px4/tests/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/px4/tests/.context diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile new file mode 100644 index 000000000..41979f85a --- /dev/null +++ b/apps/px4/tests/Makefile @@ -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. +# +############################################################################ + +# +# Makefile to build assorted test cases +# + +APPNAME = tests +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c new file mode 100644 index 000000000..7795f07ac --- /dev/null +++ b/apps/px4/tests/test_adc.c @@ -0,0 +1,211 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/arch.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +#include <nuttx/analog/adc.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_gpio + ****************************************************************************/ + +int test_adc(int argc, char *argv[]) +{ + int fd0; + int ret = 0; + //struct adc_msg_s sample[4],sample2[4],sample3[4],sample4[4],sample5[4],sample6[4],sample7[4],sample8[4],sample9[4]; + + struct adc_msg4_s { + uint8_t am_channel1; /* The 8-bit ADC Channel */ + int32_t am_data1; /* ADC convert result (4 bytes) */ + uint8_t am_channel2; /* The 8-bit ADC Channel */ + int32_t am_data2; /* ADC convert result (4 bytes) */ + uint8_t am_channel3; /* The 8-bit ADC Channel */ + int32_t am_data3; /* ADC convert result (4 bytes) */ + uint8_t am_channel4; /* The 8-bit ADC Channel */ + int32_t am_data4; /* ADC convert result (4 bytes) */ + } __attribute__((__packed__));; + + struct adc_msg4_s sample1[4], sample2[4]; + + size_t readsize; + ssize_t nbytes, nbytes2; + int i; + int j; + int errval; + + for (j = 0; j < 1; j++) { + char name[11]; + sprintf(name, "/dev/adc%d", j); + fd0 = open(name, O_RDONLY | O_NONBLOCK); + + if (fd0 < 0) { + printf("ADC: %s open fail\n", name); + return ERROR; + + } else { + printf("Opened %s successfully\n", name); + } + + /* first adc read round */ + readsize = 4 * sizeof(struct adc_msg_s); + up_udelay(10000);//microseconds + nbytes = read(fd0, sample1, readsize); + up_udelay(10000);//microseconds + nbytes2 = read(fd0, sample2, readsize); +// nbytes2 = read(fd0, sample3, readsize); +// nbytes2 = read(fd0, sample4, readsize); +// nbytes2 = read(fd0, sample5, readsize); +// nbytes2 = read(fd0, sample6, readsize); +// nbytes2 = read(fd0, sample7, readsize); +// nbytes2 = read(fd0, sample8, readsize); + //nbytes2 = read(fd0, sample9, readsize); + + /* Handle unexpected return values */ + + if (nbytes < 0) { + errval = errno; + + if (errval != EINTR) { + message("read %s failed: %d\n", + name, errval); + errval = 3; + goto errout_with_dev; + } + + message("\tInterrupted read...\n"); + + } else if (nbytes == 0) { + message("\tNo data read, Ignoring\n"); + } + + /* Print the sample data on successful return */ + + else { + int nsamples = nbytes / sizeof(struct adc_msg_s); + + if (nsamples * sizeof(struct adc_msg_s) != nbytes) { + message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n", + nbytes, sizeof(struct adc_msg_s)); + + } else { + message("Sample:"); + + for (i = 0; i < 1 ; i++) { + message("%d: channel: %d value: %d\n", + i, sample1[i].am_channel1, sample1[i].am_data1); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i, sample1[i].am_channel2, sample1[i].am_data2); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i, sample1[i].am_channel3, sample1[i].am_data3); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i, sample1[i].am_channel4, sample1[i].am_data4); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i + 1, sample2[i].am_channel1, sample2[i].am_data1); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i + 1, sample2[i].am_channel2, sample2[i].am_data2); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i + 1, sample2[i].am_channel3, sample2[i].am_data3); + message("Sample:"); + message("%d: channel: %d value: %d\n", + i + 1, sample2[i].am_channel4, sample2[i].am_data4); +// message("%d: channel: %d value: %d\n", +// i, sample9[i].am_channel, sample9[i].am_data); + } + } + } + } + + printf("\t ADC test successful.\n"); + +errout_with_dev: + + if (fd0 != 0) close(fd0); + + return ret; +} diff --git a/apps/px4/tests/test_eeproms.c b/apps/px4/tests/test_eeproms.c new file mode 100644 index 000000000..29ca8267f --- /dev/null +++ b/apps/px4/tests/test_eeproms.c @@ -0,0 +1,328 @@ +/**************************************************************************** + * px4/eeproms/test_eeproms.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include "tests.h" + +#include <arch/board/drv_eeprom.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int onboard_eeprom(int argc, char *argv[]); +static int baseboard_eeprom(int argc, char *argv[]); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct { + const char *name; + const char *path; + int (* test)(int argc, char *argv[]); +} eeproms[] = { + {"onboard_eeprom", "/dev/eeprom", onboard_eeprom}, + {"baseboard_eeprom", "/dev/baseboard_eeprom", baseboard_eeprom}, + {NULL, NULL, NULL} +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int +onboard_eeprom(int argc, char *argv[]) +{ + printf("\tonboard_eeprom: test start\n"); + fflush(stdout); + + int fd; + uint8_t buf1[210] = {' ', 'P', 'X', '4', ' ', 'E', 'E', 'P', 'R', 'O', 'M', ' ', 'T', 'E', 'S', 'T', ' '}; + int ret; + bool force_write = false; + if (strcmp(argv[0], "jig") == 0) force_write = true; + + /* fill with spaces */ + //memset(buf1+16, 'x', sizeof(buf1-16)); + + /* fill in some magic values at magic positions */ + buf1[63] = 'E'; + buf1[64] = 'S'; + buf1[127] = 'F'; + buf1[128] = 'T'; + + /* terminate string */ + buf1[sizeof(buf1) - 1] = '\0'; + + fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); + + if (fd < 0) { + printf("onboard eeprom: open fail\n"); + return ERROR; + } + + /* read data */ + ret = read(fd, buf1, 1); + + if (ret != 1) { + printf("\tonboard eeprom: ERROR: reading first byte fail: %d\n", ret); + + switch (-ret) { + case EPERM: + printf("\treason: %s\n", EPERM_STR); + break; + + case ENOENT: + printf("\treason: %s\n", ENOENT_STR); + break; + + case ESRCH: + printf("\treason: %s\n", ESRCH_STR); + break; + + case EINTR: + printf("\treason: %s\n", EINTR_STR); + break; + + } + } + + printf("\tonboard eeprom: first byte: %d\n", buf1[0]); + if (!force_write) { + printf("\tonboard eeprom: WARNING: FURTHER TEST STEPS WILL DESTROY YOUR FLIGHT PARAMETER CONFIGURATION. PROCEED? (y/N)\n"); + + printf("Input: "); + char c = getchar(); + printf("%c\n", c); + if (c != 'y' && c != 'Y') { + /* not yes, abort */ + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: onboard eeprom test aborted by user, read test successful\r\n"); + return OK; + } + } + + printf("\tonboard eeprom: proceeding with write test\r\n"); + + /* increment counter */ + buf1[0] = buf1[0] + 1; + + /* rewind to the start of the file */ + lseek(fd, 0, SEEK_SET); + + /* write data */ + ret = write(fd, buf1, sizeof(buf1)); + + if (ret != sizeof(buf1)) { + printf("\tonboard eeprom: ERROR: write fail: %d\n", (char)ret); + + switch (-ret) { + case EPERM: + printf("\treason: %s\n", EPERM_STR); + break; + + case ENOENT: + printf("\treason: %s\n", ENOENT_STR); + break; + + case ESRCH: + printf("\treason: %s\n", ESRCH_STR); + break; + + case EINTR: + printf("\treason: %s\n", EINTR_STR); + break; + + } + + //return ERROR; + } + + /* rewind to the start of the file */ + lseek(fd, 0, SEEK_SET); + + /* read data */ + ret = read(fd, buf1, sizeof(buf1)); + + if (ret != sizeof(buf1)) { + printf("\tonboard eeprom: ERROR: read fail: %d\n", ret); + + switch (-ret) { + case EPERM: + printf("\treason: %s\n", EPERM_STR); + break; + + case ENOENT: + printf("\treason: %s\n", ENOENT_STR); + break; + + case ESRCH: + printf("\treason: %s\n", ESRCH_STR); + break; + + case EINTR: + printf("\treason: %s\n", EINTR_STR); + break; + + } + + return ERROR; + + } else { + /* enforce null termination and print as string */ + if (buf1[sizeof(buf1) - 1] != 0) { + printf("\tWARNING: Null termination in file not present as expected, enforcing it now..\r\n"); + buf1[sizeof(buf1) - 1] = '\0'; + } + + /* read out counter and replace val */ + int counter = buf1[0]; + printf("\tonboard eeprom: count: #%d, read values: %s\n", counter, (char *)buf1 + 1); + printf("\tAll %d bytes:\n\n\t", sizeof(buf1)); + + for (int i = 0; i < sizeof(buf1); i++) { + printf("0x%02x ", buf1[i]); + + if (i % 8 == 7) printf("\n\t"); + + if (i % 64 == 63) printf("\n\t"); + } + + /* end any open line */ + printf("\n\n"); + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: onboard eeprom passed all tests successfully\n"); + return ret; +} + +static int +baseboard_eeprom(int argc, char *argv[]) +{ + printf("\tbaseboard eeprom: test start\n"); + fflush(stdout); + + int fd; + uint8_t buf[128] = {'R', 'E', 'A', 'D', ' ', 'F', 'A', 'I', 'L', 'E', 'D', '\0'}; + int ret; + + fd = open("/dev/baseboard_eeprom", O_RDONLY | O_NONBLOCK); + + if (fd < 0) { + printf("\tbaseboard eeprom: open fail\n"); + return ERROR; + } + + /* read data */ + ret = read(fd, buf, sizeof(buf)); + /* set last char to string termination */ + buf[127] = '\0'; + + if (ret != sizeof(buf)) { + printf("\tbaseboard eeprom: ERROR: read fail\n", ret); + return ERROR; + + } else { + printf("\tbaseboard eeprom: string: %s\n", (char *)buf); + } + + close(fd); + + /* XXX more tests here */ + + /* Let user know everything is ok */ + printf("\tOK: baseboard eeprom passed all tests successfully\n"); + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_eeproms + ****************************************************************************/ + +int test_eeproms(int argc, char *argv[]) +{ + unsigned i; + + printf("Running EEPROMs tests:\n\n"); + fflush(stdout); + + for (i = 0; eeproms[i].name; i++) { + printf(" eeprom: %s\n", eeproms[i].name); + eeproms[i].test(argc, argv); + fflush(stdout); + /* wait 100 ms to make sure buffer is emptied */ + usleep(100000); + } + + return 0; +} diff --git a/apps/px4/tests/test_float.c b/apps/px4/tests/test_float.c new file mode 100644 index 000000000..02e748e70 --- /dev/null +++ b/apps/px4/tests/test_float.c @@ -0,0 +1,282 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +typedef union { + float f; + double d; + uint8_t b[8]; +} test_float_double_t; + +int test_float(int argc, char *argv[]) +{ + int ret = 0; + + printf("\n--- SINGLE PRECISION TESTS ---\n"); + printf("The single precision test involves calls to fabs(),\nif test fails check this function as well.\n\n"); + + float f1 = 1.55f; + + float sinf_zero = sinf(0.0f); + float sinf_one = sinf(1.0f); + float sqrt_two = sqrt(2.0f); + + if (sinf_zero == 0.0f) { + printf("\t success: sinf(0.0f) == 0.0f\n"); + + } else { + printf("\t FAIL: sinf(0.0f) != 0.0f, result: %f\n", sinf_zero); + ret = -4; + } + + fflush(stdout); + + if (fabs((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) { + printf("\t success: sinf(1.0f) == 0.84147f\n"); + + } else { + printf("\t FAIL: sinf(1.0f) != 0.84147f, result: %f\n", sinf_one); + ret = -1; + } + + fflush(stdout); + + float asinf_one = asinf(1.0f); + + if (fabs((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) { + printf("\t success: asinf(1.0f) == 1.57079f\n"); + + } else { + printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one); + ret = -1; + } + + fflush(stdout); + + float cosf_one = cosf(1.0f); + + if (fabs((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) { + printf("\t success: cosf(1.0f) == 0.54030f\n"); + + } else { + printf("\t FAIL: cosf(1.0f) != 0.54030f, result: %f\n", cosf_one); + ret = -1; + } + + fflush(stdout); + + + float acosf_one = acosf(1.0f); + + if (fabs((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) { + printf("\t success: acosf(1.0f) == 0.0f\n"); + + } else { + printf("\t FAIL: acosf(1.0f) != 0.0f, result: %f\n", acosf_one); + ret = -1; + } + + fflush(stdout); + + + float sinf_zero_one = sinf(0.1f); + + if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { + printf("\t success: sinf(0.1f) == 0.09983f\n"); + + } else { + printf("\t FAIL: sinf(0.1f) != 0.09983f, result: %f\n", sinf_zero_one); + ret = -2; + } + + if (sqrt_two == 1.41421356f) { + printf("\t success: sqrt(2.0f) == 1.41421f\n"); + + } else { + printf("\t FAIL: sqrt(2.0f) != 1.41421f, result: %f\n", sinf_zero_one); + ret = -3; + } + + float atan2f_ones = atan2(1.0f, 1.0f); + + if (fabs(atan2f_ones - 0.785398163397448278999490867136f) < FLT_EPSILON) { + printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n"); + + } else { + printf("\t FAIL: atan2f(1.0f, 1.0f) != 0.78539f, result: %f\n", atan2f_ones); + ret = -4; + } + + printf("\t testing printing: printf(0.553415f): %f\n", 0.553415f); + + + + + + printf("\n--- DOUBLE PRECISION TESTS ---\n"); + + double d1 = 1.0111; + double d2 = 2.0; + + double d1d2 = d1 * d2; + + if (d1d2 == 2.022200000000000219557705349871) { + printf("\t success: 1.0111 * 2.0 == 2.0222\n"); + + } else { + printf("\t FAIL: 1.0111 * 2.0 != 2.0222, result: %f\n", d1d2); + } + + fflush(stdout); + + // Assign value of f1 to d1 + d1 = f1; + + if (f1 == (float)d1) { + printf("\t success: (float) 1.55f == 1.55 (double)\n"); + + } else { + printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %f\n", f1); + ret = -4; + } + + fflush(stdout); + + + double sin_zero = sin(0.0); + double sin_one = sin(1.0); + double atan2_ones = atan2(1.0, 1.0); + + if (sin_zero == 0.0) { + printf("\t success: sin(0.0) == 0.0\n"); + + } else { + printf("\t FAIL: sin(0.0) != 0.0, result: %f\n", sin_zero); + ret = -5; + } + + if (sin_one == 0.841470984807896504875657228695) { + printf("\t success: sin(1.0) == 0.84147098480\n"); + + } else { + printf("\t FAIL: sin(1.0) != 1.0, result: %f\n", sin_one); + ret = -6; + } + + if (atan2_ones != 0.785398) { + printf("\t success: atan2(1.0, 1.0) == 0.785398\n"); + + } else { + printf("\t FAIL: atan2(1.0, 1.0) != 0.785398, result: %f\n", atan2_ones); + ret = -7; + } + + printf("\t testing printing: printf(0.553415): %f\n", 0.553415); + + printf("\t testing pow() with magic value\n"); + printf("\t (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295)));\n"); + fflush(stdout); + usleep(20000); + double powres = (44330.0 * (1.0 - pow((96286LL / 101325.0), 0.190295))); + printf("\t success: result: %f\n", (float)powres); + + + if (ret == 0) { + printf("\n SUCCESS: All float and double tests passed.\n"); + + } else { + printf("\n FAIL: One or more tests failed.\n"); + } + + printf("\n"); + + return ret; +} diff --git a/apps/px4/tests/test_gpio.c b/apps/px4/tests/test_gpio.c new file mode 100644 index 000000000..ab536d956 --- /dev/null +++ b/apps/px4/tests/test_gpio.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/analog/adc.h> + +#include "tests.h" + +#include <drivers/drv_gpio.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_gpio + ****************************************************************************/ + +int test_gpio(int argc, char *argv[]) +{ + int fd; + int ret = 0; + + fd = open(GPIO_DEVICE_PATH, 0); + + if (fd < 0) { + printf("GPIO: open fail\n"); + return ERROR; + } + + /* set all GPIOs to default state */ + ioctl(fd, GPIO_RESET, ~0); + + + /* XXX need to add some GPIO waving stuff here */ + + + /* Go back to default */ + ioctl(fd, GPIO_RESET, ~0); + + printf("\t GPIO test successful.\n"); + + return ret; +} diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c new file mode 100644 index 000000000..41f207b7e --- /dev/null +++ b/apps/px4/tests/test_hrt.c @@ -0,0 +1,219 @@ +/**************************************************************************** + * px4/sensors/test_hrt.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <time.h> +#include <unistd.h> + +#include <arch/board/board.h> +#include <arch/board/up_hrt.h> +#include <arch/board/drv_tone_alarm.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +extern uint16_t ppm_buffer[]; +extern unsigned ppm_decoded_channels; +extern uint16_t ppm_edge_history[]; +extern uint16_t ppm_pulse_history[]; + +int test_ppm(int argc, char *argv[]) +{ +#ifdef CONFIG_HRT_PPM + unsigned i; + + printf("channels: %u\n", ppm_decoded_channels); + + for (i = 0; i < ppm_decoded_channels; i++) + printf(" %u\n", ppm_buffer[i]); + + printf("edges\n"); + + for (i = 0; i < 32; i++) + printf(" %u\n", ppm_edge_history[i]); + + printf("pulses\n"); + + for (i = 0; i < 32; i++) + printf(" %u\n", ppm_pulse_history[i]); + + fflush(stdout); +#else + printf("PPM not configured\n"); +#endif + return 0; +} + +int test_tone(int argc, char *argv[]) +{ +#ifdef CONFIG_TONE_ALARM + int fd, result; + unsigned long tone; + + fd = open("/dev/tone_alarm", O_WRONLY); + + if (fd < 0) { + printf("failed opening /dev/tone_alarm\n"); + goto out; + } + + tone = 1; + + if (argc == 2) + tone = atoi(argv[1]); + + if (tone == 0) { + result = ioctl(fd, TONE_SET_ALARM, 0); + + if (result < 0) { + printf("failed clearing alarms\n"); + goto out; + + } else { + printf("Alarm stopped.\n"); + } + + } else { + result = ioctl(fd, TONE_SET_ALARM, 0); + + if (result < 0) { + printf("failed clearing alarms\n"); + goto out; + } + + result = ioctl(fd, TONE_SET_ALARM, tone); + + if (result < 0) { + printf("failed setting alarm %lu\n", tone); + + } else { + printf("Alarm %lu (disable with: tests tone 0)\n", tone); + } + } + +out: + + if (fd >= 0) + close(fd); + +#endif + return 0; +} + +/**************************************************************************** + * Name: test_hrt + ****************************************************************************/ + +int test_hrt(int argc, char *argv[]) +{ + struct hrt_call call; + hrt_abstime prev, now; + int i; + struct timeval tv1, tv2; + + printf("start-time (hrt, sec/usec), end-time (hrt, sec/usec), microseconds per half second\n"); + + for (i = 0; i < 10; i++) { + prev = hrt_absolute_time(); + gettimeofday(&tv1, NULL); + usleep(500000); + now = hrt_absolute_time(); + gettimeofday(&tv2, NULL); + printf("%lu (%lu/%lu), %lu (%lu/%lu), %lu\n", + (unsigned long)prev, (unsigned long)tv1.tv_sec, (unsigned long)tv1.tv_usec, + (unsigned long)now, (unsigned long)tv2.tv_sec, (unsigned long)tv2.tv_usec, + (unsigned long)(hrt_absolute_time() - prev)); + fflush(stdout); + } + + usleep(1000000); + + printf("one-second ticks\n"); + + for (i = 0; i < 10; i++) { + hrt_call_after(&call, 1000000, NULL, NULL); + + while (!hrt_called(&call)) { + usleep(1000); + } + + printf("tick\n"); + fflush(stdout); + } + + return 0; +} diff --git a/apps/px4/tests/test_int.c b/apps/px4/tests/test_int.c new file mode 100644 index 000000000..40e030641 --- /dev/null +++ b/apps/px4/tests/test_int.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +typedef union { + int32_t i; + int64_t l; + uint8_t b[8]; +} test_32_64_t; + +int test_int(int argc, char *argv[]) +{ + int ret = 0; + + printf("\n--- 64 BIT MATH TESTS ---\n"); + + int64_t large = 354156329598; + + int64_t calc = large * 5; + + if (calc == 1770781647990) { + printf("\t success: 354156329598 * 5 == %lld\n", calc); + + } else { + printf("\t FAIL: 354156329598 * 5 != %lld\n", calc); + ret = -1; + } + + fflush(stdout); + + + + + + printf("\n--- 32 BIT / 64 BIT MIXED MATH TESTS ---\n"); + + + int32_t small = 50; + int32_t large_int = 2147483647; // MAX INT value + + uint64_t small_times_large = large_int * (uint64_t)small; + + if (small_times_large == 107374182350) { + printf("\t success: 64bit calculation: 50 * 2147483647 (max int val) == %lld\n", small_times_large); + + } else { + printf("\t FAIL: 50 * 2147483647 != %lld, 64bit cast might fail\n", small_times_large); + ret = -1; + } + + fflush(stdout); + + if (ret == 0) { + printf("\n SUCCESS: All float and double tests passed.\n"); + + } else { + printf("\n FAIL: One or more tests failed.\n"); + } + + printf("\n"); + + return ret; +} diff --git a/apps/px4/tests/test_jig_voltages.c b/apps/px4/tests/test_jig_voltages.c new file mode 100644 index 000000000..51f9b9a5b --- /dev/null +++ b/apps/px4/tests/test_jig_voltages.c @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/arch.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +#include <nuttx/analog/adc.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_gpio + ****************************************************************************/ + +int test_jig_voltages(int argc, char *argv[]) +{ + int fd0 = 0; + int ret = OK; + const int nchannels = 4; + + struct adc_msg4_s + { + uint8_t am_channel1; /* The 8-bit ADC Channel */ + int32_t am_data1; /* ADC convert result (4 bytes) */ + uint8_t am_channel2; /* The 8-bit ADC Channel */ + int32_t am_data2; /* ADC convert result (4 bytes) */ + uint8_t am_channel3; /* The 8-bit ADC Channel */ + int32_t am_data3; /* ADC convert result (4 bytes) */ + uint8_t am_channel4; /* The 8-bit ADC Channel */ + int32_t am_data4; /* ADC convert result (4 bytes) */ + }__attribute__((__packed__));; + + struct adc_msg4_s sample1[4]; + + size_t readsize; + ssize_t nbytes; + int i = 0; + int j = 0; + int errval; + + char name[11]; + sprintf(name, "/dev/adc%d", j); + fd0 = open(name, O_RDONLY | O_NONBLOCK); + if (fd0 < 0) + { + printf("ADC: %s open fail\n", name); + return ERROR; + } else { + printf("Opened %s successfully\n", name); + } + + + /* Expected values */ + int16_t expected_min[] = {2700, 2700, 2200, 2000}; + int16_t expected_max[] = {3000, 3000, 2500, 2200}; + char* check_res[nchannels]; + + /* first adc read round */ + readsize = 4 * sizeof(struct adc_msg_s); + + /* Empty all buffers */ + do { + nbytes = read(fd0, sample1, readsize); + } + while (nbytes > 0); + + up_udelay(20000);//microseconds + /* Take measurements */ + nbytes = read(fd0, sample1, readsize); + + /* Handle unexpected return values */ + + if (nbytes <= 0) + { + errval = errno; + if (errval != EINTR) + { + message("read %s failed: %d\n", + name, errval); + errval = 3; + goto errout_with_dev; + } + + message("\tInterrupted read...\n"); + } + else if (nbytes == 0) + { + message("\tNo data read, Ignoring\n"); + } + + /* Print the sample data on successful return */ + + else + { + int nsamples = nbytes / sizeof(struct adc_msg_s); + if (nsamples * sizeof(struct adc_msg_s) != nbytes) + { + message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n", + nbytes, sizeof(struct adc_msg_s)); + } + else + { + /* Check values */ + check_res[0] = (expected_min[0] < sample1[i].am_data1 && expected_max[0] > sample1[i].am_data1) ? "OK" : "FAIL"; + check_res[1] = (expected_min[1] < sample1[i].am_data2 && expected_max[1] > sample1[i].am_data2) ? "OK" : "FAIL"; + check_res[2] = (expected_min[2] < sample1[i].am_data3 && expected_max[2] > sample1[i].am_data3) ? "OK" : "FAIL"; + check_res[3] = (expected_min[3] < sample1[i].am_data4 && expected_max[3] > sample1[i].am_data4) ? "OK" : "FAIL"; + + /* Accumulate result */ + ret += (expected_min[0] > sample1[i].am_data1 || expected_max[0] < sample1[i].am_data1) ? 1 : 0; + // XXX Chan 11 not connected on test setup + //ret += (expected_min[1] > sample1[i].am_data2 || expected_max[1] < sample1[i].am_data2) ? 1 : 0; + ret += (expected_min[2] > sample1[i].am_data3 || expected_max[2] < sample1[i].am_data3) ? 1 : 0; + ret += (expected_min[3] > sample1[i].am_data4 || expected_max[3] < sample1[i].am_data4) ? 1 : 0; + + message("Sample:"); + message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + i, sample1[i].am_channel1, sample1[i].am_data1, expected_min[0], expected_max[0], check_res[0]); + message("Sample:"); + message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + i, sample1[i].am_channel2, sample1[i].am_data2, expected_min[1], expected_max[1], check_res[1]); + message("Sample:"); + message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + i, sample1[i].am_channel3, sample1[i].am_data3, expected_min[2], expected_max[2], check_res[2]); + message("Sample:"); + message("%d: channel: %d value: %d (allowed min: %d, allowed max: %d), result: %s\n", + i, sample1[i].am_channel4, sample1[i].am_data4, expected_min[3], expected_max[3], check_res[3]); + + if (ret != OK) { + printf("\t ADC test FAILED. Some channels where out of allowed range. Check supply voltages.\n"); + goto errout_with_dev; + } + } + } + + printf("\t ADC test successful.\n"); + + errout_with_dev: + if (fd0 != 0) close(fd0); + + return ret; +} diff --git a/apps/px4/tests/test_led.c b/apps/px4/tests/test_led.c new file mode 100644 index 000000000..53615ccd8 --- /dev/null +++ b/apps/px4/tests/test_led.c @@ -0,0 +1,134 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +int test_led(int argc, char *argv[]) +{ + int fd; + int ret = 0; + + fd = open("/dev/led", O_RDONLY | O_NONBLOCK); + + if (fd < 0) { + printf("\tLED: open fail\n"); + return ERROR; + } + + if (ioctl(fd, LED_ON, LED_BLUE) || + ioctl(fd, LED_ON, LED_AMBER)) { + + printf("\tLED: ioctl fail\n"); + return ERROR; + } + + /* let them blink for fun */ + + int i; + uint8_t ledon = 1; + + for (i = 0; i < 10; i++) { + if (ledon) { + ioctl(fd, LED_ON, LED_BLUE); + ioctl(fd, LED_OFF, LED_AMBER); + + } else { + ioctl(fd, LED_OFF, LED_BLUE); + ioctl(fd, LED_ON, LED_AMBER); + } + + ledon = !ledon; + usleep(60000); + } + + /* Go back to default */ + ioctl(fd, LED_ON, LED_BLUE); + ioctl(fd, LED_OFF, LED_AMBER); + + printf("\t LED test completed, no errors.\n"); + + return ret; +} diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c new file mode 100644 index 000000000..bdb68f88b --- /dev/null +++ b/apps/px4/tests/test_sensors.c @@ -0,0 +1,501 @@ +/**************************************************************************** + * px4/sensors/test_sensors.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +#include <arch/board/drv_lis331.h> +#include <arch/board/drv_bma180.h> +#include <arch/board/drv_l3gd20.h> +#include <arch/board/drv_hmc5883l.h> +#include <arch/board/drv_mpu6000.h> + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +//static int lis331(int argc, char *argv[]); +static int l3gd20(int argc, char *argv[]); +static int bma180(int argc, char *argv[]); +static int hmc5883l(int argc, char *argv[]); +static int ms5611(int argc, char *argv[]); +static int mpu6000(int argc, char *argv[]); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct { + const char *name; + const char *path; + int (* test)(int argc, char *argv[]); +} sensors[] = { + {"l3gd20", "/dev/l3gd20", l3gd20}, + {"bma180", "/dev/bma180", bma180}, + {"hmc5883l", "/dev/hmc5883l", hmc5883l}, + {"ms5611", "/dev/ms5611", ms5611}, + {"mpu6000", "/dev/mpu6000", mpu6000}, +// {"lis331", "/dev/lis331", lis331}, + {NULL, NULL, NULL} +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +//static int +//lis331(int argc, char *argv[]) +//{ +// int fd; +// int16_t buf[3]; +// int ret; +// +// fd = open("/dev/lis331", O_RDONLY); +// if (fd < 0) { +// printf("\tlis331: not present on PX4FMU v1.5 and later\n"); +// return ERROR; +// } +// +// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) || +// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) { +// +// printf("LIS331: ioctl fail\n"); +// return ERROR; +// } +// +// /* wait at least 100ms, sensor should have data after no more than 20ms */ +// usleep(100000); +// +// /* read data - expect samples */ +// ret = read(fd, buf, sizeof(buf)); +// if (ret != sizeof(buf)) { +// printf("LIS331: read1 fail (%d)\n", ret); +// return ERROR; +// } +// +// /* read data - expect no samples (should not be ready again yet) */ +// ret = read(fd, buf, sizeof(buf)); +// if (ret != 0) { +// printf("LIS331: read2 fail (%d)\n", ret); +// return ERROR; +// } +// +// /* XXX more tests here */ +// +// return 0; +//} + +static int +l3gd20(int argc, char *argv[]) +{ + printf("\tL3GD20: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/l3gd20", O_RDONLY | O_NONBLOCK); + + if (fd < 0) { + printf("L3GD20: open fail\n"); + return ERROR; + } + +// if (ioctl(fd, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_50HZ) || +// ioctl(fd, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { +// +// printf("L3GD20: ioctl fail\n"); +// return ERROR; +// } else { +// printf("\tconfigured..\n"); +// } +// +// /* wait at least 100ms, sensor should have data after no more than 2ms */ +// usleep(100000); + + + + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tL3GD20: read1 fail (%d should have been %d)\n", ret, sizeof(buf)); + //return ERROR; + + } else { + printf("\tL3GD20 values #1: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* wait at least 2 ms, sensor should have data after no more than 1.5ms */ + usleep(2000); + + /* read data - expect no samples (should not be ready again yet) */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tL3GD20: read2 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tL3GD20 values #2: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* empty sensor buffer */ + ret = 0; + + while (ret != sizeof(buf)) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + } + + /* test if FIFO is operational */ + usleep(14800); // Expecting 10 measurements + + ret = 0; + int count = 0; + bool dataready = true; + + while (dataready) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + dataready = false; + + } else { + count++; + } + } + + printf("\tL3GD20: Drained FIFO with %d values (expected 8-12)\n", count); + + /* read data - expect no samples (should not be ready again yet) */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != 0) { + printf("\tL3GD20: Note: read3 got data - there should not have been data ready\n", ret); +// return ERROR; + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: L3GD20 passed all tests successfully\n"); + return OK; +} + +static int +bma180(int argc, char *argv[]) +{ + printf("\tBMA180: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/bma180", O_RDONLY); + + if (fd < 0) { + printf("\tBMA180: open fail\n"); + return ERROR; + } + +// if (ioctl(fd, LIS331_SETRATE, LIS331_RATE_50Hz) || +// ioctl(fd, LIS331_SETRANGE, LIS331_RANGE_4G)) { +// +// printf("BMA180: ioctl fail\n"); +// return ERROR; +// } +// + /* wait at least 100ms, sensor should have data after no more than 20ms */ + usleep(100000); + + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tBMA180: read1 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tBMA180 values: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* wait at least 10ms, sensor should have data after no more than 2ms */ + usleep(100000); + + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tBMA180: read2 fail (%d)\n", ret); + close(fd); + return ERROR; + + } else { + printf("\tBMA180: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + + /* empty sensor buffer */ + ret = 0; + + while (ret != sizeof(buf)) { + // Keep reading until successful + ret = read(fd, buf, sizeof(buf)); + } + + ret = read(fd, buf, sizeof(buf)); + + if (ret != 0) { + printf("\tBMA180: Note: read3 got data - there should not have been data ready\n", ret); + } + + /* Let user know everything is ok */ + printf("\tOK: BMA180 passed all tests successfully\n"); + close(fd); + + return OK; +} + +static int +mpu6000(int argc, char *argv[]) +{ + printf("\tMPU-6000: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[5] = { -1, 0, -1, 0, -1, 0}; + int ret; + + fd = open("/dev/mpu6000", O_RDONLY); + + if (fd < 0) { + printf("\tMPU-6000: open fail\n"); + return ERROR; + } + + // /* wait at least 100ms, sensor should have data after no more than 20ms */ + // usleep(100000); + + // /* read data - expect samples */ + // ret = read(fd, buf, sizeof(buf)); + + // if (ret != sizeof(buf)) { + // printf("\tMPU-6000: read1 fail (%d)\n", ret); + // return ERROR; + + // } else { + // printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); + // } + + // /* wait at least 10ms, sensor should have data after no more than 2ms */ + // usleep(100000); + + // ret = read(fd, buf, sizeof(buf)); + + // if (ret != sizeof(buf)) { + // printf("\tMPU-6000: read2 fail (%d)\n", ret); + // return ERROR; + + // } else { + // printf("\tMPU-6000 values: acc: x:%d\ty:%d\tz:%d\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); + // } + + /* XXX more tests here */ + + /* Let user know everything is ok */ + printf("\tOK: MPU-6000 passed all tests successfully\n"); + + return OK; +} + +static int +ms5611(int argc, char *argv[]) +{ + printf("\tMS5611: test start\n"); + fflush(stdout); + + int fd; + float buf[3] = {0.0f, 0.0f, 0.0f}; + int ret; + + fd = open("/dev/ms5611", O_RDONLY); + + if (fd < 0) { + printf("\tMS5611: open fail\n"); + return ERROR; + } + + for (int i = 0; i < 5; i++) { + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + if ((uint8_t)ret == -EAGAIN || (uint8_t)ret == -EINPROGRESS || i < 3) { + /* waiting for device to become ready, this is not an error */ + } else { + printf("\tMS5611: read fail (%d)\n", ret); + close(fd); + return ERROR; + } + + } else { + + /* hack for float printing */ + int32_t pressure_int = buf[0]; + int32_t altitude_int = buf[1]; + int32_t temperature_int = buf[2]; + + printf("\tMS5611: pressure:%d.%03d mbar - altitude: %d.%02d meters - temp:%d.%02d deg celcius\n", pressure_int, (int)(buf[0] * 1000 - pressure_int * 1000), altitude_int, (int)(buf[1] * 100 - altitude_int * 100), temperature_int, (int)(buf[2] * 100 - temperature_int * 100)); + } + + /* wait at least 10ms, sensor should have data after no more than 6.5ms */ + usleep(10000); + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: MS5611 passed all tests successfully\n"); + + return OK; +} + +static int +hmc5883l(int argc, char *argv[]) +{ + printf("\tHMC5883L: test start\n"); + fflush(stdout); + + int fd; + int16_t buf[3] = {0, 0, 0}; + int ret; + + fd = open("/dev/hmc5883l", O_RDONLY); + + if (fd < 0) { + printf("\tHMC5883L: open fail\n"); + return ERROR; + } + + int i; + + for (i = 0; i < 5; i++) { + /* wait at least 7ms, sensor should have data after no more than 6.5ms */ + usleep(7000); + + /* read data - expect samples */ + ret = read(fd, buf, sizeof(buf)); + + if (ret != sizeof(buf)) { + printf("\tHMC5883L: read1 fail (%d) values: x:%d\ty:%d\tz:%d\n", ret, buf[0], buf[1], buf[2]); + close(fd); + return ERROR; + + } else { + printf("\tHMC5883L: x:%d\ty:%d\tz:%d\n", buf[0], buf[1], buf[2]); + } + } + + close(fd); + + /* Let user know everything is ok */ + printf("\tOK: HMC5883L passed all tests successfully\n"); + + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_sensors + ****************************************************************************/ + +int test_sensors(int argc, char *argv[]) +{ + unsigned i; + + printf("Running sensors tests:\n\n"); + fflush(stdout); + + int ret = OK; + + for (i = 0; sensors[i].name; i++) { + printf(" sensor: %s\n", sensors[i].name); + + /* Flush and leave enough time for the flush to become effective */ + fflush(stdout); + usleep(50000); + /* Test the sensor - if the tests crash at this point, the right sensor name has been printed */ + + ret += sensors[i].test(argc, argv); + } + + return ret; +} diff --git a/apps/px4/tests/test_servo.c b/apps/px4/tests/test_servo.c new file mode 100644 index 000000000..f95760ca8 --- /dev/null +++ b/apps/px4/tests/test_servo.c @@ -0,0 +1,132 @@ +/**************************************************************************** + * px4/sensors/test_hrt.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <time.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> + +#include <nuttx/spi.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_servo + ****************************************************************************/ + +int test_servo(int argc, char *argv[]) +{ + int fd, result; + servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; + servo_position_t pos; + + fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); + + if (fd < 0) { + printf("failed opening /dev/pwm_servo\n"); + goto out; + } + + result = read(fd, &data, sizeof(data)); + + if (result != sizeof(data)) { + printf("failed bulk-reading channel values\n"); + goto out; + } + + printf("Servo readback, pairs of values should match defaults\n"); + + for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) { + result = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&pos); + + if (result < 0) { + printf("failed reading channel %u\n", i); + goto out; + } + + printf("%u: %u %u\n", i, pos, data[i]); + + } + + printf("Servos arming at default values\n"); + result = ioctl(fd, PWM_SERVO_ARM, 0); + usleep(5000000); + printf("Advancing channel 0 to 1500\n"); + result = ioctl(fd, PWM_SERVO_SET(0), 1500); +out: + return 0; +} diff --git a/apps/px4/tests/test_sleep.c b/apps/px4/tests/test_sleep.c new file mode 100644 index 000000000..911a9c2e1 --- /dev/null +++ b/apps/px4/tests/test_sleep.c @@ -0,0 +1,103 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_sleep + ****************************************************************************/ + +int test_sleep(int argc, char *argv[]) +{ + unsigned int nsleeps = 100; + printf("\t %d 100ms sleeps\n", nsleeps); + fflush(stdout); + + for (int i = 0; i < nsleeps; i++) { + usleep(100000); + //printf("\ttick\n"); + } + + printf("\t Sleep test successful.\n"); + + return OK; +} diff --git a/apps/px4/tests/test_time.c b/apps/px4/tests/test_time.c new file mode 100644 index 000000000..c128c73a3 --- /dev/null +++ b/apps/px4/tests/test_time.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * px4/tests/test_time.c + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> +#include <arch/board/up_hrt.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* emulate hrt_absolute_time using the cycle counter */ +static hrt_abstime +cycletime(void) +{ + static uint64_t basetime; + static uint32_t lasttime; + uint32_t cycles; + + cycles = *(unsigned long *)0xe0001004; + + if (cycles < lasttime) + basetime += 0x100000000ULL; + + lasttime = cycles; + + return (basetime + cycles) / 168; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +int test_time(int argc, char *argv[]) +{ + hrt_abstime h, c; + int64_t lowdelta, maxdelta = 0; + int64_t delta, deltadelta; + + /* enable the cycle counter */ + (*(unsigned long *)0xe000edfc) |= (1 << 24); /* DEMCR |= DEMCR_TRCENA */ + (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */ + + /* get an average delta between the two clocks - this should stay roughly the same */ + delta = 0; + + for (unsigned i = 0; i < 100; i++) { + uint32_t flags = irqsave(); + + h = hrt_absolute_time(); + c = cycletime(); + + irqrestore(flags); + + delta += h - c; + } + + lowdelta = abs(delta / 100); + + /* loop checking the time */ + for (unsigned i = 0; i < 100000; i++) { + + usleep(rand() * 10); + + uint32_t flags = irqsave(); + + c = cycletime(); + h = hrt_absolute_time(); + + irqrestore(flags); + + delta = abs(h - c); + deltadelta = abs(delta - lowdelta); + + if (deltadelta > maxdelta) + maxdelta = deltadelta; + + if (deltadelta > 1000) + fprintf(stderr, "h %llu c %llu d %lld\n", h, c, delta - lowdelta); + } + + printf("Maximum jitter %lld\n", maxdelta); + + return 0; +} diff --git a/apps/px4/tests/test_uart_baudchange.c b/apps/px4/tests/test_uart_baudchange.c new file mode 100644 index 000000000..06965bd3d --- /dev/null +++ b/apps/px4/tests/test_uart_baudchange.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <termios.h> +#include <string.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +int test_uart_baudchange(int argc, char *argv[]) +{ + int uart2_nwrite = 0; + + /* assuming NuttShell is on UART1 (/dev/ttyS0) */ + int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); // + + if (uart2 < 0) { + printf("ERROR opening UART2, aborting..\n"); + return uart2; + } + + struct termios uart2_config; + + struct termios uart2_config_original; + + int termios_state = 0; + +#define UART_BAUDRATE_RUNTIME_CONF +#ifdef UART_BAUDRATE_RUNTIME_CONF + + if ((termios_state = tcgetattr(uart2, &uart2_config)) < 0) { + printf("ERROR getting termios config for UART2: %d\n", termios_state); + exit(termios_state); + } + + memcpy(&uart2_config_original, &uart2_config, sizeof(struct termios)); + + /* Set baud rate */ + if (cfsetispeed(&uart2_config, B9600) < 0 || cfsetospeed(&uart2_config, B9600) < 0) { + printf("ERROR setting termios config for UART2: %d\n", termios_state); + exit(ERROR); + } + + if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config)) < 0) { + printf("ERROR setting termios config for UART2\n"); + exit(termios_state); + } + + /* Set back to original settings */ + if ((termios_state = tcsetattr(uart2, TCSANOW, &uart2_config_original)) < 0) { + printf("ERROR setting termios config for UART2\n"); + exit(termios_state); + } + +#endif + + uint8_t sample_uart2[] = {'U', 'A', 'R', 'T', '2', ' ', '#', 0, '\n'}; + + int i, r; + + for (i = 0; i < 100; i++) { + /* uart2 -> */ + r = write(uart2, sample_uart2, sizeof(sample_uart2)); + + if (r > 0) + uart2_nwrite += r; + } + + close(uart2); + + printf("uart2_nwrite %d\n", uart2_nwrite); + + return OK; +} diff --git a/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c new file mode 100644 index 000000000..de1249b8c --- /dev/null +++ b/apps/px4/tests/test_uart_console.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * px4/sensors/test_uart_console.c + * + * Copyright (C) 2012 Lorenz Meier. All rights reserved. + * Authors: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> +#include <arch/board/up_hrt.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +static void *receive_loop(void *arg) +{ + int uart_usb = open("/dev/ttyACM0", O_RDONLY | O_NOCTTY); + + while (1) { + char c; + read(uart_usb, &c, 1); + printf("%c", c); + fflush(stdout); + } + + return NULL; +} + +int test_uart_console(int argc, char *argv[]) +{ + /* assuming NuttShell is on UART1 (/dev/ttyS0) */ + // + int uart_usb = open("/dev/ttyACM0", O_WRONLY | O_NOCTTY); + + if (uart_usb < 0) { + printf("ERROR opening /dev/ttyACM0. Do you need to run sercon first? Aborting..\n"); + return uart_usb; + } + + uint8_t sample_uart_usb[] = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'}; + + pthread_t receive_thread; + + pthread_create(&receive_thread, NULL, receive_loop, NULL); + + //wait for threads to complete: + pthread_join(receive_thread, NULL); + + for (int i = 0; i < 30; i++) { + write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb)); + printf("."); + fflush(stdout); + sleep(1); + } + +// uint64_t start_time = hrt_absolute_time(); +// +//// while (true) +// for (int i = 0; i < 1000; i++) +// { +// //write(uart_usb, sample_uart_usb, sizeof(sample_uart_usb)); +// int nread = 0; +// char c; +// do { +// nread = read(uart_usb, &c, 1); +// if (nread > 0) +// { +// printf("%c", c); +// } +// } while (nread > 0); +// +// do { +// nread = read(uart_console, &c, 1); +// if (nread > 0) +// { +// if (c == 0x03) +// { +// close(uart_usb); +// close(uart_console); +// exit(OK); +// } +// else +// { +// write(uart_usb, &c, 1); +// } +// } +// } while (nread > 0); +// usleep(10000); +// } +// +// int interval = hrt_absolute_time() - start_time; + + close(uart_usb); +// close(uart_console); + + return 0; +} diff --git a/apps/px4/tests/test_uart_loopback.c b/apps/px4/tests/test_uart_loopback.c new file mode 100644 index 000000000..b2e07df1c --- /dev/null +++ b/apps/px4/tests/test_uart_loopback.c @@ -0,0 +1,197 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +int test_uart_loopback(int argc, char *argv[]) +{ + + int uart5_nread = 0; + int uart2_nread = 0; + int uart5_nwrite = 0; + int uart2_nwrite = 0; + + int uart1 = open("/dev/ttyS0", O_RDWR | O_NOCTTY); // + + /* assuming NuttShell is on UART1 (/dev/ttyS0) */ + int uart2 = open("/dev/ttyS1", O_RDWR | O_NONBLOCK | O_NOCTTY); // + int uart5 = open("/dev/ttyS2", O_RDWR | O_NONBLOCK | O_NOCTTY); // + + if (uart2 < 0) { + printf("ERROR opening UART2, aborting..\n"); + return uart2; + } + + if (uart5 < 0) { + printf("ERROR opening UART5, aborting..\n"); + exit(uart5); + } + + uint8_t sample_uart1[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', '\n'}; + uint8_t sample_uart2[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0}; + uint8_t sample_uart5[] = {'C', 'O', 'U', 'N', 'T', ' ', '#', 0}; + + int i, r; + + for (i = 0; i < 1000; i++) { +// printf("TEST #%d\n",i); + write(uart1, sample_uart1, sizeof(sample_uart1)); + + /* uart2 -> uart5 */ + r = write(uart2, sample_uart2, sizeof(sample_uart2)); + + if (r > 0) + uart2_nwrite += r; + +// printf("TEST #%d\n",i); + write(uart1, sample_uart1, sizeof(sample_uart1)); + + /* uart2 -> uart5 */ + r = write(uart5, sample_uart5, sizeof(sample_uart5)); + + if (r > 0) + uart5_nwrite += r; + +// printf("TEST #%d\n",i); + write(uart1, sample_uart1, sizeof(sample_uart1)); + + /* try to read back values */ + do { + r = read(uart5, sample_uart2, sizeof(sample_uart2)); + + if (r > 0) + uart5_nread += r; + } while (r > 0); + +// printf("TEST #%d\n",i); + write(uart1, sample_uart1, sizeof(sample_uart1)); + + do { + r = read(uart2, sample_uart5, sizeof(sample_uart5)); + + if (r > 0) + uart2_nread += r; + } while (r > 0); + +// printf("TEST #%d\n",i); +// write(uart1, sample_uart1, sizeof(sample_uart5)); + } + + for (i = 0; i < 200000; i++) { + + /* try to read back values */ + r = read(uart5, sample_uart2, sizeof(sample_uart2)); + + if (r > 0) + uart5_nread += r; + + r = read(uart2, sample_uart5, sizeof(sample_uart5)); + + if (r > 0) + uart2_nread += r; + + if ((uart2_nread == uart2_nwrite) && (uart5_nread == uart5_nwrite)) + break; + } + + + close(uart1); + close(uart2); + close(uart5); + + printf("uart2_nwrite %d\n", uart2_nwrite); + printf("uart5_nwrite %d\n", uart5_nwrite); + printf("uart2_nread %d\n", uart2_nread); + printf("uart5_nread %d\n", uart5_nread); + + + return 0; +} diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c new file mode 100644 index 000000000..83d205440 --- /dev/null +++ b/apps/px4/tests/test_uart_send.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * px4/sensors/test_gpio.c + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <arch/board/drv_led.h> + +#include "tests.h" + +#include <math.h> +#include <float.h> +#include <arch/board/up_hrt.h> + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_led + ****************************************************************************/ + +int test_uart_send(int argc, char *argv[]) +{ + /* input handling */ + char *uart_name = "/dev/ttyS3"; + + if (argc > 1) uart_name = argv[1]; + + /* assuming NuttShell is on UART1 (/dev/ttyS0) */ + int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); // + + if (test_uart < 0) { + printf("ERROR opening UART %s, aborting..\n", uart_name); + return test_uart; + + } else { + printf("Writing to UART %s\n", uart_name); + } + + char sample_test_uart[25];// = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'}; + + int i, r, n; + + uint64_t start_time = hrt_absolute_time(); + + for (i = 0; i < 30000; i++) { + n = sprintf(sample_test_uart, "SAMPLE #%d\n", i); + write(test_uart, sample_test_uart, n); + } + + int interval = hrt_absolute_time() - start_time; + + int bytes = i * sizeof(sample_test_uart); + + printf("Wrote %d bytes in %d ms on UART %s\n", bytes, interval / 1000, uart_name); + + close(test_uart); + + return 0; +} diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h new file mode 100644 index 000000000..aa6dae1e0 --- /dev/null +++ b/apps/px4/tests/tests.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef __APPS_PX4_TESTS_H +#define __APPS_PX4_TESTS_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lib_rawprintf(__VA_ARGS__) +# define msgflush() +# else +# define message(...) printf(__VA_ARGS__) +# define msgflush() fflush(stdout) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lib_rawprintf +# define msgflush() +# else +# define message printf +# define msgflush() fflush(stdout) +# endif +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +extern int test_sensors(int argc, char *argv[]); +extern int test_gpio(int argc, char *argv[]); +extern int test_hrt(int argc, char *argv[]); +extern int test_tone(int argc, char *argv[]); +extern int test_led(int argc, char *argv[]); +extern int test_adc(int argc, char *argv[]); +extern int test_int(int argc, char *argv[]); +extern int test_float(int argc, char *argv[]); +extern int test_eeproms(int argc, char *argv[]); +extern int test_ppm(int argc, char *argv[]); +extern int test_servo(int argc, char *argv[]); +extern int test_uart_loopback(int argc, char *argv[]); +extern int test_uart_baudchange(int argc, char *argv[]); +extern int test_cpuload(int argc, char *argv[]); +extern int test_uart_send(int argc, char *argv[]); +extern int test_sleep(int argc, char *argv[]); +extern int test_time(int argc, char *argv[]); +extern int test_uart_console(int argc, char *argv[]); +extern int test_jig_voltages(int argc, char *argv[]); + +#endif /* __APPS_PX4_TESTS_H */ diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c new file mode 100644 index 000000000..704b5a237 --- /dev/null +++ b/apps/px4/tests/tests_main.c @@ -0,0 +1,339 @@ +/**************************************************************************** + * px4/sensors/tests_main.c + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include <nuttx/spi.h> + +#include <systemlib/perf_counter.h> + +#include "tests.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int test_help(int argc, char *argv[]); +static int test_all(int argc, char *argv[]); +static int test_perf(int argc, char *argv[]); +static int test_jig(int argc, char *argv[]); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct { + const char *name; + int (* fn)(int argc, char *argv[]); + unsigned options; + int passed; +#define OPT_NOHELP (1<<0) +#define OPT_NOALLTEST (1<<1) +#define OPT_NOJIGTEST (1<<2) +} tests[] = { + {"led", test_led, 0, 0}, + {"int", test_int, 0, 0}, + {"float", test_float, 0, 0}, + {"sensors", test_sensors, 0, 0}, + {"gpio", test_gpio, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"ppm", test_ppm, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"adc", test_adc, OPT_NOJIGTEST, 0}, + {"jig_voltages", test_jig_voltages, OPT_NOALLTEST, 0}, + {"eeproms", test_eeproms, 0, 0}, + {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"tone", test_tone, 0, 0}, + {"sleep", test_sleep, OPT_NOJIGTEST, 0}, + {"time", test_time, OPT_NOJIGTEST, 0}, + {"perf", test_perf, OPT_NOJIGTEST, 0}, + {"all", test_all, OPT_NOALLTEST | OPT_NOJIGTEST, 0}, + {"jig", test_jig, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0}, + {NULL, NULL, 0, 0} +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int +test_help(int argc, char *argv[]) +{ + unsigned i; + + printf("Available tests:\n"); + + for (i = 0; tests[i].name; i++) + printf(" %s\n", tests[i].name); + + return 0; +} + +static int +test_all(int argc, char *argv[]) +{ + unsigned i; + char *args[2] = {"all", NULL}; + unsigned int failcount = 0; + + printf("\nRunning all tests...\n\n"); + + for (i = 0; tests[i].name; i++) { + /* Only run tests that are not excluded */ + if (!(tests[i].options & OPT_NOALLTEST)) { + printf(" [%s] \t\t\tSTARTING TEST\n", tests[i].name); + fflush(stdout); + + /* Execute test */ + if (tests[i].fn(1, args) != 0) { + fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name); + fflush(stderr); + failcount++; + + } else { + tests[i].passed = 1; + printf(" [%s] \t\t\tPASS\n", tests[i].name); + fflush(stdout); + } + } + } + + /* Print summary */ + printf("\n"); + int j; + + for (j = 0; j < 40; j++) { + printf("-"); + } + + printf("\n\n"); + + printf(" T E S T S U M M A R Y\n\n"); + + if (failcount == 0) { + printf(" ______ __ __ ______ __ __ \n"); + printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n"); + printf(" \\ \\ __ \\ \\ \\ \\____ \\ \\ \\____ \\ \\ \\/\\ \\ \\ \\ _\"-. \n"); + printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n"); + printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n"); + printf("\n"); + printf(" All tests passed (%d of %d)\n", i, i); + + } else { + printf(" ______ ______ __ __ \n"); + printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n"); + printf(" \\ \\ __\\ \\ \\ __ \\ \\ \\ \\ \\ \\ \\__\n"); + printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n"); + printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n"); + printf("\n"); + printf(" Some tests failed (%d of %d)\n", failcount, i); + } + + printf("\n"); + + /* Print failed tests */ + if (failcount > 0) printf(" Failed tests:\n\n"); + + int k; + + for (k = 0; k < i; k++) { + if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOALLTEST)) { + printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); + } + } + + fflush(stdout); + + return 0; +} + +static int +test_perf(int argc, char *argv[]) +{ + perf_counter_t cc, ec; + + cc = perf_alloc(PC_COUNT, "test_count"); + ec = perf_alloc(PC_ELAPSED, "test_elapsed"); + + if ((cc == NULL) || (ec == NULL)) { + printf("perf: counter alloc failed\n"); + return 1; + } + + perf_begin(ec); + perf_count(cc); + perf_count(cc); + perf_count(cc); + perf_count(cc); + printf("perf: expect count of 4\n"); + perf_print_counter(cc); + perf_end(ec); + printf("perf: expect count of 1\n"); + perf_print_counter(ec); + printf("perf: expect at least two counters\n"); + perf_print_all(); + + perf_free(cc); + perf_free(ec); +} + +test_jig(int argc, char *argv[]) +{ + unsigned i; + char *args[2] = {"jig", NULL}; + unsigned int failcount = 0; + + printf("\nRunning all tests...\n\n"); + for (i = 0; tests[i].name; i++) { + /* Only run tests that are not excluded */ + if (!(tests[i].options & OPT_NOJIGTEST)) { + printf(" [%s] \t\t\tSTARTING TEST\n", tests[i].name); + fflush(stdout); + /* Execute test */ + if (tests[i].fn(1, args) != 0) { + fprintf(stderr, " [%s] \t\t\tFAIL\n", tests[i].name); + fflush(stderr); + failcount++; + } else { + tests[i].passed = 1; + printf(" [%s] \t\t\tPASS\n", tests[i].name); + fflush(stdout); + } + } + } + + /* Print summary */ + printf("\n"); + int j; + for (j = 0; j < 40; j++) + { + printf("-"); + } + printf("\n\n"); + + printf(" T E S T S U M M A R Y\n\n"); + if (failcount == 0) { + printf(" ______ __ __ ______ __ __ \n"); + printf(" /\\ __ \\ /\\ \\ /\\ \\ /\\ __ \\ /\\ \\/ / \n"); + printf(" \\ \\ __ \\ \\ \\ \\____ \\ \\ \\____ \\ \\ \\/\\ \\ \\ \\ _\"-. \n"); + printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n"); + printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n"); + printf("\n"); + printf(" All tests passed (%d of %d)\n", i, i); + } else { + printf(" ______ ______ __ __ \n"); + printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n"); + printf(" \\ \\ __\\ \\ \\ __ \\ \\ \\ \\ \\ \\ \\__\n"); + printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n"); + printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n"); + printf("\n"); + printf(" Some tests failed (%d of %d)\n", failcount, i); + } + printf("\n"); + + /* Print failed tests */ + if (failcount > 0) printf(" Failed tests:\n\n"); + int k; + for (k = 0; k < i; k++) + { + if ((tests[k].passed == 0) && !(tests[k].options & OPT_NOJIGTEST)) + { + printf(" [%s] to obtain details, please re-run with\n\t nsh> tests %s\n\n", tests[k].name, tests[k].name); + } + } + fflush(stdout); + + return 0; +} + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int tests_main(int argc, char *argv[]); + +/**************************************************************************** + * Name: tests_main + ****************************************************************************/ + +int tests_main(int argc, char *argv[]) +{ + unsigned i; + + if (argc < 2) { + printf("tests: missing test name - 'tests help' for a list of tests\n"); + return 1; + } + + for (i = 0; tests[i].name; i++) { + if (!strcmp(tests[i].name, argv[1])) + return tests[i].fn(argc - 1, argv + 1); + } + + printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]); + return ERROR; +} diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile new file mode 100644 index 000000000..06be90a0c --- /dev/null +++ b/apps/px4io/Makefile @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build the px4io application. +# + +include $(APPDIR)/mk/app.mk diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c new file mode 100644 index 000000000..19802bf4f --- /dev/null +++ b/apps/px4io/comms.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 FMU communication for the PX4IO module. + */ + + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <debug.h> +#include <stdlib.h> +#include <errno.h> +#include <string.h> + +#include <nuttx/clock.h> + +#include <arch/board/up_hrt.h> +#include <systemlib/hx_stream.h> +#include <systemlib/perf_counter.h> + +#include "px4io.h" + +#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */ +#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */ + +static int fmu_fd; +static hx_stream_t stream; + +static struct px4io_report report; + +static void comms_handle_frame(void *arg, const void *buffer, size_t length); + +void +comms_init(void) +{ + fmu_fd = open("/dev/ttyS1", O_RDWR | O_NONBLOCK); + if (fmu_fd < 0) + lib_lowprintf("COMMS: fmu open failed %d\n", errno); + + stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL); + + report.i2f_magic = I2F_MAGIC; +} + +void +comms_check(void) +{ + static hrt_abstime last_report_time; + hrt_abstime now, delta; + uint8_t c; + + /* should we send a report to the FMU? */ + now = hrt_absolute_time(); + delta = now - last_report_time; + if ((delta > FMU_MIN_REPORT_INTERVAL) && + (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) { + + system_state.fmu_report_due = false; + last_report_time = now; + + /* populate the report */ + for (unsigned i = 0; i < system_state.rc_channels; i++) + report.rc_channel[i] = system_state.rc_channel_data[i]; + report.channel_count = system_state.rc_channels; + report.armed = system_state.armed; + + /* and send it */ + hx_stream_send(stream, &report, sizeof(report)); + } + + /* feed any received bytes to the HDLC receive engine */ + while (read(fmu_fd, &c, 1) == 1) + hx_stream_rx(stream, c); +} + +static void +comms_handle_frame(void *arg, const void *buffer, size_t length) +{ + struct px4io_command *cmd; + + /* make sure it's what we are expecting */ + if (length != sizeof(struct px4io_command)) + return; + + cmd = (struct px4io_command *)buffer; + + /* fetch new PWM output values */ + for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) + system_state.fmu_channel_data[i] = cmd->servo_command[i]; + + system_state.arm_ok = cmd->arm_ok; + system_state.mixer_use_fmu = true; + system_state.fmu_data_received = true; + + /* handle changes signalled by FMU */ + if (!system_state.arm_ok && system_state.armed) + system_state.armed = false; + + /* XXX do relay changes here */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) + system_state.relays[i] = cmd->relay_state[i]; +} diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c new file mode 100644 index 000000000..9dc1fdcba --- /dev/null +++ b/apps/px4io/mixer.c @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * 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 Control channel input/output mixer and failsafe. + */ + +#include <nuttx/config.h> +#include <nuttx/arch.h> + +#include <sys/types.h> +#include <stdbool.h> + +#include <assert.h> +#include <errno.h> +#include <fcntl.h> + +#include <arch/board/drv_ppm_input.h> +#include <arch/board/drv_pwm_servo.h> +#include <arch/board/up_hrt.h> + +#include "px4io.h" + +#ifdef CONFIG_DISABLE_MQUEUE +# error Mixer requires message queues - set CONFIG_DISABLE_MQUEUE=n and try again +#endif + +static mqd_t input_queue; + +/* + * Count of periodic calls in which we have no data. + */ +static unsigned mixer_input_drops; +#define MIXER_INPUT_DROP_LIMIT 10 + +/* + * Count of periodic calls in which we have no FMU input. + */ +static unsigned fmu_input_drops; +#define FMU_INPUT_DROP_LIMIT 10 + +/* + * HRT periodic call used to check for control input data. + */ +static struct hrt_call mixer_input_call; + +/* + * Mixer periodic tick. + */ +static void mixer_tick(void *arg); + +/* + * Collect RC input data from the controller source(s). + */ +static void mixer_get_rc_input(void); + +/* + * Update a mixer based on the current control signals. + */ +static void mixer_update(int mixer, uint16_t *inputs, int input_count); + +/* servo driver handle */ +int mixer_servo_fd; + +/* current servo arm/disarm state */ +bool mixer_servos_armed; + +/* + * Each mixer consumes a set of inputs and produces a single output. + */ +struct mixer { + uint16_t current_value; + /* XXX more config here */ +} mixers[IO_SERVO_COUNT]; + +int +mixer_init(const char *mq_name) +{ + /* open the control input queue; this should always exist */ + input_queue = mq_open(mq_name, O_RDONLY | O_NONBLOCK); + ASSERTCODE((input_queue >= 0), A_INPUTQ_OPEN_FAIL); + + /* open the servo driver */ + mixer_servo_fd = open("/dev/pwm_servo", O_WRONLY); + ASSERTCODE((mixer_servo_fd >= 0), A_SERVO_OPEN_FAIL); + + /* look for control data at 50Hz */ + hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); + + return 0; +} + +static void +mixer_tick(void *arg) +{ + uint16_t *control_values; + int control_count; + int i; + bool should_arm; + + /* + * Start by looking for R/C control inputs. + * This updates system_state with any control inputs received. + */ + mixer_get_rc_input(); + + /* + * Decide which set of inputs we're using. + */ + if (system_state.mixer_use_fmu) { + /* we have recent control data from the FMU */ + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + + /* check that we are receiving fresh data from the FMU */ + if (!system_state.fmu_data_received) { + fmu_input_drops++; + + /* too many frames without FMU input, time to go to failsafe */ + if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { + system_state.mixer_use_fmu = false; + } + } else { + fmu_input_drops = 0; + system_state.fmu_data_received = false; + } + + } else if (system_state.rc_channels > 0) { + /* we have control data from an R/C input */ + control_count = system_state.rc_channels; + control_values = &system_state.rc_channel_data[0]; + + } else { + /* we have no control input */ + control_count = 0; + } + + /* + * Tickle each mixer, if we have control data. + */ + if (control_count > 0) { + for (i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) { + mixer_update(i, control_values, control_count); + + /* + * If we are armed, update the servo output. + */ + if (system_state.armed) + ioctl(mixer_servo_fd, PWM_SERVO_SET(i), mixers[i].current_value); + } + + } + + /* + * Decide whether the servos should be armed right now. + */ + should_arm = system_state.armed && (control_count > 0); + if (should_arm && !mixer_servos_armed) { + /* need to arm, but not armed */ + ioctl(mixer_servo_fd, PWM_SERVO_ARM, 0); + mixer_servos_armed = true; + + } else if (!should_arm && mixer_servos_armed) { + /* armed but need to disarm*/ + ioctl(mixer_servo_fd, PWM_SERVO_DISARM, 0); + mixer_servos_armed = false; + } +} + +static void +mixer_get_rc_input(void) +{ + ssize_t len; + + /* + * Pull channel data from the message queue into the system state structure. + * + */ + len = mq_receive(input_queue, &system_state.rc_channel_data, sizeof(system_state.rc_channel_data), NULL); + + /* + * If we have data, update the count and status. + */ + if (len > 0) { + system_state.rc_channels = len / sizeof(system_state.rc_channel_data[0]); + mixer_input_drops = 0; + + system_state.fmu_report_due = true; + } else { + /* + * No data; count the 'frame drops' and once we hit the limit + * assume that we have lost input. + */ + if (mixer_input_drops < MIXER_INPUT_DROP_LIMIT) { + mixer_input_drops++; + + /* if we hit the limit, stop pretending we have input and let the FMU know */ + if (mixer_input_drops == MIXER_INPUT_DROP_LIMIT) { + system_state.rc_channels = 0; + system_state.fmu_report_due = true; + } + } + } +} + +static void +mixer_update(int mixer, uint16_t *inputs, int input_count) +{ + /* simple passthrough for now */ + if (mixer < input_count) { + mixers[mixer].current_value = inputs[mixer]; + } else { + mixers[mixer].current_value = 0; + } +} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h new file mode 100644 index 000000000..92558a61d --- /dev/null +++ b/apps/px4io/protocol.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 PX4FMU <-> PX4IO messaging protocol. + * + * This initial version of the protocol is very simple; each side transmits a + * complete update with each frame. This avoids the sending of many small + * messages and the corresponding complexity involved. + */ + +/* + * XXX MUST BE KEPT IN SYNC WITH THE VERSION IN PX4FMU UNTIL + * TREES ARE MERGED. + */ + +#define PX4IO_OUTPUT_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_RELAY_CHANNELS 2 + +/* command from FMU to IO */ +struct px4io_command { + uint16_t f2i_magic; +#define F2I_MAGIC 0x636d + + uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; + bool relay_state[PX4IO_RELAY_CHANNELS]; + bool arm_ok; +} __attribute__((packed)); + +/* report from IO to FMU */ +struct px4io_report { + uint16_t i2f_magic; +#define I2F_MAGIC 0x7570 + + uint16_t rc_channel[PX4IO_INPUT_CHANNELS]; + bool armed; + uint8_t channel_count; +} __attribute__((packed)); diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c new file mode 100644 index 000000000..44b62f3f9 --- /dev/null +++ b/apps/px4io/px4io.c @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * 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 Top-level logic for the PX4IO module. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <debug.h> +#include <stdlib.h> +#include <errno.h> + +#include <nuttx/clock.h> + +#include <arch/board/up_boardinitialize.h> +#include <arch/board/drv_gpio.h> +#include <arch/board/drv_ppm_input.h> +#include <arch/board/up_hrt.h> + +#include "px4io.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int user_start(int argc, char *argv[]); + +/**************************************************************************** + * user_start + ****************************************************************************/ + +struct sys_state_s system_state; +int gpio_fd; + +static const char cursor[] = {'|', '/', '-', '\\'}; + +static const char *rc_input_mq_name = "rc_input"; + +static struct hrt_call timer_tick_call; +volatile int timers[TIMER_NUM_TIMERS]; +static void timer_tick(void *arg); + +int user_start(int argc, char *argv[]) +{ + int cycle = 0; + bool heartbeat = false; + bool failsafe = false; + + /* Do board init */ + (void)up_boardinitialize(); + + /* print some startup info */ + lib_lowprintf("\nPX4IO: starting\n"); + struct mallinfo minfo = mallinfo(); + lib_lowprintf("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + + /* start the software timer service */ + hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL); + + /* Open the GPIO driver so we can do LEDs and the like. */ + gpio_fd = open("/dev/gpio", 0); + ASSERTCODE((gpio_fd >= 0), A_GPIO_OPEN_FAIL); + + /* default all the LEDs to off while we start */ + LED_AMBER(heartbeat); + LED_BLUE(failsafe); + LED_SAFETY(false); + + /* turn on servo power */ + POWER_SERVO(true); + + /* configure the PPM input driver */ + ppm_input_init(rc_input_mq_name); + + /* start the mixer */ + mixer_init(rc_input_mq_name); + + /* start the safety switch handler */ + safety_init(); + + /* init the FMU link */ + comms_init(); + + /* set up some timers for the main loop */ + timers[TIMER_BLINK_AMBER] = 250; /* heartbeat blink @ 2Hz */ + timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */ + + /* + * Main loop servicing communication with FMU + */ + while(true) { + + /* check for communication from FMU, send updates */ + comms_check(); + + /* blink the heartbeat LED */ + if (timers[TIMER_BLINK_AMBER] == 0) { + timers[TIMER_BLINK_AMBER] = 250; + LED_AMBER((heartbeat = !heartbeat)); + } + + /* blink the failsafe LED if we don't have FMU input */ + if (!system_state.mixer_use_fmu) { + if (timers[TIMER_BLINK_BLUE] == 0) { + timers[TIMER_BLINK_BLUE] = 125; + LED_BLUE((failsafe = !failsafe)); + } + } else { + LED_BLUE((failsafe = false)); + } + + /* print some simple status */ + if (timers[TIMER_STATUS_PRINT] == 0) { + timers[TIMER_STATUS_PRINT] = 1000; + lib_lowprintf("%c %s | %s | %s | C=%d \r", + cursor[cycle++ & 3], + (system_state.armed ? "ARMED" : "SAFE"), + (system_state.rc_channels ? "RC OK" : "NO RC"), + (system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"), + system_state.rc_channels + ); + } + + } + + /* Should never reach here */ + return ERROR; +} + +static void +timer_tick(void *arg) +{ + for (unsigned i = 0; i < TIMER_NUM_TIMERS; i++) + if (timers[i] > 0) + timers[i]--; +} diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h new file mode 100644 index 000000000..d90f7e36b --- /dev/null +++ b/apps/px4io/px4io.h @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * 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 General defines and structures for the PX4IO module firmware. + */ + +#include <arch/board/drv_gpio.h> +#include "protocol.h" + +/* + * Constants and limits. + */ +#define MAX_CONTROL_CHANNELS 12 +#define IO_SERVO_COUNT 8 + +/* + * System state structure. + */ +struct sys_state_s +{ + + bool armed; /* actually armed */ + bool arm_ok; /* FMU says OK to arm */ + + /* + * Data from the remote control input(s) + */ + int rc_channels; + uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS]; + + /* + * Control signals from FMU. + */ + uint16_t fmu_channel_data[PX4IO_OUTPUT_CHANNELS]; + + /* + * Relay controls + */ + bool relays[PX4IO_RELAY_CHANNELS]; + + /* + * If true, we are using the FMU controls. + */ + bool mixer_use_fmu; + + /* + * If true, state that should be reported to FMU has been updated. + */ + bool fmu_report_due; + + /* + * If true, new control data from the FMU has been received. + */ + bool fmu_data_received; +}; + +extern struct sys_state_s system_state; + +/* + * Software countdown timers. + * + * Each timer counts down to zero at one tick per ms. + */ +#define TIMER_BLINK_AMBER 0 +#define TIMER_BLINK_BLUE 1 +#define TIMER_STATUS_PRINT 2 +#define TIMER_SANITY 7 +#define TIMER_NUM_TIMERS 8 +extern volatile int timers[TIMER_NUM_TIMERS]; + +/* + * GPIO handling. + */ +extern int gpio_fd; + +#define POWER_SERVO(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_POWER), (_s)) +#define POWER_ACC1(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC1), (_s)) +#define POWER_ACC2(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC2), (_s)) +#define POWER_RELAY1(_s) ioctl(gpio_fd, GPIO_SET(GPIO_RELAY1, (_s)) +#define POWER_RELAY2(_s) ioctl(gpio_fd, GPIO_SET(GPIO_RELAY2, (_s)) + +#define LED_AMBER(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_AMBER), !(_s)) +#define LED_BLUE(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_BLUE), !(_s)) +#define LED_SAFETY(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_SAFETY), !(_s)) + +#define OVERCURRENT_ACC ioctl(gpio_fd, GPIO_GET(GPIO_ACC_OVERCURRENT), 0) +#define OVERCURRENT_SERVO ioctl(gpio_fd, GPIO_GET(GPIO_SERVO_OVERCURRENT), 0) +#define BUTTON_SAFETY ioctl(gpio_fd, GPIO_GET(GPIO_SAFETY_BUTTON), 0) + +/* + * Mixer + */ +extern int mixer_init(const char *mq_name); + +/* + * Safety switch/LED. + */ +extern void safety_init(void); + +/* + * FMU communications + */ +extern void comms_init(void); +extern void comms_check(void); + +/* + * Assertion codes + */ +#define A_GPIO_OPEN_FAIL 100 +#define A_SERVO_OPEN_FAIL 101 +#define A_INPUTQ_OPEN_FAIL 102 diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c new file mode 100644 index 000000000..895c33806 --- /dev/null +++ b/apps/px4io/safety.c @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 Safety button logic. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <debug.h> +#include <stdlib.h> +#include <errno.h> + +#include <nuttx/clock.h> + +#include <arch/board/up_boardinitialize.h> +#include <arch/board/drv_gpio.h> +#include <arch/board/drv_ppm_input.h> +#include <arch/board/up_hrt.h> + +#include "px4io.h" + +static struct hrt_call arming_call; + +/* + * Count the number of times in a row that we see the arming button + * held down. + */ +static unsigned arm_counter; +#define ARM_COUNTER_THRESHOLD 10 + +static bool safety_led_state; + +static void safety_check_button(void *arg); + +void +safety_init(void) +{ + /* arrange for the button handler to be called at 10Hz */ + hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} + +static void +safety_check_button(void *arg) +{ + /* + * Debounce the safety button, change state if it has been held for long enough. + * + * Ignore the button if FMU has not said it's OK to arm yet. + */ + if (BUTTON_SAFETY && system_state.arm_ok) { + if (arm_counter < ARM_COUNTER_THRESHOLD) { + arm_counter++; + } else if (arm_counter == ARM_COUNTER_THRESHOLD) { + /* change our armed state and notify the FMU */ + system_state.armed = !system_state.armed; + arm_counter++; + system_state.fmu_report_due = true; + } + } else { + arm_counter = 0; + } + + /* when armed, toggle the LED; when safe, leave it on */ + if (system_state.armed) { + safety_led_state = !safety_led_state; + } else { + safety_led_state = true; + } + LED_SAFETY(safety_led_state); +} diff --git a/apps/sdlog/Makefile b/apps/sdlog/Makefile new file mode 100644 index 000000000..1e5abdb55 --- /dev/null +++ b/apps/sdlog/Makefile @@ -0,0 +1,44 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# sdlog Application +# + +APPNAME = sdlog +PRIORITY = SCHED_PRIORITY_DEFAULT - 1 +STACKSIZE = 3000 + +INCLUDES = $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/sdlog/mfile.template b/apps/sdlog/mfile.template new file mode 100644 index 000000000..0f7ebd045 --- /dev/null +++ b/apps/sdlog/mfile.template @@ -0,0 +1,136 @@ +%% Import logfiles + +if (exist ('OCTAVE_VERSION', 'builtin')) + % Octave + graphics_toolkit ("fltk") +else + % Matlab +end + +% define log file and GPSs +logfileFolder = 'logfiles'; +fileName = 'all'; +fileEnding = 'px4log'; + +numberOfLogTypes = length(logTypes); + +path = [fileName,'.', fileEnding]; +fid = fopen(path, 'r'); + +% get file length +fseek(fid, 0,'eof'); +fileLength = ftell(fid); +fseek(fid, 0,'bof'); + +% get length of one block +blockLength = 4; % check: $$$$ +for i=1:numberOfLogTypes + blockLength = blockLength + logTypes{i}.type_bytes*logTypes{i}.number_of_array; +end + +% determine number of entries +entries = fileLength / blockLength; + + +% import data +offset = 0; +for i=1:numberOfLogTypes + + data.(genvarname(logTypes{i}.data)).(genvarname(logTypes{i}.variable_name)) = transpose(fread(fid,[logTypes{i}.number_of_array,entries],[num2str(logTypes{i}.number_of_array),'*',logTypes{i}.type_name,'=>',logTypes{i}.type_name],blockLength-logTypes{i}.type_bytes*logTypes{i}.number_of_array)); + offset = offset + logTypes{i}.type_bytes*logTypes{i}.number_of_array; + fseek(fid, offset,'bof'); + + progressPercentage = i/numberOfLogTypes*100; + fprintf('%3.0f%%',progressPercentage); +end + + +%% Plots + +figure +plot(data.sensors_raw.timestamp,data.sensors_raw.gyro_raw) + +figure +plot(data.sensors_raw.timestamp,data.sensors_raw.accelerometer_raw) + +%% Check for lost data + +% to detect lost frames (either when logging to sd card or if no new data is +% data is available for some time) +diff_counter = diff(data.sensors_raw.gyro_raw_counter); +figure +plot(diff_counter) + +% to detect how accurate the timing was +diff_timestamp = diff(data.sensors_raw.timestamp); + +figure +plot(diff_timestamp) + +%% Export to file for google earth + + +if(isfield(data.gps,'lat') && isfield(data.gps,'lon') && isfield(data.gps,'alt')) + + % extract coordinates and height where they are not zero + maskWhereNotZero = ((data.gps.lon ~= 0 & data.gps.lat ~= 0 ) & data.gps.alt ~= 0); + + % plot + figure + plot3(data.gps.lon(maskWhereNotZero),data.gps.lat(maskWhereNotZero),data.gps.alt(maskWhereNotZero)) + + + % create a kml file according to https://developers.google.com/kml/documentation/kml_tut + % also see https://support.google.com/earth/bin/answer.py?hl=en&answer=148072&topic=2376756&ctx=topic + + % open file and overwrite content + fileId = fopen('gps_path.kml','w+'); + + % define strings that should be written to file + fileStartDocumentString = ['<?xml version="1.0" encoding="UTF-8"?><kml xmlns="http://www.opengis.net/kml/2.2"><Document><name>Paths</name><description>Path</description>']; + + fileStyleString = '<Style id="blueLinebluePoly"><LineStyle><color>7fff0000</color><width>4</width></LineStyle><PolyStyle><color>7fff0000</color></PolyStyle></Style>'; + + filePlacemarkString = '<Placemark><name>Absolute Extruded</name><description>Transparent blue wall with blue outlines</description><styleUrl>#blueLinebluePoly</styleUrl><LineString><extrude>1</extrude><tessellate>1</tessellate><altitudeMode>absolute</altitudeMode><coordinates>'; + + fileEndPlacemarkString = '</coordinates></LineString></Placemark>'; + fileEndDocumentString = '</Document></kml>'; + + % start writing to file + fprintf(fileId,fileStartDocumentString); + + fprintf(fileId,fileStyleString); + fprintf(fileId,filePlacemarkString); + + + lonTemp = double(data.gps.lon(maskWhereNotZero))/1E7; + latTemp = double(data.gps.lat(maskWhereNotZero))/1E7; + altTemp = double(data.gps.alt(maskWhereNotZero))/1E3 + 100.0; % in order to see the lines above ground + + % write coordinates to file + for k=1:length(data.gps.lat(maskWhereNotZero)) + if(mod(k,10)==0) + fprintf(fileId,'%.10f,%.10f,%.10f\\n',lonTemp(k),latTemp(k),altTemp(k)); + end + end + + % write end placemark + fprintf(fileId,fileEndPlacemarkString); + + % write end of file + fprintf(fileId,fileEndDocumentString); + + % close file, it should now be readable in Google Earth using File -> Open + fclose(fileId); + +end + +if(isfield(data.position,'lat') && isfield(data.position,'lon') && isfield(data.position,'alt')) + + % extract coordinates and height where they are not zero + maskWhereNotZero = ((data.position.lon ~= 0 & data.position.lat ~= 0 ) & data.position.alt ~= 0); + + % plot + figure + plot3(data.position.lon(maskWhereNotZero),data.position.lat(maskWhereNotZero),data.position.alt(maskWhereNotZero)) +end diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c new file mode 100644 index 000000000..1b87aab77 --- /dev/null +++ b/apps/sdlog/sdlog.c @@ -0,0 +1,442 @@ +/**************************************************************************** + * examples/hello/main.c + * + * Copyright (C) 2008, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + + +#include <time.h> +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <stdint.h> +#include <stdlib.h> +#include <string.h> +#include <float.h> +#include <stdbool.h> +#include <sys/mount.h> +#include <sys/stat.h> +#include <time.h> +#include <pthread.h> +#include <fcntl.h> +#include <sys/prctl.h> +#include <errno.h> + +#include "sdlog.h" +#include "sdlog_generated.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +__EXPORT int sdlog_main(int argc, char *argv[]); + +const char *src = "/dev/mmcsd0"; +const char *trgt = "/fs/microsd"; +const char *type = "vfat"; +const char *logfile_end = ".px4log"; +const char *mfile_end = ".m"; +char folder_path[64]; + +#define SDLOG_LED_PRIORITY LED_REQUEST_PRIORITY_MAX +#define BUFFER_BYTES 1000 // length of buffer +#define SAVE_EVERY_BYTES 2000 +#define MAX_MOUNT_TRIES 5 + +static void sdlog_sig_handler(int signo, siginfo_t *info, void *ucontext); // is executed when SIGUSR1 is received +bool sdlog_sigusr1_rcvd; // if this is set to true through SIGUSR1, sdlog will terminate + +static pthread_t logbuffer_thread; // thread to copy log values to the buffer +static void *logbuffer_loop(void *arg); + + +uint8_t *buffer_start; // always points to the very beginning +uint8_t *buffer_end; // always points to the very end +uint8_t *buffer_cursor_start; // points to the start of the current buffer +uint8_t *buffer_cursor_end; // points to the end of the current buffer +size_t buffer_bytes_used; // amount stored in the buffer at the moment + +uint32_t bytes_recv; // to count bytes received and written to the sdcard +uint32_t bytes_sent; // to count the bytes written to the buffer + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int file_exist(const char *filename) +{ + struct stat buffer; + return (stat(filename, &buffer) == 0); +} + + +int sdlog_main(int argc, char *argv[]) +{ + // print text + printf("[sdlog] initialized\n"); + usleep(10000); + + sdlog_sigusr1_rcvd = false; + + led_request_t amber_on_request = {.led = LED_AMBER, .priority = SDLOG_LED_PRIORITY, .request_type = LED_REQUEST_ON}; + led_request_t amber_off_request = {.led = LED_AMBER, .priority = SDLOG_LED_PRIORITY, .request_type = LED_REQUEST_OFF}; + amber_on_request.pid = getpid(); + amber_off_request.pid = getpid(); + + /* signal handler to abort when low voltage occurs */ + struct sigaction act; + struct sigaction oact; + act.sa_sigaction = sdlog_sig_handler; + (void)sigemptyset(&act.sa_mask); + (void)sigaddset(&act.sa_mask, SIGUSR1); + (void)sigaction(SIGUSR1, &act, &oact); + + uint8_t mount_counter = 0; + + commander_state_machine_t current_system_state; + + global_data_send_led_request(&amber_on_request); + + if (file_exist(trgt) == 1) { + printf("[sdlog] Mount already available at %s\n", trgt); + global_data_send_led_request(&amber_off_request); + + } else { + printf("[sdlog] Mount not available yet, trying to mount...\n"); + + /* mostly the first mount fails, sometimes it helps to press the card onto the board! */ + while (mount(src, trgt, type, 0, "") != 0) { + /* abort if kill signal is received */ + if (sdlog_sigusr1_rcvd == true) { + return 0; + } + + //make sure were not airborne + global_data_trylock(&global_data_sys_status->access_conf); + current_system_state = global_data_sys_status->state_machine; + global_data_unlock(&global_data_sys_status->access_conf); + + if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT || current_system_state == SYSTEM_STATE_GROUND_ERROR) { + usleep(1000000); + printf("[sdlog] ERROR: Failed to mount SD card (attempt %d of %d), reason: %s\n", mount_counter + 1, MAX_MOUNT_TRIES, strerror((int)*get_errno_ptr())); + global_data_send_led_request(&amber_off_request); + mount_counter++; + + } else { + printf("[sdlog] WARNING: Not mounting SD card in flight!\n"); + printf("[sdlog] ending now...\n"); + fflush(stdout); + return 0; + } + + if (mount_counter >= MAX_MOUNT_TRIES) { + printf("[sdlog] ERROR: SD card could not be mounted!\n"); + printf("[sdlog] ending now...\n"); + fflush(stdout); + return 0; + } + } + + printf("[sdlog] Mount created at %s...\n", trgt); + global_data_send_led_request(&amber_off_request); + } + + + /* make folder on sdcard */ + uint16_t foldernumber = 1; // start with folder 0001 + int mkdir_ret; + + /* look for the next folder that does not exist */ + while (foldernumber < MAX_NO_LOGFOLDER) { + /* set up file path: e.g. /mnt/sdcard/logfile0001.txt */ + sprintf(folder_path, "%s/session%04u", trgt, foldernumber); + mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); + /* the result is -1 if the folder exists */ + + if (mkdir_ret == 0) { + /* folder does not exist */ + break; + + } else if (mkdir_ret == -1) { + /* folder exists already */ + foldernumber++; // to try next time + continue; + + } else { + printf("[sdlog] ERROR: Failed creating new folder: %s\n", strerror((int)*get_errno_ptr())); + return -1; + } + } + + if (foldernumber >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + printf("[sdlog] ERROR: all %d possible folders exist already\n", MAX_NO_LOGFOLDER); + return -1; + } + + + /* write m-file for evaluation in Matlab */ + FILE *mfile; + char mfile_path[64] = ""; // string to hold the path to the mfile + const char *mfilename = "sdlog_eval"; + + sprintf(mfile_path, "%s/%s%s", folder_path, mfilename, mfile_end); + + if (NULL == (mfile = fopen(mfile_path, "w"))) { + printf("[sdlog] ERROR: opening %s failed: %s\n", mfile_path, strerror((int)*get_errno_ptr())); + } + + /* write file contents generated using updatesdlog.sh and mfile.template in nuttx/configs/px4fmu/include */ + fwrite(MFILE_STRING, sizeof(MFILE_STRING), 1, mfile); + fprintf(mfile, "\n"); + fclose(mfile); + printf("[sdlog] m-file written to sdcard\n"); + + + /* create the ringbuffer */ + uint8_t buffer[BUFFER_BYTES]; + buffer_start = buffer; + buffer_cursor_start = buffer; + buffer_cursor_end = buffer; + buffer_end = buffer + BUFFER_BYTES; + + /* create loop to write log data in a ringbuffer */ + pthread_attr_t logbuffer_attr; + pthread_attr_init(&logbuffer_attr); + pthread_attr_setstacksize(&logbuffer_attr, 2400); + pthread_create(&logbuffer_thread, &logbuffer_attr, logbuffer_loop, NULL); + + /* create logfile */ + FILE *logfile; + char logfile_path[64] = ""; // string to hold the path to the logfile + const char *logfilename = "all"; + + /* set up file path: e.g. /mnt/sdcard/session0001/gpslog.txt */ + sprintf(logfile_path, "%s/%s%s", folder_path, logfilename, logfile_end); + + + if (NULL == (logfile = fopen(logfile_path, "wb"))) { + printf("[sdlog] opening %s failed: %s\n", logfile_path, strerror((int)*get_errno_ptr())); + } + +// else +// { +// printf("[sdlog] opening %s was successful\n",logfile_path); +// } + + int logfile_no = fileno(logfile); + + bytes_recv = 0; /**< count all bytes that were received and written to the sdcard */ + size_t ret_write; /**< last amount written to sdcard */ + size_t target_write; /**< desired amount to write to sdcard */ + int ret_fsync; /**< return value of fsync() system call */ + int error_count = 0; /**< number of continous errors (one successful write resets it) */ + + /* Start logging */ + while (1) { + /* write as soon as content is in the buffer */ + while (buffer_bytes_used > 1) { + /* case where the data is not wrapped in the buffer */ + if (buffer_cursor_start < buffer_cursor_end) { + /* write all available data */ + target_write = (size_t)(buffer_cursor_end - buffer_cursor_start); + + if (0 <= (ret_write = fwrite(buffer_cursor_start, 1, target_write, logfile))) { + /* decrease the amount stored in the buffer, normally to 0 */ + buffer_bytes_used -= ret_write; + /* set new cursor position: wrap it in case it falls out of the buffer */ + buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES; + bytes_recv += ret_write; + error_count = 0; + + } else { + error_count++; + printf("[sdlog] ERROR: Write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr())); + } + } + + /* case where the content is wrapped around the buffer */ + else if (buffer_cursor_start > buffer_cursor_end) { + /* write data until the end of the buffer */ + target_write = (size_t)(buffer_end - buffer_cursor_start); + + if (0 <= (ret_write = fwrite(buffer_cursor_start, 1, target_write, logfile))) { + /* decrease the amount stored in the buffer */ + buffer_bytes_used -= ret_write; + /* set new cursor position: wrap it in case it falls out of the buffer */ + buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES; + bytes_recv += ret_write; + error_count = 0; + + } else { + error_count++; + printf("[sdlog] ERROR: Write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr())); + } + + /* write the rest of the data at the beginning of the buffer */ + target_write = (size_t)(buffer_cursor_end - buffer_start); + + if (0 <= (ret_write = fwrite(buffer_start, 1, target_write, logfile))) { + /* decrease the amount stored in the buffer, now normally to 0 */ + buffer_bytes_used -= ret_write; + /* set new cursor position: wrap it in case it falls out of the buffer */ + buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES; + bytes_recv += ret_write; + error_count = 0; + + } else { + error_count++; + printf("[sdlog] ERROR: write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr())); + } + + } else { + error_count++; + printf("[sdlog] ERROR: dropped data\n"); + } + +// if(bytes_recv>500000) +// { +// goto finished; +// } + } + + + /* save and exit if we received signal 1 or have a permanent error */ + if (sdlog_sigusr1_rcvd == true || error_count > 100) { + fclose(logfile); + umount(trgt); + + if (error_count > 100) { + return ERROR; + + } else { + return OK; + } + } + + /* save file from time to time */ + else if ((bytes_recv > 0 && bytes_recv % SAVE_EVERY_BYTES == 0)) { + if ((ret_fsync = fsync(logfile_no)) != OK) { + printf("[sdlog] ERROR: sync fail: #%d, %s\n", ret_fsync, strerror((int)*get_errno_ptr())); + } + } + +// else if(bytes_recv > 2*SAVE_EVERY_BYTES) +// { +// goto finished; +// } + /* sleep, not to block everybody else */ + usleep(1000); + } + +//finished: +// +// /* at the moment we should not end here */ +// fclose(logfile); +// printf("[sdlog] logfile saved\n"); +// umount(trgt); +// printf("[sdlog] ending...\n"); +// fflush(stdout); + + return 0; +} + +/* is executed when SIGUSR1 is raised (to terminate the app) */ +static void sdlog_sig_handler(int signo, siginfo_t *info, void *ucontext) +{ + sdlog_sigusr1_rcvd = true; +} + + +static void *logbuffer_loop(void *arg) +{ + /* set name for this pthread */ + prctl(PR_SET_NAME, "sdlog logbuffer", getpid()); + + bytes_sent = 0; // count all bytes written to the buffer + + //size_t block_length = sizeof(global_data_sensors_raw_t) - 4*sizeof(access_conf_t); + size_t first_block_length; + size_t second_block_length; + buffer_bytes_used = 0; + + log_block_t log_block = {.check = {'$', '$', '$', '$'}}; + + size_t block_length = sizeof(log_block_t); +// printf("Block length is: %u",(uint16_t)block_length); + +// uint16_t test_counter = 0; + + /* start copying data to buffer */ + while (1) { + copy_block(&log_block); + + /* no more free space in buffer */ + if (buffer_bytes_used + (uint16_t)block_length > BUFFER_BYTES) { + printf("[sdlog] buf full, skipping\n"); + } + + /* data needs to be wrapped around ringbuffer*/ + else if (buffer_cursor_end + block_length >= buffer_end) { + /* first block length is from cursor until the end of the buffer */ + first_block_length = (size_t)(buffer_end - buffer_cursor_end); + /* second block length is the rest */ + second_block_length = block_length - first_block_length; + /* copy the data, not the pointer conversion */ + memcpy(buffer_cursor_end, ((uint8_t *) & (log_block)), first_block_length); + memcpy(buffer_start, ((uint8_t *) & (log_block)) + first_block_length, second_block_length); + /* set the end position of the cursor */ + buffer_cursor_end = buffer_start + second_block_length; + /* increase the amount stored in the buffer */ + buffer_bytes_used += block_length; + bytes_sent += block_length; + } + + /* data does not need to be wrapped around */ + else { + /* copy the whole block in one step */ + memcpy(buffer_cursor_end, ((uint8_t *) & (log_block)), block_length); + /* set the cursor to 0 of the buffer instead of the end */ + buffer_cursor_end = buffer_start + (buffer_cursor_end - buffer_start + block_length) % BUFFER_BYTES; + /* increase the amount stored in the buffer */ + buffer_bytes_used += block_length; + bytes_sent += block_length; + } + + /* also end the pthread when we receive a SIGUSR1 */ + if (sdlog_sigusr1_rcvd == true) { + break; + } + } + + return NULL; +} diff --git a/apps/sdlog/sdlog.h b/apps/sdlog/sdlog.h new file mode 100644 index 000000000..8012af177 --- /dev/null +++ b/apps/sdlog/sdlog.h @@ -0,0 +1,17 @@ +/* + * sdlog.h + * + * Created on: Mar 8, 2012 + * Author: romanpatscheider + */ + +#ifndef SENSORS_H_ +#define SENSORS_H_ +/**************************************************************************** + * Included Files + ****************************************************************************/ + + +#define MAX_NO_LOGFOLDER 3000 // tested up to here... + +#endif /* SENSORS_H_ */ diff --git a/apps/sdlog/sdlog_generated.h b/apps/sdlog/sdlog_generated.h new file mode 100644 index 000000000..ca16ef908 --- /dev/null +++ b/apps/sdlog/sdlog_generated.h @@ -0,0 +1,238 @@ +/* This file is autogenerated in nuttx/configs/px4fmu/include/updatesdlog.py */ + +#ifndef SDLOG_GENERATED_H_ +#define SDLOG_GENERATED_H_ + + +typedef struct { + uint64_t sensors_raw_timestamp; + int16_t sensors_raw_gyro_raw[3]; + uint16_t sensors_raw_gyro_raw_counter; + int16_t sensors_raw_accelerometer_raw[3]; + uint16_t sensors_raw_accelerometer_raw_counter; + float attitude_roll; + float attitude_pitch; + float attitude_yaw; + float position_lat; + float position_lon; + float position_alt; + float position_relative_alt; + float position_vx; + float position_vy; + float position_vz; + int32_t gps_lat; + int32_t gps_lon; + int32_t gps_alt; + uint16_t gps_eph; + float ardrone_control_setpoint_thrust_cast; + float ardrone_control_setpoint_attitude[3]; + float ardrone_control_position_control_output[3]; + float ardrone_control_attitude_setpoint_navigationframe_from_positioncontroller[3]; + char check[4]; +} __attribute__((__packed__)) log_block_t; + +void copy_block(log_block_t *log_block) +{ + if (global_data_wait(&global_data_sensors_raw->access_conf_rate_low) == 0) { + memcpy(&log_block->sensors_raw_timestamp, &global_data_sensors_raw->timestamp, sizeof(uint64_t) * 1); + memcpy(&log_block->sensors_raw_gyro_raw, &global_data_sensors_raw->gyro_raw, sizeof(int16_t) * 3); + memcpy(&log_block->sensors_raw_gyro_raw_counter, &global_data_sensors_raw->gyro_raw_counter, sizeof(uint16_t) * 1); + memcpy(&log_block->sensors_raw_accelerometer_raw, &global_data_sensors_raw->accelerometer_raw, sizeof(int16_t) * 3); + memcpy(&log_block->sensors_raw_accelerometer_raw_counter, &global_data_sensors_raw->accelerometer_raw_counter, sizeof(uint16_t) * 1); + + if (global_data_trylock(&global_data_attitude->access_conf) == 0) { + memcpy(&log_block->attitude_roll, &global_data_attitude->roll, sizeof(float) * 1); + memcpy(&log_block->attitude_pitch, &global_data_attitude->pitch, sizeof(float) * 1); + memcpy(&log_block->attitude_yaw, &global_data_attitude->yaw, sizeof(float) * 1); + global_data_unlock(&global_data_attitude->access_conf); + } + + if (global_data_trylock(&global_data_position->access_conf) == 0) { + memcpy(&log_block->position_lat, &global_data_position->lat, sizeof(float) * 1); + memcpy(&log_block->position_lon, &global_data_position->lon, sizeof(float) * 1); + memcpy(&log_block->position_alt, &global_data_position->alt, sizeof(float) * 1); + memcpy(&log_block->position_relative_alt, &global_data_position->relative_alt, sizeof(float) * 1); + memcpy(&log_block->position_vx, &global_data_position->vx, sizeof(float) * 1); + memcpy(&log_block->position_vy, &global_data_position->vy, sizeof(float) * 1); + memcpy(&log_block->position_vz, &global_data_position->vz, sizeof(float) * 1); + global_data_unlock(&global_data_position->access_conf); + } + + if (global_data_trylock(&global_data_gps->access_conf) == 0) { + memcpy(&log_block->gps_lat, &global_data_gps->lat, sizeof(int32_t) * 1); + memcpy(&log_block->gps_lon, &global_data_gps->lon, sizeof(int32_t) * 1); + memcpy(&log_block->gps_alt, &global_data_gps->alt, sizeof(int32_t) * 1); + memcpy(&log_block->gps_eph, &global_data_gps->eph, sizeof(uint16_t) * 1); + global_data_unlock(&global_data_gps->access_conf); + } + + if (global_data_trylock(&global_data_ardrone_control->access_conf) == 0) { + memcpy(&log_block->ardrone_control_setpoint_thrust_cast, &global_data_ardrone_control->setpoint_thrust_cast, sizeof(float) * 1); + memcpy(&log_block->ardrone_control_setpoint_attitude, &global_data_ardrone_control->setpoint_attitude, sizeof(float) * 3); + memcpy(&log_block->ardrone_control_position_control_output, &global_data_ardrone_control->position_control_output, sizeof(float) * 3); + memcpy(&log_block->ardrone_control_attitude_setpoint_navigationframe_from_positioncontroller, &global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller, sizeof(float) * 3); + global_data_unlock(&global_data_ardrone_control->access_conf); + } + + global_data_unlock(&global_data_sensors_raw->access_conf_rate_low); + } +} +#define MFILE_STRING "% This file is autogenerated in updatesdlog.py and mfile.template in apps/sdlog\n\ +%% Define logged values \n\ +\n\ +logTypes = {};\n\ +logTypes{end+1} = struct('data','sensors_raw','variable_name','timestamp','type_name','uint64','type_bytes',8,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','sensors_raw','variable_name','gyro_raw','type_name','int16','type_bytes',2,'number_of_array',3);\n\ +logTypes{end+1} = struct('data','sensors_raw','variable_name','gyro_raw_counter','type_name','uint16','type_bytes',2,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','sensors_raw','variable_name','accelerometer_raw','type_name','int16','type_bytes',2,'number_of_array',3);\n\ +logTypes{end+1} = struct('data','sensors_raw','variable_name','accelerometer_raw_counter','type_name','uint16','type_bytes',2,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','attitude','variable_name','roll','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','attitude','variable_name','pitch','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','attitude','variable_name','yaw','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','position','variable_name','lat','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','position','variable_name','lon','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','position','variable_name','alt','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','position','variable_name','relative_alt','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','position','variable_name','vx','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','position','variable_name','vy','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','position','variable_name','vz','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','gps','variable_name','lat','type_name','int32','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','gps','variable_name','lon','type_name','int32','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','gps','variable_name','alt','type_name','int32','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','gps','variable_name','eph','type_name','uint16','type_bytes',2,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','ardrone_control','variable_name','setpoint_thrust_cast','type_name','float','type_bytes',4,'number_of_array',1);\n\ +logTypes{end+1} = struct('data','ardrone_control','variable_name','setpoint_attitude','type_name','float','type_bytes',4,'number_of_array',3);\n\ +logTypes{end+1} = struct('data','ardrone_control','variable_name','position_control_output','type_name','float','type_bytes',4,'number_of_array',3);\n\ +logTypes{end+1} = struct('data','ardrone_control','variable_name','attitude_setpoint_navigationframe_from_positioncontroller','type_name','float','type_bytes',4,'number_of_array',3);\n\ +\ +%% Import logfiles\n\ +\n\ +% define log file and GPSs\n\ +logfileFolder = 'logfiles';\n\ +fileName = 'all';\n\ +fileEnding = 'px4log';\n\ +\n\ +numberOfLogTypes = length(logTypes);\n\ +\n\ +path = [fileName,'.', fileEnding];\n\ +fid = fopen(path, 'r');\n\ +\n\ +% get file length\n\ +fseek(fid, 0,'eof');\n\ +fileLength = ftell(fid);\n\ +fseek(fid, 0,'bof');\n\ +\n\ +% get length of one block\n\ +blockLength = 4; % check: $$$$\n\ +for i=1:numberOfLogTypes\n\ + blockLength = blockLength + logTypes{i}.type_bytes*logTypes{i}.number_of_array;\n\ +end\n\ +\n\ +% determine number of entries\n\ +entries = fileLength / blockLength;\n\ +\n\ +\n\ +% import data\n\ +offset = 0;\n\ +for i=1:numberOfLogTypes\n\ + \n\ + data.(genvarname(logTypes{i}.data)).(genvarname(logTypes{i}.variable_name)) = transpose(fread(fid,[logTypes{i}.number_of_array,entries],[num2str(logTypes{i}.number_of_array),'*',logTypes{i}.type_name,'=>',logTypes{i}.type_name],blockLength-logTypes{i}.type_bytes*logTypes{i}.number_of_array));\n\ + offset = offset + logTypes{i}.type_bytes*logTypes{i}.number_of_array;\n\ + fseek(fid, offset,'bof');\n\ + \n\ + progressPercentage = i/numberOfLogTypes*100;\n\ + fprintf('%3.0f%%',progressPercentage);\n\ +end\n\ +\n\ +\n\ +%% Plots\n\ +\n\ +figure\n\ +plot(data.sensors_raw.timestamp,data.sensors_raw.gyro_raw)\n\ +\n\ +figure\n\ +plot(data.sensors_raw.timestamp,data.sensors_raw.accelerometer_raw)\n\ +\n\ +%% Check for lost data\n\ +\n\ +% to detect lost frames (either when logging to sd card or if no new data is\n\ +% data is available for some time)\n\ +diff_counter = diff(data.sensors_raw.gyro_raw_counter);\n\ +figure\n\ +plot(diff_counter)\n\ +\n\ +% to detect how accurate the timing was\n\ +diff_timestamp = diff(data.sensors_raw.timestamp);\n\ +\n\ +figure\n\ +plot(diff_timestamp)\n\ +\n\ +%% Export to file for google earth\n\ +\n\ +\n\ +if(isfield(data.gps,'lat') && isfield(data.gps,'lon') && isfield(data.gps,'alt'))\n\ +\n\ + % extract coordinates and height where they are not zero\n\ + maskWhereNotZero = ((data.gps.lon ~= 0 & data.gps.lat ~= 0 ) & data.gps.alt ~= 0);\n\ +\n\ + % plot\n\ + figure\n\ + plot3(data.gps.lon(maskWhereNotZero),data.gps.lat(maskWhereNotZero),data.gps.alt(maskWhereNotZero))\n\ +\n\ +\n\ + % create a kml file according to https://developers.google.com/kml/documentation/kml_tut\n\ + % also see https://support.google.com/earth/bin/answer.py?hl=en&answer=148072&topic=2376756&ctx=topic\n\ +\n\ + % open file and overwrite content\n\ + fileId = fopen('gps_path.kml','w+');\n\ +\n\ + % define strings that should be written to file\n\ + fileStartDocumentString = ['<?xml version=\"1.0\" encoding=\"UTF-8\"?><kml xmlns=\"http://www.opengis.net/kml/2.2\"><Document><name>Paths</name><description>Path</description>'];\n\ +\n\ + fileStyleString = '<Style id=\"blueLinebluePoly\"><LineStyle><color>7fff0000</color><width>4</width></LineStyle><PolyStyle><color>7fff0000</color></PolyStyle></Style>';\n\ +\n\ + filePlacemarkString = '<Placemark><name>Absolute Extruded</name><description>Transparent blue wall with blue outlines</description><styleUrl>#blueLinebluePoly</styleUrl><LineString><extrude>1</extrude><tessellate>1</tessellate><altitudeMode>absolute</altitudeMode><coordinates>';\n\ +\n\ + fileEndPlacemarkString = '</coordinates></LineString></Placemark>';\n\ + fileEndDocumentString = '</Document></kml>';\n\ +\n\ + % start writing to file\n\ + fprintf(fileId,fileStartDocumentString);\n\ +\n\ + fprintf(fileId,fileStyleString);\n\ + fprintf(fileId,filePlacemarkString);\n\ +\n\ + \n\ + lonTemp = double(data.gps.lon(maskWhereNotZero))/1E7;\n\ + latTemp = double(data.gps.lat(maskWhereNotZero))/1E7;\n\ + altTemp = double(data.gps.alt(maskWhereNotZero))/1E3 + 100.0; % in order to see the lines above ground\n\ +\n\ + % write coordinates to file\n\ + for k=1:length(data.gps.lat(maskWhereNotZero))\n\ + if(mod(k,10)==0)\n\ + fprintf(fileId,'%.10f,%.10f,%.10f\\n',lonTemp(k),latTemp(k),altTemp(k));\n\ + end\n\ + end\n\ +\n\ + % write end placemark\n\ + fprintf(fileId,fileEndPlacemarkString);\n\ +\n\ + % write end of file\n\ + fprintf(fileId,fileEndDocumentString);\n\ +\n\ + % close file, it should now be readable in Google Earth using File -> Open\n\ + fclose(fileId);\n\ +\n\ +end\n\ +\n\ +if(isfield(data.position,'lat') && isfield(data.position,'lon') && isfield(data.position,'alt'))\n\ +\n\ + % extract coordinates and height where they are not zero\n\ + maskWhereNotZero = ((data.position.lon ~= 0 & data.position.lat ~= 0 ) & data.position.alt ~= 0);\n\ + \n\ + % plot\n\ + figure\n\ + plot3(data.position.lon(maskWhereNotZero),data.position.lat(maskWhereNotZero),data.position.alt(maskWhereNotZero))\n\ +end\n\ +" +#endif
\ No newline at end of file diff --git a/apps/sdlog/updatesdlog.py b/apps/sdlog/updatesdlog.py new file mode 100644 index 000000000..5892bc40a --- /dev/null +++ b/apps/sdlog/updatesdlog.py @@ -0,0 +1,199 @@ +import os +import glob + +# path to global data files +base_path = '../orb/' +cfile = './sdlog_generated.h' +mfile_template = './mfile.template' + +# there should be one LOGBROADCAST which gives the timing for the logging +logbroadcast_found = 0 + +# these types can nicely be imported into Matlab +allowed_types = ['uint8_t','int8_t','uint16_t','int16_t','uint32_t','int32_t','uint64_t','int64_t','float','double'] + +log_entries = [] +# loop through global_data_files ending in _t.h and look for LOGME (per variable) and LOGBROADCAST (overall) +for path in glob.glob( os.path.join(base_path, '*_t.h') ): + # filename is supposed to be global_data_bapedibup_t.h + if 'global_data' not in path: + print 'path: ' + path + raise 'wrong filename found' + f = open(path, 'r') + access_conf_found = False; + # strip away ../../../../apps/orb/ and _t.h + data_name = path.lstrip(base_path)[0:-4] + # strip away ../../../../apps/orb/ and global_data_ and _t.h + name = path.lstrip(base_path)[12:-4] + log_entry = {'data_name': data_name,'name':name,'vars': []} + + logbroadcast = False; + + # loop throug lines + for line in f: + + line_parts = line.split() + # access_conf is needed to lock the data + if 'access_conf_t' in line: + + # always use the access_conf which has the LOGBROADCAST flag + if 'LOGBROADCAST' in line: + access_conf_found = True + log_entry['access_conf_name'] = line_parts[1].rstrip(';') + logbroadcast = True + print 'LOGBROADCAST found in ' + data_name + logbroadcast_found += 1 + # but use an access_conf anyway + elif access_conf_found == False: + access_conf_found = True + log_entry['access_conf_name'] = line_parts[1].rstrip(';') + # variables flagged with LOGME should be logged + elif 'LOGME' in line: + var_entry = {'type': line_parts[0]} + + # check that it is an allowed type + if var_entry['type'] not in allowed_types: + print 'file: '+ path + ', type: ' + var_entry['type'] + raise 'unsupported type' + + # save variable name and number for array + if '[' in line_parts[1]: + var_entry['name'] = line_parts[1].split('[')[0] + var_entry['number'] = line_parts[1].split('[')[1].rstrip('];') + else: + var_entry['name'] = line_parts[1].rstrip(';') + var_entry['number'] = 1 + + # add the variable + log_entry['vars'].append(var_entry) + # only use the global data file if any variables have a LOGME + if logbroadcast == True and len(log_entry['vars']) > 0: + logbroadcast_entry = log_entry + elif len(log_entry['vars']) > 0: + print 'added ' + log_entry['data_name'] + log_entries.append(log_entry) + f.close() + +# check that we have one and only one LOGBROADCAST +if logbroadcast_found > 1: + raise 'too many LOGBROADCAST found\n' +elif logbroadcast_found == 0: + raise 'no LOGBROADCAST found\n' + +# write function to c file + +header = '/* This file is autogenerated in nuttx/configs/px4fmu/include/updatesdlog.py */\n\ +\n\ +#ifndef SDLOG_GENERATED_H_\n\ +#define SDLOG_GENERATED_H_\n\ +\n\ +\n\ +' + +cstruct = 'typedef struct\n{\n' + +for j in logbroadcast_entry['vars']: + cstruct += '\t' + j['type'] + ' ' + logbroadcast_entry['name'] + '_' + j['name'] + if j['number'] == 1: + cstruct += ';\n' + else: + cstruct += '[' + j['number'] + '];\n' + +for i in log_entries: + for j in i['vars']: + cstruct += '\t' + j['type'] + ' ' + i['name'] + '_' + j['name'] + if j['number'] == 1: + cstruct += ';\n' + else: + cstruct += '[' + j['number'] + '];\n' + +cstruct += '\tchar check[4];\n} __attribute__((__packed__)) log_block_t;\n\n' + + +copy_function = 'void copy_block(log_block_t* log_block)\n{\n' +copy_function += '\tif(global_data_wait(&' + logbroadcast_entry['data_name'] + '->' + logbroadcast_entry['access_conf_name'] + ') == 0)\n\t{\n' + +for j in logbroadcast_entry['vars']: + copy_function += '\t\tmemcpy(&log_block->' + logbroadcast_entry['name'] + '_' + j['name'] + ',&' + logbroadcast_entry['data_name'] + '->' + j['name'] + ',sizeof(' + j['type'] + ')*' + str(j['number']) + ');\n' + #copy_function += '\t\t}\n' + +# generate logging MACRO + + +for i in log_entries: + copy_function += '\t\tif(global_data_trylock(&' + i['data_name'] + '->' + i['access_conf_name'] + ') == 0)\n\t\t{\n' + + for j in i['vars']: + copy_function += '\t\t\tmemcpy(&log_block->' + i['name'] + '_' + j['name'] + ',&' + i['data_name'] + '->' + j['name'] + ',sizeof(' + j['type'] + ')*' + str(j['number']) + ');\n' + + copy_function += '\t\t\tglobal_data_unlock(&' + i['data_name'] + '->' + i['access_conf_name'] + ');\n' + copy_function += '\t\t}\n' +copy_function += '\t\tglobal_data_unlock(&' + logbroadcast_entry['data_name'] + '->' + logbroadcast_entry['access_conf_name'] + ');\n' +copy_function += '\t}\n' + +copy_function += '}\n' + +footer = '\n#endif' + + + +# generate mfile + +type_bytes = { +'uint8_t' : 1, +'int8_t' : 1, +'uint16_t' : 2, +'int16_t' : 2, +'uint32_t' : 4, +'int32_t' : 4, +'uint64_t' : 8, +'int64_t' : 8, +'float' : 4, +'double' : 8, +} + +type_names_matlab = { +'uint8_t' : 'uint8', +'int8_t' : 'int8', +'uint16_t' : 'uint16', +'int16_t' : 'int16', +'uint32_t' : 'uint32', +'int32_t' : 'int32', +'uint64_t' : 'uint64', +'int64_t' : 'int64', +'float' : 'float', +'double' : 'double', +} + + +# read template mfile +mf = open(mfile_template, 'r') +mfile_template_string = mf.read() + +mfile_define = '#define MFILE_STRING "% This file is autogenerated in updatesdlog.py and mfile.template in apps/sdlog\\n\\\n' +mfile_define += '%% Define logged values \\n\\\n\\n\\\nlogTypes = {};\\n\\\n' + +for j in logbroadcast_entry['vars']: + mfile_define += 'logTypes{end+1} = struct(\'data\',\'' + logbroadcast_entry['name'] + '\',\'variable_name\',\'' + j['name'] + '\',\'type_name\',\'' + type_names_matlab.get(j['type']) + '\',\'type_bytes\',' + str(type_bytes.get(j['type'])) + ',\'number_of_array\',' + str(j['number']) + ');\\n\\\n' + +for i in log_entries: + for j in i['vars']: + mfile_define += 'logTypes{end+1} = struct(\'data\',\'' + i['name'] + '\',\'variable_name\',\'' + j['name'] + '\',\'type_name\',\'' + type_names_matlab.get(j['type']) + '\',\'type_bytes\',' + str(type_bytes.get(j['type'])) + ',\'number_of_array\',' + str(j['number']) + ');\\n\\\n' + + + +mfile_define += '\\\n' + mfile_template_string.replace('\n', '\\n\\\n').replace('\"','\\\"') + '"' + + +# write to c File +cf = open(cfile, 'w') +cf.write(header) +cf.write(cstruct); +cf.write(copy_function) + +cf.write(mfile_define) + +cf.write(footer) +cf.close() + +print 'finished, cleanbuild needed!' diff --git a/apps/sensors/.context b/apps/sensors/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/sensors/.context diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile new file mode 100644 index 000000000..0bc5b52d6 --- /dev/null +++ b/apps/sensors/Makefile @@ -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. +# +############################################################################ + +# +# Makefile to build the sensor data collector +# + +APPNAME = sensors +PRIORITY = SCHED_PRIORITY_MAX-5 +STACKSIZE = 2560 + +include $(APPDIR)/mk/app.mk diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c new file mode 100644 index 000000000..a88361e1a --- /dev/null +++ b/apps/sensors/sensors.c @@ -0,0 +1,852 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sensors.c + * Sensor readout process. + */ + +#include <nuttx/config.h> + +#include <pthread.h> +#include <fcntl.h> +#include <sys/prctl.h> +#include <nuttx/analog/adc.h> +#include <unistd.h> +#include <string.h> +#include <stdbool.h> +#include <stdio.h> +#include <errno.h> +#include <float.h> + +#include <arch/board/up_hrt.h> +#include <arch/board/drv_lis331.h> +#include <arch/board/drv_bma180.h> +#include <arch/board/drv_l3gd20.h> +#include <arch/board/drv_hmc5883l.h> +#include <arch/board/up_adc.h> + +#include <systemlib/systemlib.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_status.h> + +#include "sensors.h" + +#define errno *get_errno_ptr() + +#define SENSOR_INTERVAL_MICROSEC 2000 + +#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ +#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ +#define MAGN_HEALTH_COUNTER_LIMIT_ERROR 100 /* 1000 ms downtime at 100 Hz update rate */ +#define BARO_HEALTH_COUNTER_LIMIT_ERROR 50 /* 500 ms downtime at 100 Hz update rate */ +#define ADC_HEALTH_COUNTER_LIMIT_ERROR 10 /* 100 ms downtime at 100 Hz update rate */ + +#define GYRO_HEALTH_COUNTER_LIMIT_OK 5 +#define ACC_HEALTH_COUNTER_LIMIT_OK 5 +#define MAGN_HEALTH_COUNTER_LIMIT_OK 5 +#define BARO_HEALTH_COUNTER_LIMIT_OK 5 +#define ADC_HEALTH_COUNTER_LIMIT_OK 5 + +#define ADC_BATTERY_VOLATGE_CHANNEL 10 + +#define BAT_VOL_INITIAL 12.f +#define BAT_VOL_LOWPASS_1 0.99f +#define BAT_VOL_LOWPASS_2 0.01f +#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f + +/*PPM Settings*/ +#define PPM_MIN 1000 +#define PPM_MAX 2000 +/*Our internal resolution is 10000*/ +#define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2) + +#define PPM_MID (PPM_MIN+PPM_MAX)/2 + +/**************************************************************************** + * Definitions + ****************************************************************************/ +static pthread_cond_t sensors_read_ready; +static pthread_mutex_t sensors_read_ready_mutex; + +static int sensors_timer_loop_counter = 0; + +/* File descriptors for all sensors */ +static int fd_gyro = -1; +static int fd_accelerometer = -1; +static int fd_magnetometer = -1; +static int fd_barometer = -1; +static int fd_adc = -1; + +/* Private functions declared static */ +static void sensors_timer_loop(void *arg); + +#ifdef CONFIG_HRT_PPM +extern uint16_t ppm_buffer[]; +extern unsigned ppm_decoded_channels; +#endif + +/* file handle that will be used for subscribing */ +static int sensor_pub; + +/** + * Sensor readout and publishing. + * + * This function reads all onboard sensors and publishes the sensor_combined topic. + * + * @see sensor_combined_s + * @ingroup apps + */ +__EXPORT int sensors_main(int argc, char *argv[]); + +/** + * Initialize all sensor drivers. + * + * @return 0 on success, != 0 on failure + */ +static int sensors_init(void) +{ + printf("[sensors] Sensor configuration..\n"); + + /* open magnetometer */ + fd_magnetometer = open("/dev/hmc5883l", O_RDONLY); + + if (fd_magnetometer < 0) { + fprintf(stderr, "[sensors] HMC5883L open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fflush(stderr); + /* this sensor is critical, exit on failed init */ + errno = ENOSYS; + return ERROR; + + } else { + printf("[sensors] HMC5883L open ok\n"); + } + + /* open barometer */ + fd_barometer = open("/dev/ms5611", O_RDONLY); + + if (fd_barometer < 0) { + fprintf(stderr, "[sensors] MS5611 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fflush(stderr); + + } else { + printf("[sensors] MS5611 open ok\n"); + } + + /* open gyro */ + fd_gyro = open("/dev/l3gd20", O_RDONLY); + + if (fd_gyro < 0) { + fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fflush(stderr); + /* this sensor is critical, exit on failed init */ + errno = ENOSYS; + return ERROR; + + } else { + printf("[sensors] L3GD20 open ok\n"); + } + + /* open accelerometer */ + fd_accelerometer = open("/dev/bma180", O_RDONLY); + + if (fd_accelerometer < 0) { + fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fflush(stderr); + /* this sensor is critical, exit on failed init */ + errno = ENOSYS; + return ERROR; + + } else { + printf("[sensors] BMA180 open ok\n"); + } + + /* open adc */ + fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK); + + if (fd_adc < 0) { + fprintf(stderr, "[sensors] ADC: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fflush(stderr); + /* this sensor is critical, exit on failed init */ + errno = ENOSYS; + return ERROR; + + } else { + printf("[sensors] ADC open ok\n"); + } + + /* configure gyro */ + if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { + fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fflush(stderr); + /* this sensor is critical, exit on failed init */ + errno = ENOSYS; + return ERROR; + + } else { + printf("[sensors] L3GD20 configuration ok\n"); + } + + /* XXX Add IOCTL configuration of remaining sensors */ + + printf("[sensors] All sensors configured\n"); + return OK; +} + +/** + * Callback function called by high resolution timer. + * + * This function signals a pthread condition and wakes up the + * sensor main loop. + */ +static void sensors_timer_loop(void *arg) +{ + /* Inform the read thread that it is now time to read */ + sensors_timer_loop_counter++; + /* Do not use global data broadcast because of + * use of printf() in call - would be fatal here + */ + pthread_cond_broadcast(&sensors_read_ready); +} + +int sensors_main(int argc, char *argv[]) +{ + /* inform about start */ + printf("[sensors] Initializing..\n"); + fflush(stdout); + int ret = OK; + + /* start sensor reading */ + if (sensors_init() != OK) { + fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n"); + /* Clean up */ + close(fd_gyro); + close(fd_accelerometer); + close(fd_magnetometer); + close(fd_barometer); + close(fd_adc); + + fprintf(stderr, "[sensors] rebooting system.\n"); + fflush(stderr); + fflush(stdout); + usleep(100000); + + /* Sensors are critical, immediately reboot system on failure */ + reboot(); + /* Not ever reaching here */ + + } else { + /* flush stdout from init routine */ + fflush(stdout); + } + + bool gyro_healthy = false; + bool acc_healthy = false; + bool magn_healthy = false; + bool baro_healthy = false; + bool adc_healthy = false; + + bool hil_enabled = false; + + int magcounter = 0; + int barocounter = 0; + int adccounter = 0; + + unsigned int mag_fail_count = 0; + unsigned int mag_success_count = 0; + + unsigned int baro_fail_count = 0; + unsigned int baro_success_count = 0; + + unsigned int gyro_fail_count = 0; + unsigned int gyro_success_count = 0; + + unsigned int acc_fail_count = 0; + unsigned int acc_success_count = 0; + + unsigned int adc_fail_count = 0; + unsigned int adc_success_count = 0; + + ssize_t ret_gyro; + ssize_t ret_accelerometer; + ssize_t ret_magnetometer; + ssize_t ret_barometer; + ssize_t ret_adc; + int nsamples_adc; + + int16_t buf_gyro[3]; + int16_t buf_accelerometer[3]; + int16_t buf_magnetometer[3]; + float buf_barometer[3]; + + int16_t mag_offset[3] = {0, 0, 0}; + int16_t acc_offset[3] = {0, 0, 0}; + int16_t gyro_offset[3] = {0, 0, 0}; + + struct adc_msg4_s { + uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ + int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ + uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ + int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ + uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ + int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ + uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ + int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ + } __attribute__((__packed__));; + struct adc_msg4_s buf_adc; + size_t adc_readsize = 1 * sizeof(struct adc_msg4_s); + + float battery_voltage_conversion; + battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION]; + + if (-1.0f == battery_voltage_conversion) { + /**< default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ + battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f; + } + +#ifdef CONFIG_HRT_PPM + int ppmcounter = 0; +#endif + /* initialize to 100 to execute immediately */ + int paramcounter = 100; + + int excessive_readout_time_counter = 0; + + int read_loop_counter = 0; + + /* Empty sensor buffers, avoid junk values */ + /* Read first two values of each sensor into void */ + (void)read(fd_gyro, buf_gyro, sizeof(buf_gyro)); + (void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); + (void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); + + if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer)); + + struct sensor_combined_s raw = { + .timestamp = hrt_absolute_time(), + .gyro_raw = {buf_gyro[0], buf_gyro[1], buf_gyro[2]}, + .gyro_raw_counter = 0, + .gyro_rad_s = {0, 0, 0}, + .accelerometer_raw = {buf_accelerometer[0], buf_accelerometer[1], buf_accelerometer[2]}, + .accelerometer_raw_counter = 0, + .accelerometer_m_s2 = {0, 0, 0}, + .magnetometer_raw = {buf_magnetometer[0], buf_magnetometer[1], buf_magnetometer[2]}, + .magnetometer_raw_counter = 0, + .baro_pres_mbar = 0, + .baro_alt_meter = 0, + .baro_temp_celcius = 0, + .battery_voltage_v = BAT_VOL_INITIAL, + .adc_voltage_v = {0, 0 , 0}, + .baro_raw_counter = 0, + .battery_voltage_counter = 0, + .battery_voltage_valid = false, + }; + + /* condition to wait for */ + pthread_mutex_init(&sensors_read_ready_mutex, NULL); + pthread_cond_init(&sensors_read_ready, NULL); + + /* advertise the topic and make the initial publication */ + sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + + /* advertise the rc topic */ + struct rc_channels_s rc = {0}; + int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + + /* subscribe to system status */ + struct vehicle_status_s vstatus; + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + + + printf("[sensors] rate: %u Hz\n", (unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC)); + + struct hrt_call sensors_hrt_call; + /* Enable high resolution timer callback to unblock main thread, run after 2 ms */ + hrt_call_every(&sensors_hrt_call, 2000, SENSOR_INTERVAL_MICROSEC, &sensors_timer_loop, NULL); + + while (1) { + pthread_mutex_lock(&sensors_read_ready_mutex); + + struct timespec time_to_wait = {0, 0}; + /* Wait 2 seconds until timeout */ + time_to_wait.tv_nsec = 0; + time_to_wait.tv_sec = time(NULL) + 2; + + if (pthread_cond_timedwait(&sensors_read_ready, &sensors_read_ready_mutex, &time_to_wait) == OK) { + pthread_mutex_unlock(&sensors_read_ready_mutex); + + bool gyro_updated = false; + bool acc_updated = false; + bool magn_updated = false; + bool baro_updated = false; + bool adc_updated = false; + + /* store the time closest to all measurements */ + uint64_t current_time = hrt_absolute_time(); + raw.timestamp = current_time; + + if (paramcounter == 100) { + // XXX paramcounter is not a good name, rename / restructure + // XXX make counter ticks dependent on update rate of sensor main loop + + /* Check HIL state */ + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + + /* switching from non-HIL to HIL mode */ + if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { + hil_enabled = true; + close(sensor_pub); + + /* switching from HIL to non-HIL mode */ + + } else if (!(vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && hil_enabled) { + /* advertise the topic and make the initial publication */ + sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + hil_enabled = false; + } + + + /* Update RC scalings and function mappings */ + rc.chan[0].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2) + * global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]); + rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM]; + + rc.chan[1].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2) + * global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]); + rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM]; + + rc.chan[2].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2) + * global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]); + rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM]; + + rc.chan[3].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2) + * global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]); + rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM]; + + rc.chan[4].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2) + * global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]); + rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM]; + + rc.function[0] = global_data_parameter_storage->pm.param_values[PARAM_THROTTLE_CHAN] - 1; + rc.function[1] = global_data_parameter_storage->pm.param_values[PARAM_ROLL_CHAN] - 1; + rc.function[2] = global_data_parameter_storage->pm.param_values[PARAM_PITCH_CHAN] - 1; + rc.function[3] = global_data_parameter_storage->pm.param_values[PARAM_YAW_CHAN] - 1; + rc.function[4] = global_data_parameter_storage->pm.param_values[PARAM_OVERRIDE_CHAN] - 1; + + gyro_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET]; + gyro_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET]; + gyro_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET]; + + mag_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET]; + mag_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET]; + mag_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET]; + + paramcounter = 0; + } + + paramcounter++; + + /* try reading gyro */ + uint64_t start_gyro = hrt_absolute_time(); + ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro)); + int gyrotime = hrt_absolute_time() - start_gyro; + + if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime); + + /* GYROSCOPE */ + if (ret_gyro != sizeof(buf_gyro)) { + gyro_fail_count++; + + if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { + fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } + + if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) { + // global_data_send_subsystem_info(&gyro_present_enabled); + gyro_healthy = false; + gyro_success_count = 0; + } + + } else { + gyro_success_count++; + + if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) { + // global_data_send_subsystem_info(&gyro_present_enabled_healthy); + gyro_healthy = true; + gyro_fail_count = 0; + + } + + gyro_updated = true; + } + + gyrotime = hrt_absolute_time() - start_gyro; + + if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime); + + /* try reading acc */ + uint64_t start_acc = hrt_absolute_time(); + ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); + + /* ACCELEROMETER */ + if (ret_accelerometer != sizeof(buf_accelerometer)) { + acc_fail_count++; + + if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) { + fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } + + if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { + // global_data_send_subsystem_info(&acc_present_enabled); + gyro_healthy = false; + acc_success_count = 0; + } + + } else { + acc_success_count++; + + if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { + + // global_data_send_subsystem_info(&acc_present_enabled_healthy); + acc_healthy = true; + acc_fail_count = 0; + + } + + acc_updated = true; + } + + int acctime = hrt_absolute_time() - start_acc; + + if (acctime > 500) printf("ACC: %d us\n", acctime); + + /* MAGNETOMETER */ + if (magcounter == 4) { //(magcounter == 4) // 100 Hz + uint64_t start_mag = hrt_absolute_time(); + ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); + int errcode_mag = (int) * get_errno_ptr(); + int magtime = hrt_absolute_time() - start_mag; + + if (magtime > 2000) { + printf("MAG (pure read): %d us\n", magtime); + } + + if (ret_magnetometer != sizeof(buf_magnetometer)) { + mag_fail_count++; + + if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) { + fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } + + if (magn_healthy && mag_fail_count >= MAGN_HEALTH_COUNTER_LIMIT_ERROR) { + // global_data_send_subsystem_info(&magn_present_enabled); + magn_healthy = false; + mag_success_count = 0; + } + + } else { + mag_success_count++; + + if (!magn_healthy && mag_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) { + // global_data_send_subsystem_info(&magn_present_enabled_healthy); + magn_healthy = true; + mag_fail_count = 0; + } + + magn_updated = true; + } + + magtime = hrt_absolute_time() - start_mag; + + if (magtime > 2000) { + printf("MAG (overall time): %d us\n", magtime); + fprintf(stderr, "[sensors] TIMEOUT HMC5883L ERROR #%d: %s\n", errcode_mag, strerror(errcode_mag)); + } + + magcounter = 0; + } + + magcounter++; + + /* BAROMETER */ + if (barocounter == 5 && (fd_barometer > 0)) { //(barocounter == 4) // 100 Hz + uint64_t start_baro = hrt_absolute_time(); + *get_errno_ptr() = 0; + ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer)); + + if (ret_barometer != sizeof(buf_barometer)) { + baro_fail_count++; + + if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { + fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } + + if (baro_healthy && baro_fail_count >= BARO_HEALTH_COUNTER_LIMIT_ERROR) { + /* switched from healthy to unhealthy */ + baro_healthy = false; + baro_success_count = 0; + // global_data_send_subsystem_info(&baro_present_enabled); + } + + } else { + baro_success_count++; + + if (!baro_healthy && baro_success_count >= MAGN_HEALTH_COUNTER_LIMIT_OK) { + /* switched from unhealthy to healthy */ + baro_healthy = true; + baro_fail_count = 0; + // global_data_send_subsystem_info(&baro_present_enabled_healthy); + } + + baro_updated = true; + } + + barocounter = 0; + int barotime = hrt_absolute_time() - start_baro; + + if (barotime > 2000) printf("BARO: %d us\n", barotime); + } + + barocounter++; + + /* ADC */ + if (adccounter == 5) { + ret_adc = read(fd_adc, &buf_adc, adc_readsize); + nsamples_adc = ret_adc / sizeof(struct adc_msg_s); + + if (ret_adc < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) { + adc_fail_count++; + + if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) { + fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } + + if (adc_healthy && adc_fail_count >= ADC_HEALTH_COUNTER_LIMIT_ERROR) { + adc_healthy = false; + adc_success_count = 0; + } + + } else { + adc_success_count++; + + if (!adc_healthy && adc_success_count >= ADC_HEALTH_COUNTER_LIMIT_OK) { + adc_healthy = true; + adc_fail_count = 0; + } + + adc_updated = true; + } + + adccounter = 0; + + } + + adccounter++; + + + +#ifdef CONFIG_HRT_PPM + bool ppm_updated = false; + + /* PPM */ + if (ppmcounter == 5) { + + /* Read out values from HRT */ + for (int i = 0; i < ppm_decoded_channels; i++) { + rc.chan[i].raw = ppm_buffer[i]; + /* Set the range to +-, then scale up */ + rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor; + } + + rc.chan_count = ppm_decoded_channels; + + rc.timestamp = hrt_absolute_time(); + /* publish a few lines of code later if set to true */ + ppm_updated = true; + + + //TODO: XXX check the mode switch channel and eventually send a request to the commander (see implementation in commander and mavlink) + ppmcounter = 0; + } + + ppmcounter++; +#endif + + /* Copy values of gyro, acc, magnetometer & barometer */ + + /* GYROSCOPE */ + if (gyro_updated) { + /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ + + raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32767 : buf_gyro[1]); // x of the board is y of the sensor + /* assign negated value, except for -SHORT_MAX, as it would wrap there */ + raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor + raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32767 : buf_gyro[2]); // z of the board is -z of the sensor + + /* scale measurements */ + // XXX request scaling from driver instead of hardcoding it + /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ + raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f; + raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f; + raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f; + + raw.gyro_raw_counter++; + } + + /* ACCELEROMETER */ + if (acc_updated) { + /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ + + /* assign negated value, except for -SHORT_MAX, as it would wrap there */ + raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor + raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor + raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor + + // XXX read range from sensor + float range_g = 4.0f; + /* scale from 14 bit to m/s2 */ + raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f; + + raw.accelerometer_raw_counter++; + } + + /* MAGNETOMETER */ + if (magn_updated) { + /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ + + /* assign negated value, except for -SHORT_MAX, as it would wrap there */ + raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : -buf_magnetometer[1]; // x of the board is -y of the sensor + raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? -32767 : buf_magnetometer[0]; // y on the board is x of the sensor + raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32767 : buf_magnetometer[2]; // z of the board is z of the sensor + + // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here + raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f; + raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - mag_offset[1]) / 4096.0f) * 0.88f; + raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - mag_offset[2]) / 4096.0f) * 0.88f; + + raw.magnetometer_raw_counter++; + } + + /* BAROMETER */ + if (baro_updated) { + /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ + + raw.baro_pres_mbar = buf_barometer[0]; // Pressure in mbar + raw.baro_alt_meter = buf_barometer[1]; // Altitude in meters + raw.baro_temp_celcius = buf_barometer[2]; // Temperature in degrees celcius + + raw.baro_raw_counter++; + } + + /* ADC */ + if (adc_updated) { + /* copy sensor readings to global data*/ + + if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) { + /* Voltage in volts */ + raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (uint16_t)(buf_adc.am_data1 * battery_voltage_conversion))); + + if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { + raw.battery_voltage_valid = false; + raw.battery_voltage_v = 0.f; + + } else { + raw.battery_voltage_valid = true; + } + + raw.battery_voltage_counter++; + } + } + + uint64_t total_time = hrt_absolute_time() - current_time; + + /* Inform other processes that new data is available to copy */ + if ((gyro_updated || acc_updated || magn_updated || baro_updated) && !hil_enabled) { + /* Values changed, publish */ + orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw); + } + +#ifdef CONFIG_HRT_PPM + + if (ppm_updated) { + orb_publish(ORB_ID(rc_channels), rc_pub, &rc); + } + +#endif + + if (total_time > 2600) { + excessive_readout_time_counter++; + } + + if (total_time > 2600 && excessive_readout_time_counter > 100 && excessive_readout_time_counter % 100 == 0) { + fprintf(stderr, "[sensors] slow update (>2600 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter); + + } else if (total_time > 6000) { + if (excessive_readout_time_counter < 100 || excessive_readout_time_counter % 100 == 0) fprintf(stderr, "[sensors] WARNING: Slow update (>6000 us): %d us (#%d)\n", (int)total_time, excessive_readout_time_counter); + } + + + read_loop_counter++; +#ifdef CONFIG_SENSORS_DEBUG_ENABLED + + if (read_loop_counter % 1000 == 0) printf("[sensors] read loop counter: %d\n", read_loop_counter); + + fflush(stdout); + + if (sensors_timer_loop_counter % 1000 == 0) printf("[sensors] timer/trigger loop counter: %d\n", sensors_timer_loop_counter); + +#endif + } + } + + /* Never really getting here */ + printf("[sensors] sensor readout stopped\n"); + + close(fd_gyro); + close(fd_accelerometer); + close(fd_magnetometer); + close(fd_barometer); + close(fd_adc); + + printf("[sensors] exiting.\n"); + + return ret; +} + diff --git a/apps/sensors/sensors.h b/apps/sensors/sensors.h new file mode 100644 index 000000000..87ea19972 --- /dev/null +++ b/apps/sensors/sensors.h @@ -0,0 +1,14 @@ +/* + * gps.h + * + * Created on: Mar 8, 2012 + * Author: thomasgubler + */ + +#ifndef SENSORS_H_ +#define SENSORS_H_ +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#endif /* SENSORS_H_ */ diff --git a/apps/system/Kconfig b/apps/system/Kconfig new file mode 100644 index 000000000..44bf5a2e6 --- /dev/null +++ b/apps/system/Kconfig @@ -0,0 +1,20 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +menu "Custom free memory command" +source "$APPSDIR/system/free/Kconfig" +endmenu + +menu "I2C tool" +source "$APPSDIR/system/i2c/Kconfig" +endmenu + +menu "FLASH Program Installation" +source "$APPSDIR/system/install/Kconfig" +endmenu + +menu "readline() support" +source "$APPSDIR/system/readline/Kconfig" +endmenu diff --git a/apps/system/Make.defs b/apps/system/Make.defs new file mode 100644 index 000000000..1ddabd337 --- /dev/null +++ b/apps/system/Make.defs @@ -0,0 +1,51 @@ +############################################################################ +# apps/system/Make.defs +# Adds selected applications to apps/ build +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +if ($(CONFIG_VSN_POWEROFF),y) +CONFIGURED_APPS += vsn/poweroff +endif + +if ($(CONFIG_VSN_RAMTRON),y) +CONFIGURED_APPS += vsn/ramtron +endif + +if ($(CONFIG_VSN_SDCARD),y) +CONFIGURED_APPS += vsn/sdcard +endif + +if ($(CONFIG_VSN_SYSINFO),y) +CONFIGURED_APPS += vsn/sysinfo +endif diff --git a/apps/system/Makefile b/apps/system/Makefile new file mode 100644 index 000000000..a0eb5dfde --- /dev/null +++ b/apps/system/Makefile @@ -0,0 +1,70 @@ +############################################################################ +# apps/system/Makefile +# +# 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 +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config # Current configuration + +# Sub-directories containing system task + +SUBDIRS = free i2c install readline + +# Create the list of installed runtime modules (INSTALLED_DIRS) + +define ADD_DIRECTORY +INSTALLED_DIRS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi} +endef + +$(foreach DIR, $(SUBDIRS), $(eval $(call ADD_DIRECTORY,$(DIR)))) + +all: nothing +.PHONY: nothing context depend clean distclean + +nothing: + +context: + +depend: + @for dir in $(INSTALLED_DIRS) ; do \ + $(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + +clean: + @for dir in $(INSTALLED_DIRS) ; do \ + $(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done + +distclean: clean + @for dir in $(INSTALLED_DIRS) ; do \ + $(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \ + done diff --git a/apps/system/free/Kconfig b/apps/system/free/Kconfig new file mode 100644 index 000000000..239559867 --- /dev/null +++ b/apps/system/free/Kconfig @@ -0,0 +1,14 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config SYSTEM_FREE + bool "NSH free command replacement" + default n + ---help--- + Enable support for the NSH free replacement command. + +if SYSTEM_FREE +endif + diff --git a/apps/system/free/Makefile b/apps/system/free/Makefile new file mode 100644 index 000000000..974c98913 --- /dev/null +++ b/apps/system/free/Makefile @@ -0,0 +1,114 @@ +############################################################################ +# apps/system/free/Makefile +# +# Copyright (C) 2011 Uros Platise. All rights reserved. +# Author: Uros Platise <uros.platise@isotel.eu> +# Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +# TODO, this makefile should run make under the app dirs, instead of +# sourcing the Make.defs! + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +ifeq ($(WINTOOL),y) +INCDIROPT = -w +endif + +# Hello Application +# TODO: appname can be automatically extracted from the directory name + +APPNAME = free +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 768 + +ASRCS = +CSRCS = free.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: context depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +# Register application + +.context: + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ + +context: .context + +# Create dependencies + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f .context Make.dep .depend + +-include Make.dep diff --git a/apps/system/free/README.txt b/apps/system/free/README.txt new file mode 100644 index 000000000..dd92a94ae --- /dev/null +++ b/apps/system/free/README.txt @@ -0,0 +1,6 @@ + +This application provides UNIX style memory free information. + + Source: NuttX + Author: Gregory Nutt <spudmonkey@racsa.co.cr> + Date: 17. March 2011 diff --git a/apps/system/free/free.c b/apps/system/free/free.c new file mode 100644 index 000000000..2f61a1dae --- /dev/null +++ b/apps/system/free/free.c @@ -0,0 +1,112 @@ +/**************************************************************************** + * apps/system/free/free.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/progmem.h> + +#include <stdio.h> +#include <stdlib.h> + + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* \todo Max block size only works on uniform prog mem */ + +void free_getprogmeminfo(struct mallinfo * mem) +{ + uint16_t page = 0, stpage = 0xFFFF; + uint16_t pagesize = 0; + int status; + + mem->arena = 0; + mem->fordblks = 0; + mem->uordblks = 0; + mem->mxordblk = 0; + + for (status=0, page=0; status >= 0; page++) { + + status = up_progmem_ispageerased(page); + pagesize = up_progmem_pagesize(page); + + mem->arena += pagesize; + + /* Is this beginning of new free space section */ + if (status == 0) { + if (stpage == 0xFFFF) stpage = page; + mem->fordblks += pagesize; + } + else if (status != 0) { + mem->uordblks += pagesize; + + if (stpage != 0xFFFF && up_progmem_isuniform()) { + stpage = page - stpage; + if (stpage > mem->mxordblk) + mem->mxordblk = stpage; + stpage = 0xFFFF; + } + } + } + + mem->mxordblk *= pagesize; +} + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int free_main(int argc, char **argv) +{ + struct mallinfo data; + struct mallinfo prog; + +#ifdef CONFIG_CAN_PASS_STRUCTS + data = mallinfo(); +#else + (void)mallinfo(&data); +#endif + + free_getprogmeminfo(&prog); + + printf(" total used free largest\n"); + printf("Data: %11d%11d%11d%11d\n", + data.arena, data.uordblks, data.fordblks, data.mxordblk); + printf("Prog: %11d%11d%11d%11d\n", + prog.arena, prog.uordblks, prog.fordblks, prog.mxordblk); + + return OK; +} diff --git a/apps/system/i2c/Kconfig b/apps/system/i2c/Kconfig new file mode 100644 index 000000000..745378b37 --- /dev/null +++ b/apps/system/i2c/Kconfig @@ -0,0 +1,61 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + + +config SYSTEM_I2CTOOL + bool "I2C tool" + default n + depends on I2C + ---help--- + Enable support for the I2C tool. + +config I2CTOOL_BUILTIN + bool "NSH built-in command" + default y + depends on SYSTEM_I2CTOOL && NSH_BUILTIN_APPS + ---help--- + Build the tools as an NSH built-in command + +config I2CTOOL_MINBUS + int "Minimum bus number" + default 0 + depends on SYSTEM_I2CTOOL + ---help--- + Smallest bus index supported by the hardware (default 0). + +config I2CTOOL_MAXBUS + int "Maximum bus number" + depends on SYSTEM_I2CTOOL + default 3 + ---help--- + Largest bus index supported by the hardware (default 3) + +config I2CTOOL_MINADDR + hex "Minimum I2C address" + depends on SYSTEM_I2CTOOL + default 0x03 + ---help--- + Minium 7-bit device address (default: 0x03) + +config I2CTOOL_MAXADDR + hex "Maximum I2C address" + depends on SYSTEM_I2CTOOL + default 0x77 + ---help--- + Largest 7-bit device address (default: 0x77) + +config I2CTOOL_MAXREGADDR + hex "Maximum I2C register address" + default 0xff + depends on SYSTEM_I2CTOOL + ---help--- + Largest I2C register address (default: 0xff) + +config I2CTOOL_DEFFREQ + int "Default I2C frequency" + default 4000000 + depends on SYSTEM_I2CTOOL + ---help--- + Default I2C frequency (default: 4000000) diff --git a/apps/system/i2c/Makefile b/apps/system/i2c/Makefile new file mode 100644 index 000000000..845c149f6 --- /dev/null +++ b/apps/system/i2c/Makefile @@ -0,0 +1,102 @@ +############################################################################ +# apps/system/i2c +# +# 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 +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +# I2C tool + +ASRCS = +CSRCS = i2c_bus.c i2c_common.c i2c_dev.c i2c_get.c i2c_main.c i2c_set.c i2c_verf.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . +VPATH = + +APPNAME = i2c +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +# Build targets + +all: .built +.PHONY: .context context .depend depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +.context: + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ + +context: .context + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) \ + $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f Make.dep .depend + +-include Make.dep + diff --git a/apps/system/i2c/README.txt b/apps/system/i2c/README.txt new file mode 100755 index 000000000..97801bcaa --- /dev/null +++ b/apps/system/i2c/README.txt @@ -0,0 +1,397 @@ +README File for the I2C Tool
+============================
+
+The I2C tool provides a way to debug I2C related problems. This README file
+will provide usage information for the I2C tools.
+
+CONTENTS
+========
+
+ o System Requirements
+ - I2C Driver
+ - Configuration Options
+ o Help
+ o Common Line Form
+ o Common Command Options
+ - "Sticky" Options
+ - Environment variables
+ - Common Option Summary
+ o Command summary
+ - bus
+ - dev
+ - get
+ - set
+ - verf
+ o I2C Build Configuration
+ - NuttX Configuration Requirements
+ - I2C Tool Configuration Options
+
+System Requirements
+===================
+
+I2C Driver
+----------
+In order to use the I2C driver, you system -- in particular, your I2C driver --
+must meet certain requirements:
+
+1. It support calling up_i2cinitialize() numerous times, resetting the I2C
+ hardware on each (initial) time. up_i2cuninitialize() will be called after
+ each call to up_i2cinitialize() to free any resources and disable the I2C.
+2. up_i2cinitialize must accept any interface number without crashing. It
+ must simply return NULL if the device is not supported.
+3. The I2C driver must support the transfer method (CONFIG_I2C_TRANSFER=y).
+
+The I2C tool is designed to be implemented as a NuttShell (NSH) add-on. Read
+the apps/nshlib/README.txt file for information about add-ons.
+
+Configuration Options
+---------------------
+CONFIG_I2CTOOL_BUILTIN - Build the tools as an NSH built-in command
+CONFIG_I2CTOOL_MINBUS - Smallest bus index supported by the hardware (default 0).
+CONFIG_I2CTOOL_MAXBUS - Largest bus index supported by the hardware (default 3)
+CONFIG_I2CTOOL_MINADDR - Minium device address (default: 0x03)
+CONFIG_I2CTOOL_MAXADDR - Largest device address (default: 0x77)
+CONFIG_I2CTOOL_MAXREGADDR - Largest register address (default: 0xff)
+CONFIG_I2CTOOL_DEFFREQ - Default frequency (default: 4000000)
+
+HELP
+====
+
+First of all, the I2C tools supports a pretty extensive help output. That
+help output can be view by entering either:
+
+ nsh> i2c help
+
+or
+
+ nsh> i2c ?
+
+Here is an example of the help output. I shows the general form of the
+command line, the various I2C commands supported with their unique command
+line options, and a more detailed summary of the command I2C command
+options.
+
+ nsh> i2c help
+ Usage: i2c <cmd> [arguments]
+ Where <cmd> is one of:
+
+ Show help : ?
+ List buses : bus
+ List devices : dev [OPTIONS] <first> <last>
+ Read register : get [OPTIONS] [<repititions>]
+ Show help : help
+ Write register: set [OPTIONS] <value> [<repititions>]
+ Verify access : verf [OPTIONS] <value> [<repititions>]
+
+ Where common "sticky" OPTIONS include:
+ [-a addr] is the I2C device address (hex). Default: 03 Current: 03
+ [-b bus] is the I2C bus number (decimal). Default: 1 Current: 1
+ [-r regaddr] is the I2C device register address (hex). Default: 00 Current: 00
+ [-w width] is the data width (8 or 16 decimal). Default: 8 Current: 8
+ [-s|n], send/don't send start between command and data. Default: -n Current: -n
+ [-i|j], Auto increment|don't increment regaddr on repititions. Default: NO Current: NO
+ [-f freq] I2C frequency. Default: 100000 Current: 100000
+
+ NOTES:
+ o An environment variable like $PATH may be used for any argument.
+ o Arguments are "sticky". For example, once the I2C address is
+ specified, that address will be re-used until it is changed.
+
+ WARNING:
+ o The I2C dev command may have bad side effects on your I2C devices.
+ Use only at your own risk.
+
+COMMAND LINE FORM
+=================
+
+The I2C is started from NSH by invoking the 'i2c' command from the NSH
+command line. The general form of the 'i2c' command is:
+
+ i2c <cmd> [arguments]
+
+Where <cmd> is a "sub-command" and identifies one I2C operations supported
+by the tool. [arguments] represents the list of arguments needed to perform
+the I2C operation. Those arguments vary from command to command as
+described below. However, there is also a core set of common OPTIONS
+supported by all commands. So perhaps a better representation of the
+general I2C command would be:
+
+ i2c <cmd> [OPTIONS] [arguments]
+
+Where [OPTIONS] represents the common options and and arguments represent
+the operation-specific arguments.
+
+COMMON COMMAND OPTIONS
+======================
+
+"Sticky" Options
+----------------
+In order to interact with I2C devices, there are a number of I2C parameters
+that must be set correctly. One way to do this would be to provide to set
+the value of each separate command for each I2C parameter. The I2C tool
+takes a different approach, instead: The I2C configuration can be specified
+as a (potentially long) sequence of command line arguments.
+
+These arguments, however, are "sticky." They are sticky in the sense that
+once you set the I2C parameter, that value will remain until it is reset
+with a new value (or until you reset the board).
+
+Environment Variables
+---------------------
+NOTE also that if environment variables are not disabled (by
+CONFIG_DISABLE_ENVIRON=y), then these options may also be environment
+variables. Environment variables must be preceded with the special
+character $. For example, PWD is the variable that holds the current
+working directory and so $PWD could be used as a command line argument. The
+use of environment variables on the I2C tools command is really only useful
+if you wish to write NSH scripts to execute a longer, more complex series of
+I2C commands.
+
+Common Option Summary
+---------------------
+
+[-a addr] is the I2C device address (hex). Default: 03 Current: 03
+
+ The [-a addr] sets the I2C device address. The valid range is 0x03
+ through 0x77 (this valid range is controlled by the configuration settings
+ CONFIG_I2CTOOL_MINADDR and CONFIG_I2CTOOL_MAXADDR). If you are working
+ with the same device, the address needs to be set only once.
+
+ All I2C address are 7-bit, hexadecimal values.
+
+ NOTE 1: Notice in the "help" output above it shows both default value of
+ the I2C address (03 hex) and the current address value (also 03 hex).
+
+ NOTE 2: Sometimes I2C addresses are represented as 8-bit values (with
+ bit zero indicating a read or write operation). The I2C tool uses a
+ 7-bit representation of the address with bit 7 unused and no read/write
+ indication in bit 0. Essentially, the 7-bit address is like the 8-bit
+ address shifted right by 1.
+
+ NOTE 3: Most I2C bus controllers will also support 10-bit addressing.
+ That capability has not been integrated into the I2C tool as of this
+ writing.
+
+[-b bus] is the I2C bus number (decimal). Default: 1 Current: 1
+
+ Most devices support multiple I2C devices and also have unique bus
+ numbering. This option identifies which bus you are working with now.
+ The valid range of bus numbers is controlled by the configuration settings
+ CONFIG_I2CTOOL_MINBUS and CONFIG_I2CTOOL_MAXBUS.
+
+ The bus numbers are small, decimal numbers.
+
+[-r regaddr] is the I2C device register address (hex). Default: 00 Current: 00
+
+ The I2C set and get commands will access registers on the I2C device. This
+ option selects the device register address (sometimes called the sub-address).
+ This is an 8-bit hexadecimal value. The maximum value is determined by
+ the configuration setting CONFIG_I2CTOOL_MAXREGADDR.
+
+[-w width] is the data width (8 or 16 decimal). Default: 8 Current: 8
+
+ Device register data may be 8-bit or 16-bit. This options selects one of
+ those two data widths.
+
+[-s|n], send/don't send start between command and data. Default: -n Current: -n
+
+ This determines whether or not there should be a new I2C START between
+ sending of the register address and sending/receiving of the register data.
+
+[-i|j], Auto increment|don't increment regaddr on repititions. Default: NO Current: NO
+
+ On commands that take a optional number of repetitions, the option can be
+ used to temporarily increment the regaddr value by one on each repitition.
+
+[-f freq] I2C frequency. Default: 400000 Current: 400000
+
+ The [-f freq] sets the frequency of the I2C device.
+
+COMMAND SUMMARY
+===============
+
+We have already seen the I2C help (or ?) commands above. This section will
+discusse the remaining commands.
+
+List buses: bus [OPTIONS]
+--------------------------
+
+This command will simply list all of the configured I2C buses and indicate
+which are supported by the driver and which are not:
+
+ BUS EXISTS?
+ Bus 1: YES
+ Bus 2: NO
+
+The valid range of bus numbers is controlled by the configuration settings
+CONFIG_I2CTOOL_MINBUS and CONFIG_I2CTOOL_MAXBUS.
+
+List devices: dev [OPTIONS] <first> <last>
+------------------------------------------
+
+The 'dev' command will attempt to identify all of the I2C devices on the
+selected bus. The <first> and <last> arguments are 7-bit, hexadecimal
+I2C addresses. This command will examine a range of addresses beginning
+with <first> and continuing through <last>. It will request the value
+of register zero from each device.
+
+If the device at an address responds, then this command will display the
+address of the device. If the device does not respond, this command will
+display "--". The resulting display is like:
+
+nsh> i2c dev 03 77
+ 0 1 2 3 4 5 6 7 8 9 a b c d e f
+00: -- -- -- -- -- -- -- -- -- -- -- -- --
+10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
+20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
+30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
+40: -- -- -- -- -- -- -- -- -- 49 -- -- -- -- -- --
+50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
+70: -- -- -- -- -- -- -- --
+
+WARNINGS:
+ o The I2C dev command may have bad side effects on certain I2C devices.
+ For example, if could cause data loss in an EEPROM device.
+ o The I2C dev command also depends upon the underlying behavior of the
+ I2C driver. How does the driver respond to addressing failures?
+
+Read register: get [OPTIONS]
+----------------------------
+
+ This command will read the value of the I2C register using the selected
+ I2C parameters in the common options. No other arguments are required.
+
+ This command with write the 8-bit address value then read an 8- or 16-bit
+ data value from the device. Optionally, it may re-start the transfer
+ before obtaining the data.
+
+ An optional <repititions> argument can be supplied to repeat the
+ read operation an arbitrary number of times (up to 2 billion). If
+ auto-increment is select (-i), then the register address will be
+ temporarily incremented on each repitions. The increment is temporary
+ in the since that it will not alter the "sticky" value of the
+ register address.
+
+ On success, the output will look like the following (the data value
+ read will be shown as a 4-character hexadecimal number if the 16-bit
+ data width option is selected).
+
+ READ Bus: 1 Addr: 49 Subaddr: 04 Value: 96
+
+ All values (except the bus numbers) are hexadecimal.
+
+Write register: set [OPTIONS] <value>
+-------------------------------------
+
+ This command will write a value to an I2C register using the selected
+ I2C parameters in the common options. The value to write must be provided
+ as the final, hexadecimal value. This value may be an 8-bit value (in the
+ range 00-ff) or a 16-bit value (in the range 0000-ffff), depending upon
+ the selected data width.
+
+ This command will write the 8-bit address value then write the 8- or 16-bit
+ data value to the device. Optionally, it may re-start the transfer
+ before writing the data.
+
+ An optional <repititions> argument can be supplied to repeat the
+ write operation an arbitrary number of times (up to 2 billion). If
+ auto-increment is select (-i), then the register address will be
+ temporarily incremented on each repitions. The increment is temporary
+ in the since that it will not alter the "sticky" value of the
+ register address.
+
+ On success, the output will look like the following (the data value
+ written will be shown as a 4-character hexadecimal number if the 16-bit
+ data width option is selected).
+
+ WROTE Bus: 1 Addr: 49 Subaddr: 04 Value: 96
+
+ All values (except the bus numbers) are hexadecimal.
+
+Verify access : verf [OPTIONS] <value> [<repititions>]
+------------------------------------------------------
+
+ This command combines writing and reading from an I2C device register.
+ It will write a value to an will write a value to an I2C register using
+ the selected I2C parameters in the common options just as described for
+ tie 'set' command. Then this command will read the value back just
+ as described with the 'get' command. Finally, this command will compare
+ the value read and against the value written and emit an error message
+ if they do not match.
+
+ If no value is provided, then this command will use the register address
+ itself as the data, providing for a address-in-address test.
+
+ An optional <repititions> argument can be supplied to repeat the
+ verify operation an arbitrary number of times (up to 2 billion). If
+ auto-increment is select (-i), then the register address will be
+ temporarily incremented on each repitions. The increment is temporary
+ in the since that it will not alter the "sticky" value of the
+ register address.
+
+ On success, the output will look like the following (the data value
+ written will be shown as a 4-character hexadecimal number if the 16-bit
+ data width option is selected).
+
+ VERIFY Bus: 1 Addr: 49 Subaddr: 04 Wrote: 96 Read: 92 FAILURE
+
+ All values (except the bus numbers) are hexadecimal.
+
+I2C BUILD CONFIGURATION
+=======================
+
+NuttX Configuration Requirements
+--------------------------------
+The I2C tools requires the following in your NuttX configuration:
+
+1. Device-specific I2C support must be enabled. The I2C tool will call the
+ platform-specific function up_i2cinitialize() to get instances of the
+ I2C interface and the platform-specific function up_i2cuninitialize()
+ to discard instances of the I2C interface.
+
+ NOTE 1: The I2C interface is defined in include/nuttx/i2c.h.
+
+ NOTE 2: This I2C tool uses direct I2C device interfaces. As such, it
+ relies on internal OS interfaces that are not normally available to a
+ user-space program. As a result, the I2C tool cannot be used if a
+ NuttX is built as a protected, supervisor kernel (CONFIG_NUTTX_KERNEL).
+
+2. I2C driver configuration
+
+ The CONFIG_I2C_TRANSFER option must also be set in your NuttX
+ configuration. This configuration is the defconfig file in your
+ configuration directory that is copied to the NuttX top-level
+ directory as .config when NuttX is configured.
+
+ CONFIG_I2C_TRANSFER=y
+
+ NOTE: CONFIG_I2C_TRANSFER adds extra methods to the I2C interface.
+ Not all I2C interfaces support these extra methods. If your platform's
+ I2C driver does not support these extra methods, then you cannot use
+ the I2C tool unless you extend the support in your platform I2C
+ driver.
+
+3. Application configuration.
+
+ The path to the I2C tool directory must also be set in your NuttX
+ application configuration. This application configuration is the
+ appconfig file in your configuration directory that is copied to the
+ NuttX application directory as .config when NuttX is configured.
+
+ CONFIGURE_APPS += system/i2c
+
+I2C Tool Configuration Options
+------------------------------
+
+The default behavior of the I2C tool can be modified by the setting the
+options in the NuttX configuration. This configuration is the defconfig
+file in your configuration directory that is copied to the NuttX top-level
+directory as .config when NuttX is configured.
+
+ CONFIG_I2CTOOL_BUILTIN: Build the tools as an NSH built-in command
+ CONFIG_I2CTOOL_MINBUS: Smallest bus index supported by the hardware (default 0).
+ CONFIG_I2CTOOL_MAXBUS: Largest bus index supported by the hardware (default 3)
+ CONFIG_I2CTOOL_MINADDR: Minium device address (default: 0x03)
+ CONFIG_I2CTOOL_MAXADDR: Largest device address (default: 0x77)
+ CONFIG_I2CTOOL_MAXREGADDR: Largest register address (default: 0xff)
+ CONFIG_I2CTOOL_DEFFREQ: Default frequency (default: 4000000)
diff --git a/apps/system/i2c/i2c_bus.c b/apps/system/i2c/i2c_bus.c new file mode 100644 index 000000000..07e6d2da3 --- /dev/null +++ b/apps/system/i2c/i2c_bus.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * apps/system/i2c/i2c_bus.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <nuttx/i2c.h> + +#include "i2ctool.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: i2ccmd_bus + ****************************************************************************/ + +int i2ccmd_bus(FAR struct i2ctool_s *i2ctool, int argc, char **argv) +{ + FAR struct i2c_dev_s *dev; + int i; + + i2ctool_printf(i2ctool, " BUS EXISTS?\n"); + for (i = CONFIG_I2CTOOL_MINBUS; i <= CONFIG_I2CTOOL_MAXBUS; i++) + { + dev = up_i2cinitialize(i); + if (dev) + { + i2ctool_printf(i2ctool, "Bus %d: YES\n", i); + (void)up_i2cuninitialize(dev); + } + else + { + i2ctool_printf(i2ctool, "Bus %d: NO\n", i); + } + } + + return OK; +} diff --git a/apps/system/i2c/i2c_common.c b/apps/system/i2c/i2c_common.c new file mode 100644 index 000000000..e7b27693a --- /dev/null +++ b/apps/system/i2c/i2c_common.c @@ -0,0 +1,216 @@ +/**************************************************************************** + * apps/system/i2c/i2c_common.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> + +#include "i2ctool.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: common_args + ****************************************************************************/ + +int common_args(FAR struct i2ctool_s *i2ctool, FAR char **arg) +{ + FAR char *ptr = *arg; + long value; + int ret; + + if (ptr[0] != '-') + { + goto invalid_argument; + } + + switch (ptr[1]) + { + case 'a': + ret = arg_hex(arg, &value); + if (value < CONFIG_I2CTOOL_MINADDR || value > CONFIG_I2CTOOL_MAXADDR) + { + goto out_of_range; + } + + i2ctool->addr = (uint8_t) value; + return ret; + + case 'b': + ret = arg_decimal(arg, &value); + if (value < CONFIG_I2CTOOL_MINBUS || value > CONFIG_I2CTOOL_MAXBUS) + { + goto out_of_range; + } + + i2ctool->bus = (uint8_t) value; + return ret; + + case 'f': + ret = arg_decimal(arg, &value); + if (value == 0) + { + goto out_of_range; + } + + i2ctool->freq = value; + return ret; + + case 'i': + i2ctool->autoincr = true; + return 1; + + case 'j': + i2ctool->autoincr = false; + return 1; + + case 'n': + i2ctool->start = false; + return 1; + + case 'r': + ret = arg_hex(arg, &value); + if (value < 0 || value > CONFIG_I2CTOOL_MAXREGADDR) + { + goto out_of_range; + } + + i2ctool->regaddr = (uint8_t) value; + return ret; + + case 's': + i2ctool->start = true; + return 1; + + case 'w': + ret = arg_decimal(arg, &value); + if (value != 8 && value != 16) + { + goto out_of_range; + } + + i2ctool->width = (uint8_t) value; + return ret; + + default: + goto invalid_argument; + } + +invalid_argument: + i2ctool_printf(i2ctool, g_i2carginvalid, ptr); + return ERROR; + +out_of_range: + i2ctool_printf(i2ctool, g_i2cargrange, ptr); + return ERROR; +} + +/**************************************************************************** + * Name: arg_string + ****************************************************************************/ + +int arg_string(FAR char **arg, FAR char **value) +{ + FAR char *ptr = *arg; + + if (ptr[2] == '\0') + { + *value = arg[1]; + return 2; + } + else + { + *value = &ptr[2]; + return 1; + } +} + +/**************************************************************************** + * Name: arg_decimal + ****************************************************************************/ + +int arg_decimal(FAR char **arg, FAR long *value) +{ + FAR char *string; + int ret; + + ret = arg_string(arg, &string); + *value = strtol(string, NULL, 10); + return ret; +} + +/**************************************************************************** + * Name: arg_hex + ****************************************************************************/ + +int arg_hex(FAR char **arg, FAR long *value) +{ + FAR char *string; + int ret; + + ret = arg_string(arg, &string); + *value = strtol(string, NULL, 16); + return ret; +} diff --git a/apps/system/i2c/i2c_dev.c b/apps/system/i2c/i2c_dev.c new file mode 100644 index 000000000..f2b5a0e21 --- /dev/null +++ b/apps/system/i2c/i2c_dev.c @@ -0,0 +1,235 @@ +/**************************************************************************** + * apps/system/i2c/i2c_dev.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> + +#include <nuttx/i2c.h> + +#include "i2ctool.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: i2ccmd_dev + ****************************************************************************/ + +int i2ccmd_dev(FAR struct i2ctool_s *i2ctool, int argc, char **argv) +{ + FAR struct i2c_dev_s *dev; + struct i2c_msg_s msg[2]; + FAR char *ptr; + union + { + uint16_t data16; + uint8_t data8; + } u; + + uint8_t regaddr; + long first; + long last; + int addr; + int nargs; + int argndx; + int ret; + int i; + int j; + + /* Parse any command line arguments */ + + for (argndx = 1; argndx < argc; ) + { + /* Break out of the look when the last option has been parsed */ + + ptr = argv[argndx]; + if (*ptr != '-') + { + break; + } + + /* Otherwise, check for common options */ + + nargs = common_args(i2ctool, &argv[argndx]); + if (nargs < 0) + { + return ERROR; + } + argndx += nargs; + } + + /* There should be exactly two more things on the command line: The first and + * last addresses to be probed. + */ + + if (argndx+1 < argc) + { + first = strtol(argv[argndx], NULL, 16); + last = strtol(argv[argndx+1], NULL, 16); + if (first < 0 || first > 0x7f || last < 0 || last > 0x7f || first > last) + { + i2ctool_printf(i2ctool, g_i2cargrange, argv[0]); + return ERROR; + } + + argndx += 2; + } + else + { + i2ctool_printf(i2ctool, g_i2cargrequired, argv[0]); + return ERROR; + } + + if (argndx != argc) + { + i2ctool_printf(i2ctool, g_i2ctoomanyargs, argv[0]); + return ERROR; + } + + /* Get a handle to the I2C bus */ + + dev = up_i2cinitialize(i2ctool->bus); + if (!dev) + { + i2ctool_printf(i2ctool, "Failed to get bus %d\n", i2ctool->bus); + return ERROR; + } + + /* Set the frequency and address (NOTE: Only 7-bit address supported now) */ + + I2C_SETFREQUENCY(dev, i2ctool->freq); + + /* Probe each address */ + + i2ctool_printf(i2ctool, " 0 1 2 3 4 5 6 7 8 9 a b c d e f\n"); + for (i = 0; i < 128; i += 16) + { + i2ctool_printf(i2ctool, "%02x: ", i); + for (j = 0; j < 16; j++) + { + /* Skip addresses that are out of the selected range */ + + addr = i+j; + if (addr < first || addr > last) + { + i2ctool_printf(i2ctool, " "); + continue; + } + + /* Set the I2C address */ + + I2C_SETADDRESS(dev, addr, 7); + + /* Set up data structures */ + + regaddr = 0; + + msg[0].addr = addr; + msg[0].flags = 0; + msg[0].buffer = ®addr; + msg[0].length = 1; + + msg[1].addr = addr; + msg[1].flags = I2C_M_READ; + if (i2ctool->width == 8) + { + msg[1].buffer = &u.data8; + msg[1].length = 1; + } + else + { + msg[1].buffer = (uint8_t*)&u.data16; + msg[1].length = 2; + } + + if (i2ctool->start) + { + ret = I2C_TRANSFER(dev, &msg[0], 1); + if (ret == OK) + { + ret = I2C_TRANSFER(dev, &msg[1], 1); + } + } + else + { + ret = I2C_TRANSFER(dev, msg, 2); + } + + if (ret == OK) + { + i2ctool_printf(i2ctool, "%02x ", addr); + } + else + { + i2ctool_printf(i2ctool, "-- "); + } + } + i2ctool_printf(i2ctool, "\n"); + i2ctool_flush(i2ctool); + } + + (void)up_i2cuninitialize(dev); + return OK; +} diff --git a/apps/system/i2c/i2c_get.c b/apps/system/i2c/i2c_get.c new file mode 100644 index 000000000..773f2c963 --- /dev/null +++ b/apps/system/i2c/i2c_get.c @@ -0,0 +1,257 @@ +/**************************************************************************** + * apps/system/i2c/i2c_get.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> + +#include <nuttx/i2c.h> + +#include "i2ctool.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: i2ccmd_get + ****************************************************************************/ + +int i2ccmd_get(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv) +{ + FAR struct i2c_dev_s *dev; + FAR char *ptr; + uint16_t result; + uint8_t regaddr; + long repititions; + int nargs; + int argndx; + int ret; + int i; + + /* Parse any command line arguments */ + + for (argndx = 1; argndx < argc; ) + { + /* Break out of the look when the last option has been parsed */ + + ptr = argv[argndx]; + if (*ptr != '-') + { + break; + } + + /* Otherwise, check for common options */ + + nargs = common_args(i2ctool, &argv[argndx]); + if (nargs < 0) + { + return ERROR; + } + argndx += nargs; + } + + /* There may be one more thing on the command line: The repitition + * count. + */ + + repititions = 1; + if (argndx < argc) + { + repititions = strtol(argv[argndx], NULL, 16); + if (repititions < 1) + { + i2ctool_printf(i2ctool, g_i2cargrange, argv[0]); + return ERROR; + } + + argndx++; + } + + if (argndx != argc) + { + i2ctool_printf(i2ctool, g_i2ctoomanyargs, argv[0]); + return ERROR; + } + + /* Get a handle to the I2C bus */ + + dev = up_i2cinitialize(i2ctool->bus); + if (!dev) + { + i2ctool_printf(i2ctool, "Failed to get bus %d\n", i2ctool->bus); + return ERROR; + } + + /* Set the frequency and the address (NOTE: Only 7-bit address supported now) */ + + I2C_SETFREQUENCY(dev, i2ctool->freq); + I2C_SETADDRESS(dev, i2ctool->addr, 7); + + /* Loop for the requested number of repititions */ + + regaddr = i2ctool->regaddr; + ret = OK; + + for (i = 0; i < repititions; i++) + { + /* Read from the I2C bus */ + + ret = i2ctool_get(i2ctool, dev, regaddr, &result); + + /* Display the result */ + + if (ret == OK) + { + i2ctool_printf(i2ctool, "READ Bus: %d Addr: %02x Subaddr: %02x Value: ", + i2ctool->bus, i2ctool->addr, i2ctool->regaddr); + if (i2ctool->width == 8) + { + i2ctool_printf(i2ctool, "%02x\n", result); + } + else + { + i2ctool_printf(i2ctool, "%04x\n", result); + } + } + else + { + i2ctool_printf(i2ctool, g_i2cxfrerror, argv[0], -ret); + break; + } + + /* Auto-increment the address if so configured */ + + if (i2ctool->autoincr) + { + regaddr++; + } + } + + (void)up_i2cuninitialize(dev); + return ret; +} + +/**************************************************************************** + * Name: i2ctool_get + ****************************************************************************/ + +int i2ctool_get(FAR struct i2ctool_s *i2ctool, FAR struct i2c_dev_s *dev, + uint8_t regaddr, uint16_t *result) +{ + struct i2c_msg_s msg[2]; + union + { + uint16_t data16; + uint8_t data8; + } u; + int ret; + + /* Set up data structures */ + + msg[0].addr = i2ctool->addr; + msg[0].flags = 0; + msg[0].buffer = ®addr; + msg[0].length = 1; + + msg[1].addr = i2ctool->addr; + msg[1].flags = I2C_M_READ; + if (i2ctool->width == 8) + { + msg[1].buffer = &u.data8; + msg[1].length = 1; + } + else + { + msg[1].buffer = (uint8_t*)&u.data16; + msg[1].length = 2; + } + + if (i2ctool->start) + { + ret = I2C_TRANSFER(dev, &msg[0], 1); + if (ret== OK) + { + ret = I2C_TRANSFER(dev, &msg[1], 1); + } + } + else + { + ret = I2C_TRANSFER(dev, msg, 2); + } + + /* Return the result of the read operation */ + + if (ret == OK) + { + if (i2ctool->width == 8) + { + *result = (uint16_t)u.data8; + } + else + { + *result = u.data16; + } + } + return ret; +} diff --git a/apps/system/i2c/i2c_main.c b/apps/system/i2c/i2c_main.c new file mode 100644 index 000000000..2a0e5d626 --- /dev/null +++ b/apps/system/i2c/i2c_main.c @@ -0,0 +1,454 @@ +/**************************************************************************** + * apps/system/i2c/i2c_main.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <string.h> +#include <stdarg.h> +#include <assert.h> +#include <errno.h> +#include <debug.h> + +#include "i2ctool.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int i2ccmd_help(FAR struct i2ctool_s *i2ctool, int argc, char **argv); +static int i2ccmd_unrecognized(FAR struct i2ctool_s *i2ctool, int argc, char **argv); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct i2ctool_s g_i2ctool; + +static const struct cmdmap_s g_i2ccmds[] = +{ + { "?", i2ccmd_help, "Show help ", NULL }, + { "bus", i2ccmd_bus, "List busses ", NULL }, + { "dev", i2ccmd_dev, "List devices ", "[OPTIONS] <first> <last>" }, + { "get", i2ccmd_get, "Read register ", "[OPTIONS] [<repititions>]" }, + { "help", i2ccmd_help, "Show help ", NULL }, + { "set", i2ccmd_set, "Write register", "[OPTIONS] <value> [<repititions>]" }, + { "verf", i2ccmd_verf, "Verify access ", "[OPTIONS] [<value>] [<repititions>]" }, + { NULL, NULL, NULL, NULL } +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* Common, message formats */ + +const char g_i2cargrequired[] = "i2ctool: %s: missing required argument(s)\n"; +const char g_i2carginvalid[] = "i2ctool: %s: argument invalid\n"; +const char g_i2cargrange[] = "i2ctool: %s: value out of range\n"; +const char g_i2ccmdnotfound[] = "i2ctool: %s: command not found\n"; +const char g_i2ctoomanyargs[] = "i2ctool: %s: too many arguments\n"; +const char g_i2ccmdfailed[] = "i2ctool: %s: %s failed: %d\n"; +const char g_i2cxfrerror[] = "i2ctool: %s: Transfer failed: %d\n"; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: i2ccmd_help + ****************************************************************************/ + +static int i2ccmd_help(FAR struct i2ctool_s *i2ctool, int argc, char **argv) +{ + const struct cmdmap_s *ptr; + + i2ctool_printf(i2ctool, "Usage: i2c <cmd> [arguments]\n"); + i2ctool_printf(i2ctool, "Where <cmd> is one of:\n\n"); + for (ptr = g_i2ccmds; ptr->cmd; ptr++) + { + if (ptr->usage) + { + i2ctool_printf(i2ctool, " %s: %s %s\n", ptr->desc, ptr->cmd, ptr->usage); + } + else + { + i2ctool_printf(i2ctool, " %s: %s\n", ptr->desc, ptr->cmd); + } + } + + i2ctool_printf(i2ctool, "\nWhere common \"sticky\" OPTIONS include:\n"); + i2ctool_printf(i2ctool, " [-a addr] is the I2C device address (hex). " + "Default: %02x Current: %02x\n", + CONFIG_I2CTOOL_MINADDR, i2ctool->addr); + i2ctool_printf(i2ctool, " [-b bus] is the I2C bus number (decimal). " + "Default: %d Current: %d\n", + CONFIG_I2CTOOL_MINBUS, i2ctool->bus); + i2ctool_printf(i2ctool, " [-r regaddr] is the I2C device register address (hex). " + "Default: 00 Current: %02x\n", + i2ctool->regaddr); + i2ctool_printf(i2ctool, " [-w width] is the data width (8 or 16 decimal). " + "Default: 8 Current: %d\n", + i2ctool->width); + i2ctool_printf(i2ctool, " [-s|n], send/don't send start between command and data. " + "Default: -n Current: %s\n", + i2ctool->start ? "-s" : "-n"); + i2ctool_printf(i2ctool, " [-i|j], Auto increment|don't increment regaddr on repititions. " + "Default: NO Current: %s\n", + i2ctool->autoincr ? "YES" : "NO"); + i2ctool_printf(i2ctool, " [-f freq] I2C frequency. " + "Default: %d Current: %d\n", + CONFIG_I2CTOOL_DEFFREQ, i2ctool->freq); + i2ctool_printf(i2ctool, "\nNOTES:\n"); +#ifndef CONFIG_DISABLE_ENVIRON + i2ctool_printf(i2ctool, "o An environment variable like $PATH may be used for any argument.\n"); +#endif + i2ctool_printf(i2ctool, "o Arguments are \"sticky\". For example, once the I2C address is\n"); + i2ctool_printf(i2ctool, " specified, that address will be re-used until it is changed.\n"); + i2ctool_printf(i2ctool, "\nWARNING:\n"); + i2ctool_printf(i2ctool, "o The I2C dev command may have bad side effects on your I2C devices.\n"); + i2ctool_printf(i2ctool, " Use only at your own risk.\n"); + return OK; +} + +/**************************************************************************** + * Name: i2ccmd_unrecognized + ****************************************************************************/ + +static int i2ccmd_unrecognized(FAR struct i2ctool_s *i2ctool, int argc, char **argv) +{ + i2ctool_printf(i2ctool, g_i2ccmdnotfound, argv[0]); + return ERROR; +} + +/**************************************************************************** + * Name: i2c_execute + ****************************************************************************/ + +static int i2c_execute(FAR struct i2ctool_s *i2ctool, int argc, char *argv[]) +{ + const struct cmdmap_s *cmdmap; + const char *cmd; + cmd_t handler; + int ret; + + /* The form of argv is: + * + * argv[0]: The command name. This is argv[0] when the arguments + * are, finally, received by the command vtblr + * argv[1]: The beginning of argument (up to MAX_ARGUMENTS) + * argv[argc]: NULL terminating pointer + */ + + /* See if the command is one that we understand */ + + cmd = argv[0]; + handler = i2ccmd_unrecognized; + + for (cmdmap = g_i2ccmds; cmdmap->cmd; cmdmap++) + { + if (strcmp(cmdmap->cmd, cmd) == 0) + { + handler = cmdmap->handler; + break; + } + } + + ret = handler(i2ctool, argc, argv); + return ret; +} + +/**************************************************************************** + * Name: i2c_argument + ****************************************************************************/ + +FAR char *i2c_argument(FAR struct i2ctool_s *i2ctool, int argc, char *argv[], int *pindex) +{ + FAR char *arg; + int index = *pindex; + + /* If we are at the end of the arguments with nothing, then return NULL */ + + if (index >= argc) + { + return NULL; + } + + /* Get the return parameter */ + + arg = argv[index]; + *pindex = index + 1; + +#ifndef CONFIG_DISABLE_ENVIRON + /* Check for references to environment variables */ + + if (arg[0] == '$') + { + /* Return the value of the environment variable with this name */ + + FAR char *value = getenv(arg+1); + if (value) + { + return value; + } + else + { + return (FAR char*)""; + } + } +#endif + + /* Return the next argument. */ + + return arg; +} + +/**************************************************************************** + * Name: i2c_parse + ****************************************************************************/ + +int i2c_parse(FAR struct i2ctool_s *i2ctool, int argc, char *argv[]) +{ + FAR char *newargs[MAX_ARGUMENTS+2]; + FAR char *cmd; + int nargs; + int index; + + /* Parse out the command, skipping the first argument (the program name)*/ + + index = 1; + cmd = i2c_argument(i2ctool, argc, argv, &index); + + /* Check if any command was provided */ + + if (!cmd) + { + /* An empty line is not an error and an unprocessed command cannot + * generate an error, but neither should they change the last + * command status. + */ + + return i2ccmd_help(i2ctool, 0, NULL); + } + + /* Parse all of the arguments following the command name. */ + + newargs[0] = cmd; + for (nargs = 1; nargs <= MAX_ARGUMENTS; nargs++) + { + newargs[nargs] = i2c_argument(i2ctool, argc, argv, &index); + if (!newargs[nargs]) + { + break; + } + } + newargs[nargs] = NULL; + + /* Then execute the command */ + + return i2c_execute(i2ctool, nargs, newargs); +} + +/**************************************************************************** + * Name: i2c_setup + ****************************************************************************/ + +static inline int i2c_setup(FAR struct i2ctool_s *i2ctool) +{ + /* Initialize the output stream */ + +#ifdef CONFIG_I2CTOOL_OUTDEV + i2ctool->ss_outfd = open(CONFIG_I2CTOOL_OUTDEV, O_WRONLY); + if (i2ctool->ss_outfd < 0) + { + fprintf(stderr, g_i2ccmdfailed, "open", errno); + return ERROR; + } + + /* Create a standard C stream on the console device */ + + i2ctool->ss_outstream = fdopen(i2ctool->ss_outfd, "w"); + if (!i2ctool->ss_outstream) + { + fprintf(stderr, g_i2ccmdfailed, "fdopen", errno); + return ERROR; + } +#endif + + return OK; +} + +/**************************************************************************** + * Name: i2c_teardown + * + * Description: + * Close the output stream if it is not the standard output stream. + * + ****************************************************************************/ + +static void i2c_teardown(FAR struct i2ctool_s *i2ctool) +{ + fflush(OUTSTREAM(&g_i2ctool)); + +#ifdef CONFIG_I2CTOOL_OUTDEV + fclose(i2ctool->ss_outstream); +#endif +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: i2c_main + ****************************************************************************/ + +#ifdef CONFIG_I2CTOOL_BUILTIN +# define MAIN_NAME i2c_main +# define MAIN_NAME_STRING "i2c_main" +#else +# define MAIN_NAME user_start +# define MAIN_NAME_STRING "user_start" +#endif + +int MAIN_NAME(int argc, char *argv[]) +{ + /* Verify settings */ + + if (g_i2ctool.bus < CONFIG_I2CTOOL_MINBUS || g_i2ctool.bus > CONFIG_I2CTOOL_MAXBUS) + { + g_i2ctool.bus = CONFIG_I2CTOOL_MINBUS; + } + + if (g_i2ctool.addr < CONFIG_I2CTOOL_MINADDR || g_i2ctool.addr > CONFIG_I2CTOOL_MAXADDR) + { + g_i2ctool.addr = CONFIG_I2CTOOL_MINADDR; + } + + if (g_i2ctool.regaddr < CONFIG_I2CTOOL_MAXREGADDR) + { + g_i2ctool.regaddr = 0; + } + + if (g_i2ctool.width != 8 && g_i2ctool.width != 16) + { + g_i2ctool.width = 8; + } + + if (g_i2ctool.freq == 0) + { + g_i2ctool.freq = CONFIG_I2CTOOL_DEFFREQ; + } + + /* Parse process the command line */ + + i2c_setup(&g_i2ctool); + (void)i2c_parse(&g_i2ctool, argc, argv); + + i2ctool_flush(&g_i2ctool); + i2c_teardown(&g_i2ctool); + return OK; +} + +/**************************************************************************** + * Name: i2ctool_printf + * + * Description: + * Print a string to the currently selected stream. + * + ****************************************************************************/ + +int i2ctool_printf(FAR struct i2ctool_s *i2ctool, const char *fmt, ...) +{ + va_list ap; + int ret; + + va_start(ap, fmt); + ret = vfprintf(OUTSTREAM(i2ctool), fmt, ap); + va_end(ap); + + return ret; +} + +/**************************************************************************** + * Name: i2ctool_write + * + * Description: + * write a buffer to the currently selected stream. + * + ****************************************************************************/ + +ssize_t i2ctool_write(FAR struct i2ctool_s *i2ctool, FAR const void *buffer, size_t nbytes) +{ + ssize_t ret; + + /* Write the data to the output stream */ + + ret = fwrite(buffer, 1, nbytes, OUTSTREAM(i2ctool)); + if (ret < 0) + { + dbg("[%d] Failed to send buffer: %d\n", OUTFD(i2ctool), errno); + } + return ret; +} + +/**************************************************************************** + * Name: i2ctool_flush + * + * Description: + * Flush buffered I/O to the currently selected stream. + * + ****************************************************************************/ + +void i2ctool_flush(FAR struct i2ctool_s *i2ctool) +{ + fflush(OUTSTREAM(i2ctool)); +} diff --git a/apps/system/i2c/i2c_set.c b/apps/system/i2c/i2c_set.c new file mode 100644 index 000000000..66fd8c85a --- /dev/null +++ b/apps/system/i2c/i2c_set.c @@ -0,0 +1,275 @@ +/**************************************************************************** + * apps/system/i2c/i2c_set.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> + +#include <nuttx/i2c.h> + +#include "i2ctool.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: i2ccmd_set + ****************************************************************************/ + +int i2ccmd_set(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv) +{ + FAR struct i2c_dev_s *dev; + FAR char *ptr; + uint8_t regaddr; + long value; + long repititions; + int nargs; + int argndx; + int ret; + int i; + + /* Parse any command line arguments */ + + for (argndx = 1; argndx < argc; ) + { + /* Break out of the look when the last option has been parsed */ + + ptr = argv[argndx]; + if (*ptr != '-') + { + break; + } + + /* Otherwise, check for common options */ + + nargs = common_args(i2ctool, &argv[argndx]); + if (nargs < 0) + { + return ERROR; + } + argndx += nargs; + } + + /* There must be at least one more thing on the command line: The value + * to be written. + */ + + if (argndx < argc) + { + value = strtol(argv[argndx], NULL, 16); + if (i2ctool->width == 8) + { + if (value < 0 || value > 255) + { + i2ctool_printf(i2ctool, g_i2cargrange, argv[0]); + return ERROR; + } + } + else if (value < 0 || value > 65535) + { + i2ctool_printf(i2ctool, g_i2cargrange, argv[0]); + return ERROR; + } + + argndx++; + } + else + { + i2ctool_printf(i2ctool, g_i2cargrequired, argv[0]); + return ERROR; + } + + /* There may be one more thing on the command line: The repitition + * count. + */ + + repititions = 1; + if (argndx < argc) + { + repititions = strtol(argv[argndx], NULL, 16); + if (repititions < 1) + { + i2ctool_printf(i2ctool, g_i2cargrange, argv[0]); + return ERROR; + } + + argndx++; + } + + if (argndx != argc) + { + i2ctool_printf(i2ctool, g_i2ctoomanyargs, argv[0]); + return ERROR; + } + + /* Get a handle to the I2C bus */ + + dev = up_i2cinitialize(i2ctool->bus); + if (!dev) + { + i2ctool_printf(i2ctool, "Failed to get bus %d\n", i2ctool->bus); + return ERROR; + } + + /* Set the frequency and the address (NOTE: Only 7-bit address supported now) */ + + I2C_SETFREQUENCY(dev, i2ctool->freq); + I2C_SETADDRESS(dev, i2ctool->addr, 7); + + /* Loop for the requested number of repititions */ + + regaddr = i2ctool->regaddr; + ret = OK; + + for (i = 0; i < repititions; i++) + { + /* Write to the I2C bus */ + + ret = i2ctool_set(i2ctool, dev, regaddr, (uint16_t)value); + + /* Display the result */ + + if (ret == OK) + { + i2ctool_printf(i2ctool, "WROTE Bus: %d Addr: %02x Subaddr: %02x Value: ", + i2ctool->bus, i2ctool->addr, i2ctool->regaddr); + if (i2ctool->width == 8) + { + i2ctool_printf(i2ctool, "%02x\n", (int)value); + } + else + { + i2ctool_printf(i2ctool, "%04x\n", (int)value); + } + } + else + { + i2ctool_printf(i2ctool, g_i2cxfrerror, argv[0], -ret); + break; + } + + /* Auto-increment the address if so configured */ + + if (i2ctool->autoincr) + { + regaddr++; + } + } + + (void)up_i2cuninitialize(dev); + return ret; +} + +/**************************************************************************** + * Name: i2ctool_set + ****************************************************************************/ + +int i2ctool_set(FAR struct i2ctool_s *i2ctool, FAR struct i2c_dev_s *dev, + uint8_t regaddr, uint16_t value) +{ + struct i2c_msg_s msg[2]; + union + { + uint16_t data16; + uint8_t data8; + } u; + int ret; + + /* Set up data structures */ + + msg[0].addr = i2ctool->addr; + msg[0].flags = 0; + msg[0].buffer = ®addr; + msg[0].length = 1; + + msg[1].addr = i2ctool->addr; + msg[1].flags = 0; + if (i2ctool->width == 8) + { + u.data8 = (uint8_t)value; + msg[1].buffer = &u.data8; + msg[1].length = 1; + } + else + { + u.data16 = value; + msg[1].buffer = (uint8_t*)&u.data16; + msg[1].length = 2; + } + + if (i2ctool->start) + { + ret = I2C_TRANSFER(dev, &msg[0], 1); + if (ret == OK) + { + ret = I2C_TRANSFER(dev, &msg[1], 1); + } + } + else + { + ret = I2C_TRANSFER(dev, msg, 2); + } + + return ret; +} diff --git a/apps/system/i2c/i2c_verf.c b/apps/system/i2c/i2c_verf.c new file mode 100644 index 000000000..0fa34b96e --- /dev/null +++ b/apps/system/i2c/i2c_verf.c @@ -0,0 +1,249 @@ +/**************************************************************************** + * apps/system/i2c/i2c_verf.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdlib.h> + +#include <nuttx/i2c.h> + +#include "i2ctool.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: i2ccmd_verf + ****************************************************************************/ + +int i2ccmd_verf(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv) +{ + FAR struct i2c_dev_s *dev; + FAR char *ptr; + uint16_t rdvalue; + uint8_t regaddr; + bool addrinaddr; + long wrvalue; + long repititions; + int nargs; + int argndx; + int ret; + int i; + + /* Parse any command line arguments */ + + for (argndx = 1; argndx < argc; ) + { + /* Break out of the look when the last option has been parsed */ + + ptr = argv[argndx]; + if (*ptr != '-') + { + break; + } + + /* Otherwise, check for common options */ + + nargs = common_args(i2ctool, &argv[argndx]); + if (nargs < 0) + { + return ERROR; + } + argndx += nargs; + } + + /* The options may be followed by the optional wrvalue to be written. If omitted, then + * the register address will be used as the wrvalue, providing an address-in-address + * test. + */ + + addrinaddr = true; + wrvalue = 0; + + if (argndx < argc) + { + wrvalue = strtol(argv[argndx], NULL, 16); + if (i2ctool->width == 8) + { + if (wrvalue < 0 || wrvalue > 255) + { + i2ctool_printf(i2ctool, g_i2cargrange, argv[0]); + return ERROR; + } + } + else if (wrvalue < 0 || wrvalue > 65535) + { + i2ctool_printf(i2ctool, g_i2cargrange, argv[0]); + return ERROR; + } + + addrinaddr = false; + argndx++; + } + + /* There may be one more thing on the command line: The repitition + * count. + */ + + repititions = 1; + if (argndx < argc) + { + repititions = strtol(argv[argndx], NULL, 16); + if (repititions < 1) + { + i2ctool_printf(i2ctool, g_i2cargrange, argv[0]); + return ERROR; + } + + argndx++; + } + + if (argndx != argc) + { + i2ctool_printf(i2ctool, g_i2ctoomanyargs, argv[0]); + return ERROR; + } + + /* Get a handle to the I2C bus */ + + dev = up_i2cinitialize(i2ctool->bus); + if (!dev) + { + i2ctool_printf(i2ctool, "Failed to get bus %d\n", i2ctool->bus); + return ERROR; + } + + /* Set the frequency and the address (NOTE: Only 7-bit address supported now) */ + + I2C_SETFREQUENCY(dev, i2ctool->freq); + I2C_SETADDRESS(dev, i2ctool->addr, 7); + + /* Loop for the requested number of repititions */ + + regaddr = i2ctool->regaddr; + ret = OK; + + for (i = 0; i < repititions; i++) + { + /* If we are performing an address-in-address test, then use the register + * address as the value to write. + */ + + if (addrinaddr) + { + wrvalue = regaddr; + } + + /* Write to the I2C bus */ + + ret = i2ctool_set(i2ctool, dev, regaddr, (uint16_t)wrvalue); + if (ret == OK) + { + /* Read the value back from the I2C bus */ + + ret = i2ctool_get(i2ctool, dev, regaddr, &rdvalue); + } + + /* Display the result */ + + if (ret == OK) + { + i2ctool_printf(i2ctool, "VERIFY Bus: %d Addr: %02x Subaddr: %02x Wrote: ", + i2ctool->bus, i2ctool->addr, i2ctool->regaddr); + + if (i2ctool->width == 8) + { + i2ctool_printf(i2ctool, "%02x Read: %02x", (int)wrvalue, (int)rdvalue); + } + else + { + i2ctool_printf(i2ctool, "%04x Read: %04x", (int)wrvalue, (int)rdvalue); + } + + if (wrvalue != rdvalue) + { + i2ctool_printf(i2ctool, " <<< FAILURE\n"); + } + else + { + i2ctool_printf(i2ctool, "\n"); + } + } + else + { + i2ctool_printf(i2ctool, g_i2cxfrerror, argv[0], -ret); + break; + } + + /* Auto-increment the address if so configured */ + + if (i2ctool->autoincr) + { + regaddr++; + } + } + + (void)up_i2cuninitialize(dev); + return ret; +} diff --git a/apps/system/i2c/i2ctool.h b/apps/system/i2c/i2ctool.h new file mode 100644 index 000000000..9726f0083 --- /dev/null +++ b/apps/system/i2c/i2ctool.h @@ -0,0 +1,210 @@ +/**************************************************************************** + * apps/system/i2c/i2ctool.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#ifndef __APPS_SYSTEM_I2C_I2CTOOLS_H +#define __APPS_SYSTEM_I2C_I2CTOOLS_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/compiler.h> + +#include <sys/types.h> +#include <stdio.h> +#include <stdint.h> +#include <stdbool.h> +#include <errno.h> + +#include <nuttx/i2c.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* CONFIG_I2CTOOL_BUILTIN - Build the tools as an NSH built-in command + * CONFIG_I2CTOOL_MINBUS - Smallest bus index supported by the hardware (default 0). + * CONFIG_I2CTOOL_MAXBUS - Largest bus index supported by the hardware (default 3) + * CONFIG_I2CTOOL_MINADDR - Minium device address (default: 0x03) + * CONFIG_I2CTOOL_MAXADDR - Largest device address (default: 0x77) + * CONFIG_I2CTOOL_MAXREGADDR - Largest register address (default: 0xff) + * CONFIG_I2CTOOL_DEFFREQ - Default frequency (default: 4000000) + */ + +#ifndef CONFIG_I2C_TRANSFER +# error "CONFIG_I2C_TRANSFER is required in the configuration" +#endif + +#ifndef CONFIG_I2CTOOL_MINBUS +# define CONFIG_I2CTOOL_MINBUS 0 +#endif + +#ifndef CONFIG_I2CTOOL_MAXBUS +# define CONFIG_I2CTOOL_MAXBUS 3 +#endif + +#ifndef CONFIG_I2CTOOL_MINADDR +# define CONFIG_I2CTOOL_MINADDR 0x03 +#endif + +#ifndef CONFIG_I2CTOOL_MAXADDR +# define CONFIG_I2CTOOL_MAXADDR 0x77 +#endif + +#ifndef CONFIG_I2CTOOL_MAXREGADDR +# define CONFIG_I2CTOOL_MAXREGADDR 0xff +#endif + +#ifndef CONFIG_I2CTOOL_DEFFREQ +# define CONFIG_I2CTOOL_DEFFREQ 100000 +#endif + +/* This is the maximum number of arguments that will be accepted for a + * command. The only real limit is in the OS configuration that limits + * the number of parameters passed to a task. + */ + +#define MAX_ARGUMENTS (CONFIG_MAX_TASK_ARGS-1) + +/* Maximum size of one command line */ + +#define MAX_LINELEN 80 + +/* Are we using the NuttX console for I/O? Or some other character device? */ + +#ifdef CONFIG_I2CTOOL_INDEV +# define INFD(p) ((p)->ss_infd) +# define INSTREAM(p) ((p)->ss_instream) +#else +# define INFD(p) 0 +# define INSTREAM(p) stdin +#endif + +#ifdef CONFIG_I2CTOOL_OUTDEV +# define OUTFD(p) ((p)->ss_outfd) +# define OUTSTREAM(p) ((p)->ss_outstream) +#else +# define OUTFD(p) 1 +# define OUTSTREAM(p) stdout +#endif + +/* Output is via printf but can be changed using this macro */ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# define i2c_output(v, fmt...) printf(v, ##fmt) +#else +# define i2c_output printf +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct i2ctool_s +{ + /* Sticky options */ + + uint8_t addr; /* [-a addr] is the I2C device address */ + uint8_t bus; /* [-b bus] is the I2C bus number */ + uint8_t regaddr; /* [-r regaddr] is the I2C device register address */ + uint8_t width; /* [-w width] is the data width (8 or 16) */ + bool start; /* [-s|n], send|don't send start between command and data */ + bool autoincr; /* [-i|j], Auto increment|don't increment regaddr on repititions */ + uint32_t freq; /* [-f freq] I2C frequency */ + + /* Output streams */ + +#ifdef CONFIG_I2CTOOL_OUTDEV + int ss_outfd; /* Output file descriptor */ + FILE *ss_outstream; /* Output stream */ +#endif +}; + +typedef int (*cmd_t)(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv); + +struct cmdmap_s +{ + FAR const char *cmd; /* Name of the command */ + cmd_t handler; /* Function that handles the command */ + FAR const char *desc; /* Short description */ + FAR const char *usage; /* Usage instructions for 'help' command */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +extern const char g_i2cargrequired[]; +extern const char g_i2carginvalid[]; +extern const char g_i2cargrange[]; +extern const char g_i2ccmdnotfound[]; +extern const char g_i2ctoomanyargs[]; +extern const char g_i2ccmdfailed[]; +extern const char g_i2cxfrerror[]; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/* Message handler */ + +ssize_t i2ctool_write(FAR struct i2ctool_s *i2ctool, FAR const void *buffer, size_t nbytes); +int i2ctool_printf(FAR struct i2ctool_s *i2ctool, const char *fmt, ...); +void i2ctool_flush(FAR struct i2ctool_s *i2ctool); + +/* Command handlers */ + +int i2ccmd_bus(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv); +int i2ccmd_dev(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv); +int i2ccmd_get(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv); +int i2ccmd_set(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv); +int i2ccmd_verf(FAR struct i2ctool_s *i2ctool, int argc, FAR char **argv); + +/* I2C access functions */ + +int i2ctool_get(FAR struct i2ctool_s *i2ctool, FAR struct i2c_dev_s *dev, + uint8_t addr, uint16_t *result); +int i2ctool_set(FAR struct i2ctool_s *i2ctool, FAR struct i2c_dev_s *dev, + uint8_t regaddr, uint16_t value); + +/* Common logic */ + +int common_args(FAR struct i2ctool_s *i2ctool, FAR char **arg); +int arg_string(FAR char **arg, FAR char **value); +int arg_decimal(FAR char **arg, FAR long *value); +int arg_hex(FAR char **arg, FAR long *value); + +#endif /* __APPS_SYSTEM_I2C_I2CTOOLS_H */ diff --git a/apps/system/install/Kconfig b/apps/system/install/Kconfig new file mode 100644 index 000000000..a48f67f20 --- /dev/null +++ b/apps/system/install/Kconfig @@ -0,0 +1,14 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config SYSTEM_INSTALL + bool "FLASH installation tool" + default n + ---help--- + Enable support for the FLASH installation tool. + +if SYSTEM_INSTALL +endif + diff --git a/apps/system/install/Makefile b/apps/system/install/Makefile new file mode 100755 index 000000000..4528ad44e --- /dev/null +++ b/apps/system/install/Makefile @@ -0,0 +1,114 @@ +############################################################################ +# apps/system/install/Makefile +# +# Copyright (C) 2011 Uros Platise. All rights reserved. +# Author: Uros Platise <uros.platise@isotel.eu> +# Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +# TODO, this makefile should run make under the app dirs, instead of +# sourcing the Make.defs! + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +ifeq ($(WINTOOL),y) +INCDIROPT = -w +endif + +# Hello Application +# TODO: appname can be automatically extracted from the directory name + +APPNAME = install +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 1024 + +ASRCS = +CSRCS = install.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: context depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +# Register application + +.context: + $(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main) + @touch $@ + +context: .context + +# Create dependencies + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f .context Make.dep .depend + +-include Make.dep diff --git a/apps/system/install/README.txt b/apps/system/install/README.txt new file mode 100755 index 000000000..66e9b0939 --- /dev/null +++ b/apps/system/install/README.txt @@ -0,0 +1,26 @@ + +Install Program +=============== + + Source: NuttX + Author: Uros Platise + Date: 7. May 2011 + +This application installs XIP application by placing it directly into +the program memory (flash) area into free area and creates a start-up +script into selected directory (i.e. /usr/bin/progname). + +Usage: + install [--stack RequiredStackSpace] [--priority Priority] + source-file destination-directory + +If stackspace is not given default stack space of 4096 Bytes is used. +If priority is not given system default is used. + +Additional options: + + --remove destination-file i.e. install --remove /usr/bin/myapp + --force to replace existing installation + --start <page> install app at or after <page> + --margin <pages> leave some free space after the kernel + Default is 16 pages so kernel may grow. diff --git a/apps/system/install/install.c b/apps/system/install/install.c new file mode 100755 index 000000000..2f11c6434 --- /dev/null +++ b/apps/system/install/install.c @@ -0,0 +1,412 @@ +/**************************************************************************** + * apps/system/install/install.c + * + * Copyright (C) 2011 Uros Platise. All rights reserved. + * Author: Uros Platise <uros.platise@isotel.eu> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/progmem.h> +#include <sys/stat.h> + +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <errno.h> + + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define ACTION_INSTALL 0x01 +#define ACTION_REMOVE 0x00 +#define ACTION_REINSTALL 0x03 +#define ACTION_INSUFPARAM 0x80 + +#define INSTALL_PROGRAMBLOCKSIZE 1024 + + +/**************************************************************************** + * Private data + ****************************************************************************/ + +const char *install_help = + "Installs XIP program into flash and creates a start-up script in the\n" + "destination directory.\n\n" + "Usage:\t%s [options] source-file.xip destination-directory\n\n" + "Example:\n\t%s --stack 1024 /sdcard/demo.xip /usr/bin\n\n" + "Options:\n" + "\t--stack <required_stack_space>\n" + "\t--priority <priority>\n" + "\t--remove <dest-file>\tRemoves installed application\n" + "\t--force\t\t\tReplaces existing installation\n" + "\t--start <page>\t\tInstalls application at or after <page>\n" + "\t--margin <pages>\tLeave some free space after the kernel (default 16)\n"; + +const char *install_script_text = + "# XIP stacksize=%x priority=%x size=%x\n"; + +const char *install_script_exec = + "exec 0x%x\n"; + + +/**************************************************************************** + * Private functions + ****************************************************************************/ + +int install_getstartpage(int startpage, int pagemargin, int desiredsize) +{ + uint16_t page = 0, stpage = 0xFFFF; + uint16_t pagesize = 0; + int maxlen = -1; + int maxlen_start = 0xFFFF; + int status; + + for (status=0, page=0; status >= 0; page++) { + + status = up_progmem_ispageerased(page); + pagesize = up_progmem_pagesize(page); + + /* Is this beginning of new free space section */ + if (status == 0) { + if (stpage == 0xFFFF) stpage = page; + } + else if (status != 0) { + + if (stpage != 0xFFFF) { + + if ( (page - stpage) > maxlen) { + + if (maxlen==-1) { /* First time found sth? */ + stpage += pagemargin; + maxlen = 0; + } + + if(stpage < startpage) + stpage = startpage; + + if (page > stpage) { + maxlen = page - stpage; + maxlen_start = stpage; + } + + if (maxlen*pagesize >= desiredsize) { + /* printf("Found page at %d ... %d\n", stpage, page); */ + return maxlen_start*pagesize; + } + } + + stpage = 0xFFFF; + } + } + } + + /* Requested space is not available */ + + return -1; +} + + +int install_programflash(int startaddr, const char *source) +{ + int status; + int count; + int totalsize = 0; + char *buf; + FILE *fp; + + if ( (buf = malloc(INSTALL_PROGRAMBLOCKSIZE)) == NULL ) + return -errno; + + if ( (fp=fopen(source, "r")) ) { + do { + count = fread(buf, 1, INSTALL_PROGRAMBLOCKSIZE, fp); + + if ( (status = up_progmem_write(startaddr, buf, count)) < 0) { + totalsize = status; + break; + } + + startaddr += count; + totalsize += count; + } + while(count); + } + else totalsize = -errno; + + fclose(fp); + free(buf); + + return totalsize; +} + + +void install_getscriptname(char *scriptname, const char *progname, const char *destdir) +{ + const char * progonly; + + /* I.e. as /usr/bin */ + strcpy(scriptname, destdir); + + /* extract from i.e. /sdcard/demo -> /demo, together with / */ + progonly = strrchr(progname, '/'); + strcat(scriptname, progonly); +} + + +int install_getprogsize(const char *progname) +{ + struct stat fileinfo; + + if ( stat(progname, &fileinfo) < 0 ) + return -1; + + return fileinfo.st_size; +} + + +int install_alreadyexists(const char *scriptname) +{ + FILE *fp; + + if ( (fp=fopen(scriptname, "r"))==NULL ) + return 0; + + fclose(fp); + return 1; +} + + +int install_createscript(int addr, int stacksize, int progsize, + int priority, const char *scriptname) +{ + FILE *fp; + + if ( (fp=fopen(scriptname, "w+"))==NULL ) + return -errno; + + fprintf(fp, install_script_text, stacksize, priority, progsize); + fprintf(fp, install_script_exec, addr); + + fflush(fp); + fclose(fp); + + return 0; +} + + +int install_getlasthexvalue(FILE *fp, char delimiter) +{ + char buf[128]; + char *p; + + if (fgets(buf, 127, fp)) { + if ( (p = strrchr(buf, delimiter)) ) { + return strtol(p+1, NULL, 16); + } + } + return -1; +} + + +int install_remove(const char *scriptname) +{ + FILE *fp; + int progsize, addr, freedsize; + uint16_t page; + int status = 0; + + /* Parse script */ + + if ( (fp=fopen(scriptname, "r")) ) { + progsize = install_getlasthexvalue(fp,'='); + addr = install_getlasthexvalue(fp,' '); + freedsize = progsize; + } + else return -errno; + + fclose(fp); + + /* Remove pages */ + + if (progsize <= 0 || addr <= 0) + return -EIO; + + do { + if ((page = up_progmem_getpage(addr)) < 0) { + status = -page; + break; + } + + if ( up_progmem_erasepage(page) < 0) { + status = -page; + break; + } + + addr += up_progmem_pagesize(page); + progsize -= up_progmem_pagesize(page); + + } while(progsize > 0); + + if (status < 0) return status; + + /* Remove script file */ + + if (unlink(scriptname) < 0) return -errno; + + return freedsize; +} + + +/**************************************************************************** + * Start + ****************************************************************************/ + +int install_main(int argc, char *argv[]) +{ + int i; + int progsize; + int scrsta; + int stacksize = 4096; + int priority = SCHED_PRIORITY_DEFAULT; + int pagemargin = 16; + int startpage = 0; + int startaddr = 0; + int action = ACTION_INSTALL; + char scriptname[128]; + + /* Supported? */ + + if ( !up_progmem_isuniform() ) { + fprintf(stderr, "Error: install supports uniform organization only.\n"); + return -1; + } + + /* Parse arguments */ + + for (i=1; i<argc; i++) { + if (argv[i][0]=='-' && argv[i][1]=='-' && i<=argc) { + + if (strcmp(argv[i]+2, "stack")==0) { + stacksize = atoi(argv[++i]); + } + else if (strcmp(argv[i]+2, "priority")==0) { + priority = atoi(argv[++i]); + } + else if (strcmp(argv[i]+2, "start")==0) { + startpage = atoi(argv[++i]); + } + else if (strcmp(argv[i]+2, "margin")==0) { + pagemargin = atoi(argv[++i]); + } + else if (strcmp(argv[i]+2, "remove")==0) { + action = ACTION_REMOVE; + } + else if (strcmp(argv[i]+2, "force")==0) { + action = ACTION_REINSTALL; + } + else fprintf(stderr, "Unknown option: %s\n", argv[i]); + } + else break; + } + + /* Do the job */ + + switch(action & 1) { + + case ACTION_REMOVE: + if (i > argc-1) { + action = ACTION_INSUFPARAM; + break; /* are there sufficient parameters */ + } + if ( (scrsta=install_remove(argv[i])) < 0) { + fprintf(stderr, "Could not remove program: %s\n", strerror(-scrsta) ); + return -1; + } + printf("Removed %s and freed %d bytes\n", argv[i], scrsta); + return 0; + + + case ACTION_INSTALL: + if (i > argc-2) { + action = ACTION_INSUFPARAM; + break; /* are there sufficient parameters */ + } + + install_getscriptname(scriptname, argv[i], argv[i+1]); + + // script-exists? + if (install_alreadyexists(scriptname)==1) { + + if (action != ACTION_REINSTALL) { + fprintf(stderr, "Program with that name already exists.\n"); + return -EEXIST; + } + + if ( (scrsta=install_remove(scriptname)) < 0) { + fprintf(stderr, "Could not remove program: %s\n", strerror(-scrsta) ); + return -1; + } + + printf("Replacing %s\n", scriptname); + } + + startaddr = install_getstartpage(startpage, pagemargin, install_getprogsize(argv[i]) ); + if (startpage < 0) { + fprintf(stderr, "Not enough memory\n"); + return -ENOMEM; + } + + if ( (progsize = install_programflash(startaddr, argv[i])) <= 0) { + fprintf(stderr, "Error writing program memory: %s\n" + "Note: Flash pages are not released, so you may try again and program will be\n" + " written in other pages.\n", strerror(-progsize) ); + + return -EIO; + } + if ( (scrsta = install_createscript(startaddr, stacksize, progsize, + priority, scriptname)) < 0) { + fprintf(stderr, "Error writing program script at %s: %s\n", + argv[i+1], strerror(-scrsta) ); + return -EIO; + } + + printf("Installed application of size %d bytes to program memory [%xh - %xh].\n", + progsize, startaddr, startaddr + progsize); + + return 0; + } + + fprintf(stderr, install_help, argv[0], argv[0]); + return -1; +} diff --git a/apps/system/readline/Kconfig b/apps/system/readline/Kconfig new file mode 100644 index 000000000..2ed9cafd6 --- /dev/null +++ b/apps/system/readline/Kconfig @@ -0,0 +1,39 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +config SYSTEM_READLINE + bool "readline() support" + default n + ---help--- + Enable support for the readline() function. + +if SYSTEM_READLINE +config READLINE_ECHO + bool "Echo character input" + default y + ---help--- + Echo received character input back via stdout. This is normal + behavior and should be selected unless the source of stdin input + already has local echo support or you need to suppress the back-channel + responses for any other reason. + +choice + prompt "Newline Options" + default EOL_IS_EITHER_CRLF + +config EOL_IS_CR + bool "EOL is CR" + +config EOL_IS_LF + bool "EOL is LF" + +config EOL_IS_BOTH_CRLF + bool "EOL is CR and LF" + +config EOL_IS_EITHER_CRLF + bool "EOL is CR or LF" + +endchoice +endif diff --git a/apps/system/readline/Makefile b/apps/system/readline/Makefile new file mode 100644 index 000000000..34fab7e81 --- /dev/null +++ b/apps/system/readline/Makefile @@ -0,0 +1,103 @@ +############################################################################ +# apps/system/readline/Makefile +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/.config +-include $(TOPDIR)/Make.defs +include $(APPDIR)/Make.defs + +ifeq ($(WINTOOL),y) +INCDIROPT = -w +endif + +# The Readline Library + +ASRCS = +CSRCS = readline.c + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ifeq ($(WINTOOL),y) + BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}" +else + BIN = "$(APPDIR)/libapps$(LIBEXT)" +endif + +ROOTDEPPATH = --dep-path . + +# Common build + +VPATH = + +all: .built +.PHONY: context depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +.built: $(OBJS) + @( for obj in $(OBJS) ; do \ + $(call ARCHIVE, $(BIN), $${obj}); \ + done ; ) + @touch .built + +# Context build phase target + +context: + +# Dependency build phase target + +.depend: Makefile $(SRCS) + @$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +# Housekeeping targets + +clean: + @rm -f *.o *~ .*.swp .built + $(call CLEAN) + +distclean: clean + @rm -f .context Make.dep .depend + +-include Make.dep diff --git a/apps/system/readline/readline.c b/apps/system/readline/readline.c new file mode 100644 index 000000000..f7fa6a635 --- /dev/null +++ b/apps/system/readline/readline.c @@ -0,0 +1,345 @@ +/**************************************************************************** + * lib/stdio/lib_fgets.c + * + * Copyright (C) 2007-2008, 2011-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <ctype.h> +#include <errno.h> +#include <debug.h> + +#include <nuttx/ascii.h> +#include <nuttx/vt100.h> + +#include <apps/readline.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* In some systems, the underlying serial logic may automatically echo + * characters back to the console. We will assume that that is not the case + & here + */ + +#define CONFIG_READLINE_ECHO 1 + +/* Some environments may return CR as end-of-line, others LF, and others + * both. The logic here assumes either but not both. + */ + +#undef CONFIG_EOL_IS_CR +#undef CONFIG_EOL_IS_LF +#undef CONFIG_EOL_IS_BOTH_CRLF +#define CONFIG_EOL_IS_EITHER_CRLF 1 + +/**************************************************************************** + * Private Type Declarations + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ +/* <esc>[K is the VT100 command erases to the end of the line. */ + +static const char g_erasetoeol[] = VT100_CLEAREOL; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: readline_rawgetc + ****************************************************************************/ + +static inline int readline_rawgetc(int infd) +{ + char buffer; + ssize_t nread; + + nread = read(infd, &buffer, 1); + if (nread < 1) + { + /* Return EOF if the end of file (0) or error (-1) occurs */ + + return EOF; + } + return (int)buffer; +} + +/**************************************************************************** + * Name: readline_consoleputc + ****************************************************************************/ + +#ifdef CONFIG_READLINE_ECHO +static inline void readline_consoleputc(int ch, int outfd) +{ + char buffer = ch; + (void)write(outfd, &buffer, 1); +} +#endif + +/**************************************************************************** + * Name: readline_consolewrite + ****************************************************************************/ + +#ifdef CONFIG_READLINE_ECHO +static inline void readline_consolewrite(int outfd, FAR const char *buffer, size_t buflen) +{ + (void)write(outfd, buffer, buflen); +} +#endif + +/**************************************************************************** + * Global Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: readline + * + * readline() reads in at most one less than 'buflen' characters from + * 'instream' and stores them into the buffer pointed to by 'buf'. + * Characters are echoed on 'outstream'. Reading stops after an EOF or a + * newline. If a newline is read, it is stored into the buffer. A null + * terminator is stored after the last character in the buffer. + * + * This version of realine assumes that we are reading and writing to + * a VT100 console. This will not work well if 'instream' or 'outstream' + * corresponds to a raw byte steam. + * + * This function is inspired by the GNU readline but is an entirely + * different creature. + * + * Input Parameters: + * buf - The user allocated buffer to be filled. + * buflen - the size of the buffer. + * instream - The stream to read characters from + * outstream - The stream to each characters to. + * + * Returned values: + * On success, the (positive) number of bytes transferred is returned. + * A length of zero would indicate an end of file condition. On failure, + * a negated errno value is returned. + * + **************************************************************************/ + +ssize_t readline(FAR char *buf, int buflen, FILE *instream, FILE *outstream) +{ + int infd; + int outfd; + int escape; + int nch; + + /* Sanity checks */ + + if (!instream || !outstream || !buf || buflen < 1) + { + return -EINVAL; + } + + if (buflen < 2) + { + *buf = '\0'; + return 0; + } + + /* Extract the file descriptions. This is cheating (and horribly non- + * standard) + */ + + infd = instream->fs_filedes; + outfd = outstream->fs_filedes; + + /* <esc>[K is the VT100 command that erases to the end of the line. */ + +#ifdef CONFIG_READLINE_ECHO + readline_consolewrite(outfd, g_erasetoeol, sizeof(g_erasetoeol)); +#endif + + /* Read characters until we have a full line. On each the loop we must + * be assured that there are two free bytes in the line buffer: One for + * the next character and one for the null terminator. + */ + + escape = 0; + nch = 0; + + for(;;) + { + /* Get the next character */ + + int ch = readline_rawgetc(infd); + + /* Are we processing a VT100 escape sequence */ + + if (escape) + { + /* Yes, is it an <esc>[, 3 byte sequence */ + + if (ch != ASCII_LBRACKET || escape == 2) + { + /* We are finished with the escape sequence */ + + escape = 0; + ch = 'a'; + } + else + { + /* The next character is the end of a 3-byte sequence. + * NOTE: Some of the <esc>[ sequences are longer than + * 3-bytes, but I have not encountered any in normal use + * yet and, so, have not provided the decoding logic. + */ + + escape = 2; + } + } + + /* Check for backspace + * + * There are several notions of backspace, for an elaborate summary see + * http://www.ibb.net/~anne/keyboard.html. There is no clean solution. + * Here both DEL and backspace are treated like backspace here. The + * Unix/Linux screen terminal by default outputs DEL (0x7f) when the + * backspace key is pressed. + */ + + else if (ch == ASCII_BS || ch == ASCII_DEL) + { + /* Eliminate that last character in the buffer. */ + + if (nch > 0) + { + nch--; + +#ifdef CONFIG_READLINE_ECHO + /* Echo the backspace character on the console. Always output + * the backspace character because the VT100 terminal doesn't + * understand DEL properly. + */ + + readline_consoleputc(ASCII_BS, outfd); + readline_consolewrite(outfd, g_erasetoeol, sizeof(g_erasetoeol)); +#endif + } + } + + /* Check for the beginning of a VT100 escape sequence */ + + else if (ch == ASCII_ESC) + { + /* The next character is escaped */ + + escape = 1; + } + + /* Check for end-of-line. This is tricky only in that some + * environments may return CR as end-of-line, others LF, and + * others both. + */ + +#if defined(CONFIG_EOL_IS_LF) || defined(CONFIG_EOL_IS_BOTH_CRLF) + else if (ch == '\n') +#elif defined(CONFIG_EOL_IS_CR) + else if (ch == '\r') +#elif CONFIG_EOL_IS_EITHER_CRLF + else if (ch == '\n' || ch == '\r') +#endif + { + /* The newline is stored in the buffer along with the null + * terminator. + */ + + buf[nch++] = '\n'; + buf[nch] = '\0'; + +#ifdef CONFIG_READLINE_ECHO + /* Echo the newline to the console */ + + readline_consoleputc('\n', outfd); +#endif + return nch; + } + + /* Check for end-of-file */ + + else if (ch == EOF) + { + /* Terminate the line (which might be zero length) */ + + buf[nch] = '\0'; + return nch; + } + + /* Otherwise, check if the character is printable and, if so, put the + * character in the line buffer + */ + + else if (isprint(ch)) + { + buf[nch++] = ch; + +#ifdef CONFIG_READLINE_ECHO + /* Echo the character to the console */ + + readline_consoleputc(ch, outfd); +#endif + /* Check if there is room for another character and the line's + * null terminator. If not then we have to end the line now. + */ + + if (nch + 1 >= buflen) + { + buf[nch] = '\0'; + return nch; + } + } + } +} + diff --git a/apps/systemcmds/boardinfo/.context b/apps/systemcmds/boardinfo/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/systemcmds/boardinfo/.context diff --git a/apps/systemcmds/boardinfo/Makefile b/apps/systemcmds/boardinfo/Makefile new file mode 100644 index 000000000..753a6843e --- /dev/null +++ b/apps/systemcmds/boardinfo/Makefile @@ -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. +# +############################################################################ + +# +# Build the boardinfo tool. +# + +APPNAME = boardinfo +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c new file mode 100644 index 000000000..61a6ea221 --- /dev/null +++ b/apps/systemcmds/boardinfo/boardinfo.c @@ -0,0 +1,265 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file autopilot and carrier board information app */ + + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> + +#include "systemlib/systemlib.h" + +__EXPORT int boardinfo_main(int argc, char *argv[]); + +/* + * Reads out the board information + * + * @param argc the number of string arguments (including the executable name) + * @param argv the argument strings + * + * @return 0 on success, 1 on error + */ +int boardinfo_main(int argc, char *argv[]) +{ + const char *commandline_usage = "\tusage: boardinfo [-c|-f] [-t id] [-w \"<info>\"]\n"; + + bool carrier_mode = false; + bool fmu_mode = false; + bool write_mode = false; + char *write_string = 0; + bool silent = false; + bool test_enabled = false; + int test_boardid = -1; + int ch; + + while ((ch = getopt(argc, argv, "cft:w:v")) != -1) { + switch (ch) { + case 'c': + carrier_mode = true; + break; + + case 'f': + fmu_mode = true; + break; + + case 't': + test_enabled = true; + test_boardid = strtol(optarg, NULL, 10); + silent = true; + break; + + case 'w': + write_mode = true; + write_string = optarg; + break; + + default: + printf(commandline_usage); + exit(0); + } + } + + /* Check if write is required - only one mode is allowed then */ + if (write_mode && fmu_mode && carrier_mode) { + fprintf(stderr, "[boardinfo] Please choose only one mode for writing: --carrier or --fmu\n"); + printf(commandline_usage); + return ERROR; + } + + /* Write FMU information + if (fmu_mode && write_mode) { + struct fmu_board_info_s info; + int ret = fmu_store_board_info(&info); + + + if (ret == OK) { + printf("[boardinfo] Successfully wrote FMU board info\n"); + } else { + fprintf(stderr, "[boardinfo] ERROR writing board info to FMU EEPROM, aborting\n"); + return ERROR; + } + }*/ + + /* write carrier board info */ + if (carrier_mode && write_mode) { + + struct carrier_board_info_s info; + bool parse_ok = true; + unsigned parse_idx = 0; + //int maxlen = strlen(write_string); + char *curr_char; + + /* Parse board info string */ + if (write_string[0] != 'P' || write_string[1] != 'X' || write_string[2] != '4') { + fprintf(stderr, "[boardinfo] header must start with 'PX4'\n"); + parse_ok = false; + } + + info.header[0] = 'P'; info.header[1] = 'X'; info.header[2] = '4'; + parse_idx = 3; + /* Copy board name */ + + int i = 0; + + while (write_string[parse_idx] != 0x20 && (parse_idx < sizeof(info.board_name) + sizeof(info.header))) { + info.board_name[i] = write_string[parse_idx]; + i++; parse_idx++; + } + + /* Enforce null termination */ + info.board_name[sizeof(info.board_name) - 1] = '\0'; + + curr_char = write_string + parse_idx; + + /* Index is now on next field */ + info.board_id = strtol(curr_char, &curr_char, 10);//atoi(write_string + parse_index); + info.board_version = strtol(curr_char, &curr_char, 10); + + /* Read in multi ports */ + for (i = 0; i < MULT_COUNT; i++) { + info.multi_port_config[i] = strtol(curr_char, &curr_char, 10); + } + + /* Read in production data */ + info.production_year = strtol(curr_char, &curr_char, 10); + + if (info.production_year < 2012 || info.production_year > 3000) { + fprintf(stderr, "[boardinfo] production year is invalid: %d\n", info.production_year); + parse_ok = false; + } + + info.production_month = strtol(curr_char, &curr_char, 10); + + if (info.production_month < 1 || info.production_month > 12) { + fprintf(stderr, "[boardinfo] production month is invalid: %d\n", info.production_month); + parse_ok = false; + } + + info.production_day = strtol(curr_char, &curr_char, 10); + + if (info.production_day < 1 || info.production_day > 31) { + fprintf(stderr, "[boardinfo] production day is invalid: %d\n", info.production_day); + parse_ok = false; + } + + info.production_fab = strtol(curr_char, &curr_char, 10); + info.production_tester = strtol(curr_char, &curr_char, 10); + + if (!parse_ok) { + /* Parsing failed */ + fprintf(stderr, "[boardinfo] failed parsing info string:\n\t%s\naborting\n", write_string); + return ERROR; + + } else { + int ret = carrier_store_board_info(&info); + + /* Display result */ + if (ret == sizeof(info)) { + printf("[boardinfo] Successfully wrote carrier board info\n"); + + } else { + fprintf(stderr, "[boardinfo] ERROR writing board info to carrier EEPROM (forgot to pull the WRITE_ENABLE line high?), aborting\n"); + return ERROR; + } + } + } + + /* Print FMU information */ + if (fmu_mode && !silent) { + struct fmu_board_info_s info; + int ret = fmu_get_board_info(&info); + + /* Print human readable name */ + if (ret == sizeof(info)) { + printf("[boardinfo] Autopilot:\n\t%s\n", info.header); + + } else { + fprintf(stderr, "[boardinfo] ERROR loading board info from FMU, aborting\n"); + return ERROR; + } + } + + /* print carrier information */ + if (carrier_mode && !silent) { + + struct carrier_board_info_s info; + int ret = carrier_get_board_info(&info); + + /* Print human readable name */ + if (ret == sizeof(info)) { + printf("[boardinfo] Carrier board:\n\t%s\n", info.header); + printf("\tboard id:\t\t%d\n", info.board_id); + printf("\tversion:\t\t%d\n", info.board_version); + + for (unsigned i = 0; i < MULT_COUNT; i++) { + printf("\tmulti port #%d:\t\t%s: function #%d\n", i, multiport_info.port_names[i], info.multi_port_config[i]); + } + + printf("\tproduction date:\t%d-%d-%d (fab #%d / tester #%d)\n", info.production_year, info.production_month, info.production_day, info.production_fab, info.production_tester); + + } else { + fprintf(stderr, "[boardinfo] ERROR loading board info from carrier EEPROM (errno #%d), aborting\n", -ret); + return ERROR; + } + } + + /* test for a specific carrier */ + if (test_enabled) { + + struct carrier_board_info_s info; + int ret = carrier_get_board_info(&info); + + if (ret != sizeof(info)) { + fprintf(stderr, "[boardinfo] ERROR loading board info from EEPROM (errno #%d), aborting\n", -ret); + exit(1); + } + + if (info.board_id == test_boardid) { + printf("[boardinfo] Found carrier board with ID %d, test succeeded\n", info.board_id); + exit(0); + + } else { + /* exit silently with an error so we can test for multiple boards quietly */ + exit(1); + } + } + + return 0; +} + + diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile new file mode 100644 index 000000000..aa1aa7761 --- /dev/null +++ b/apps/systemcmds/calibration/Makefile @@ -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. +# +############################################################################ + +# +# Makefile to build the calibration tool +# + +APPNAME = calibration +PRIORITY = SCHED_PRIORITY_MAX - 1 +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/calibration/calibration.c b/apps/systemcmds/calibration/calibration.c new file mode 100755 index 000000000..7f8b9240f --- /dev/null +++ b/apps/systemcmds/calibration/calibration.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * calibration.c + * + * Copyright (C) 2012 Ivan Ovinnikov. All rights reserved. + * Authors: Nils Wenzler <wenzlern@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include "calibration.h" + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int calibrate_help(int argc, char *argv[]); +static int calibrate_all(int argc, char *argv[]); + +__EXPORT int calibration_main(int argc, char *argv[]); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct { + const char *name; + int (* fn)(int argc, char *argv[]); + unsigned options; +#define OPT_NOHELP (1<<0) +#define OPT_NOALLTEST (1<<1) +} calibrates[] = { + {"range", range_cal, 0}, + {"servo", servo_cal, 0}, + {"all", calibrate_all, OPT_NOALLTEST}, + {"help", calibrate_help, OPT_NOALLTEST | OPT_NOHELP}, + {NULL, NULL} +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int +calibrate_help(int argc, char *argv[]) +{ + unsigned i; + + printf("Available calibration routines:\n"); + + for (i = 0; calibrates[i].name; i++) + printf(" %s\n", calibrates[i].name); + + return 0; +} + +static int +calibrate_all(int argc, char *argv[]) +{ + unsigned i; + char *args[2] = {"all", NULL}; + + printf("Running all calibration routines...\n\n"); + + for (i = 0; calibrates[i].name; i++) { + printf(" %s:\n", calibrates[i].name); + + if (calibrates[i].fn(1, args)) { + printf(" FAIL\n"); + + } else { + printf(" DONE\n"); + } + } + + return 0; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void press_enter(void) +{ + int c; + printf("Press CTRL+ENTER to continue... \n"); + fflush(stdout); + + do c = getchar(); while ((c != '\n') && (c != EOF)); +} + +/**************************************************************************** + * Name: calibrate_main + ****************************************************************************/ + +int calibration_main(int argc, char *argv[]) +{ + unsigned i; + + if (argc < 2) { + printf("calibration: missing name - 'calibration help' for a list of routines\n"); + return 1; + } + + for (i = 0; calibrates[i].name; i++) { + if (!strcmp(calibrates[i].name, argv[1])) + return calibrates[i].fn(argc - 1, argv + 1); + } + + printf("calibrate: no routines called '%s' - 'calibration help' for a list of routines\n", argv[1]); + return 1; +} diff --git a/apps/systemcmds/calibration/calibration.h b/apps/systemcmds/calibration/calibration.h new file mode 100644 index 000000000..028853ec8 --- /dev/null +++ b/apps/systemcmds/calibration/calibration.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef _CALIBRATION_H +#define _CALIBRATION_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +extern int range_cal(int argc, char *argv[]); +extern int servo_cal(int argc, char *argv[]); + +#endif diff --git a/apps/systemcmds/calibration/channels_cal.c b/apps/systemcmds/calibration/channels_cal.c new file mode 100755 index 000000000..575138b12 --- /dev/null +++ b/apps/systemcmds/calibration/channels_cal.c @@ -0,0 +1,196 @@ +/**************************************************************************** + * channels_cal.c + * + * Copyright (C) 2012 Nils Wenzler. All rights reserved. + * Authors: Nils Wenzler <wenzlern@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include "calibration.h" + +/**************************************************************************** + * Defines + ****************************************************************************/ +#define ABS(a) (((a) < 0) ? -(a) : (a)) +#define MIN(a,b) (((a)<(b))?(a):(b)) +#define MAX(a,b) (((a)>(b))?(a):(b)) + +/**************************************************************************** + * Private Data + ****************************************************************************/ +//Store the values here before writing them to global_data_rc_channels +uint16_t old_values[NR_CHANNELS]; +uint16_t cur_values[NR_CHANNELS]; +uint8_t function_map[NR_CHANNELS]; +char names[12][9]; + + + +/**************************************************************************** + * Private Functions + ****************************************************************************/ +void press_enter_2(void) +{ + int c; + printf("Press CTRL+ENTER to continue... \n"); + fflush(stdout); + + do c = getchar(); while ((c != '\n') && (c != EOF)); +} + +/**This gets all current values and writes them to min or max + */ +uint8_t get_val(uint16_t *buffer) +{ + if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { + uint8_t i; + + for (i = 0; i < NR_CHANNELS; i++) { + printf("Channel: %i\t RAW Value: %i: \n", i, global_data_rc_channels->chan[i].raw); + buffer[i] = global_data_rc_channels->chan[i].raw; + } + + global_data_unlock(&global_data_rc_channels->access_conf); + return 0; + + } else + return -1; +} + +void set_channel(void) +{ + static uint8_t j = 0; + uint8_t i; + + for (i = 0; i < NR_CHANNELS; i++) { + if (ABS(old_values - cur_values) > 50) { + function_map[j] = i; + strcpy(names[i], global_data_rc_channels->function_names[j]); + j++; + } + } +} + +void write_dat(void) +{ + global_data_lock(&global_data_rc_channels->access_conf); + uint8_t i; + + for (i = 0; i < NR_CHANNELS; i++) { + global_data_rc_channels->function[i] = function_map[i]; + strcpy(global_data_rc_channels->chan[i].name, names[i]); + + printf("Channel %i\t Function %s\n", i, + global_data_rc_channels->chan[i].name); + } + + global_data_unlock(&global_data_rc_channels->access_conf); +} + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +int channels_cal(int argc, char *argv[]) +{ + + printf("This routine maps the input functions on the remote control\n"); + printf("to the corresponding function (Throttle, Roll,..)\n"); + printf("Always move the stick all the way\n"); + press_enter_2(); + + printf("Pull the THROTTLE stick down\n"); + press_enter_2(); + + while (get_val(old_values)); + + printf("Move the THROTTLE stick up\n "); + press_enter_2(); + + while (get_val(cur_values)); + + set_channel(); + + printf("Pull the PITCH stick down\n"); + press_enter_2(); + + while (get_val(old_values)); + + printf("Move the PITCH stick up\n "); + press_enter_2(); + + while (get_val(cur_values)); + + set_channel(); + + printf("Put the ROLL stick to the left\n"); + press_enter_2(); + + while (get_val(old_values)); + + printf("Put the ROLL stick to the right\n "); + press_enter_2(); + + while (get_val(cur_values)); + + set_channel(); + + printf("Put the YAW stick to the left\n"); + press_enter_2(); + + while (get_val(old_values)); + + printf("Put the YAW stick to the right\n "); + press_enter_2(); + + while (get_val(cur_values)); + + set_channel(); + + uint8_t k; + + for (k = 5; k < NR_CHANNELS; k++) { + function_map[k] = k; + strcpy(names[k], global_data_rc_channels->function_names[k]); + } + +//write the values to global_data_rc_channels + write_dat(); + + return 0; + +} + diff --git a/apps/systemcmds/calibration/range_cal.c b/apps/systemcmds/calibration/range_cal.c new file mode 100755 index 000000000..159a0d06b --- /dev/null +++ b/apps/systemcmds/calibration/range_cal.c @@ -0,0 +1,224 @@ +/**************************************************************************** + * range_cal.c + * + * Copyright (C) 2012 Nils Wenzler. All rights reserved. + * Authors: Nils Wenzler <wenzlern@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ +#include <stdio.h> +#include <stdlib.h> +#include "calibration.h" + +/**************************************************************************** + * Defines + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ +//Store the values here before writing them to global_data_rc_channels +uint16_t max_values[NR_CHANNELS]; +uint16_t min_values[NR_CHANNELS]; +uint16_t mid_values[NR_CHANNELS]; + + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**This sets the middle values + */ +uint8_t set_mid(void) +{ + if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { + uint8_t i; + + for (i = 0; i < NR_CHANNELS; i++) { + if (i == global_data_rc_channels->function[ROLL] || + i == global_data_rc_channels->function[YAW] || + i == global_data_rc_channels->function[PITCH]) { + + mid_values[i] = global_data_rc_channels->chan[i].raw; + + } else { + mid_values[i] = (max_values[i] + min_values[i]) / 2; + } + + } + + global_data_unlock(&global_data_rc_channels->access_conf); + return 0; + + } else + return -1; +} + +/**This gets all current values and writes them to min or max + */ +uint8_t get_value(void) +{ + if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { + uint8_t i; + + for (i = 0; i < NR_CHANNELS; i++) { + //see if the value is bigger or smaller than 1500 (roughly neutral) + //and write to the appropriate array + if (global_data_rc_channels->chan[i].raw != 0 && + global_data_rc_channels->chan[i].raw < 1500) { + min_values[i] = global_data_rc_channels->chan[i].raw; + + } else if (global_data_rc_channels->chan[i].raw != 0 && + global_data_rc_channels->chan[i].raw > 1500) { + max_values[i] = global_data_rc_channels->chan[i].raw; + + } else { + max_values[i] = 0; + min_values[i] = 0; + } + } + + global_data_unlock(&global_data_rc_channels->access_conf); + return 0; + + } else + return -1; +} + + +void write_data(void) +{ + // global_data_lock(&global_data_rc_channels->access_conf); + // uint8_t i; + // for(i=0; i < NR_CHANNELS; i++){ + // //Write the data to global_data_rc_channels (if not 0) + // if(mid_values[i]!=0 && min_values[i]!=0 && max_values[i]!=0){ + // global_data_rc_channels->chan[i].scaling_factor = + // 10000/((max_values[i] - min_values[i])/2); + // + // global_data_rc_channels->chan[i].mid = mid_values[i]; + // } + // + // printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", + // i, + // global_data_rc_channels->function_name[global_data_rc_channels->function[i]], + // min_values[i], max_values[i], + // global_data_rc_channels->chan[i].scaling_factor, + // global_data_rc_channels->chan[i].mid); + // } + // global_data_unlock(&global_data_rc_channels->access_conf); + + //Write to the Parameter storage + + global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN] = min_values[0]; + global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN] = min_values[1]; + global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN] = min_values[2]; + global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN] = min_values[3]; + global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN] = min_values[4]; + global_data_parameter_storage->pm.param_values[PARAM_RC6_MIN] = min_values[5]; + global_data_parameter_storage->pm.param_values[PARAM_RC7_MIN] = min_values[6]; + global_data_parameter_storage->pm.param_values[PARAM_RC8_MIN] = min_values[7]; + + + global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] = max_values[0]; + global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] = max_values[1]; + global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] = max_values[2]; + global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] = max_values[3]; + global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] = max_values[4]; + global_data_parameter_storage->pm.param_values[PARAM_RC6_MAX] = max_values[5]; + global_data_parameter_storage->pm.param_values[PARAM_RC7_MAX] = max_values[6]; + global_data_parameter_storage->pm.param_values[PARAM_RC8_MAX] = max_values[7]; + + + global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM] = mid_values[0]; + global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM] = mid_values[1]; + global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM] = mid_values[2]; + global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM] = mid_values[3]; + global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM] = mid_values[4]; + global_data_parameter_storage->pm.param_values[PARAM_RC6_TRIM] = mid_values[5]; + global_data_parameter_storage->pm.param_values[PARAM_RC7_TRIM] = mid_values[6]; + global_data_parameter_storage->pm.param_values[PARAM_RC8_TRIM] = mid_values[7]; + + usleep(3e6); + uint8_t i; + + for (i = 0; i < NR_CHANNELS; i++) { + printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", + i, + min_values[i], max_values[i], + global_data_rc_channels->chan[i].scaling_factor, + global_data_rc_channels->chan[i].mid); + } + + +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +int range_cal(int argc, char *argv[]) +{ + + printf("The range calibration routine assumes you already did the channel\n"); + printf("assignment\n"); + printf("This routine chooses the minimum, maximum and middle\n"); + printf("value for each channel separatly. The ROLL, PITCH and YAW function\n"); + printf("get their middle value from the RC direct, for the rest it is\n"); + printf("calculated out of the min and max values.\n"); + press_enter(); + + printf("Hold both sticks in lower left corner and continue\n "); + press_enter(); + usleep(500000); + + while (get_value()); + + printf("Hold both sticks in upper right corner and continue\n"); + press_enter(); + usleep(500000); + + while (get_value()); + + printf("Set the trim to 0 and leave the sticks in the neutral position\n"); + press_enter(); + + //Loop until successfull + while (set_mid()); + + //write the values to global_data_rc_channels + write_data(); + + return 0; + +} + diff --git a/apps/systemcmds/calibration/servo_cal.c b/apps/systemcmds/calibration/servo_cal.c new file mode 100644 index 000000000..96b3045a9 --- /dev/null +++ b/apps/systemcmds/calibration/servo_cal.c @@ -0,0 +1,264 @@ +/**************************************************************************** + * servo_cal.c + * + * Copyright (C) 2012 Nils Wenzler. All rights reserved. + * Authors: Nils Wenzler <wenzlern@ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ +#include <stdio.h> +#include <stdlib.h> +#include <arch/board/drv_pwm_servo.h> +#include <fcntl.h> +#include "calibration.h" + +/**************************************************************************** + * Defines + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ +//Store the values here before writing them to global_data_rc_channels +uint16_t max_values_servo[PWM_SERVO_MAX_CHANNELS]; +uint16_t min_values_servo[PWM_SERVO_MAX_CHANNELS]; +uint16_t mid_values_servo[PWM_SERVO_MAX_CHANNELS]; + +// Servo loop thread + +pthread_t servo_calib_thread; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**This sets the middle values + */ +uint8_t set_mid_s(void) +{ + if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { + uint8_t i; + + for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + if (i == global_data_rc_channels->function[ROLL] || + i == global_data_rc_channels->function[YAW] || + i == global_data_rc_channels->function[PITCH]) { + + mid_values_servo[i] = global_data_rc_channels->chan[i].raw; + + } else { + mid_values_servo[i] = (max_values_servo[i] + min_values_servo[i]) / 2; + } + + } + + global_data_unlock(&global_data_rc_channels->access_conf); + return 0; + + } else + return -1; +} + +/**This gets all current values and writes them to min or max + */ +uint8_t get_value_s(void) +{ + if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { + uint8_t i; + + for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + //see if the value is bigger or smaller than 1500 (roughly neutral) + //and write to the appropriate array + if (global_data_rc_channels->chan[i].raw != 0 && + global_data_rc_channels->chan[i].raw < 1500) { + min_values_servo[i] = global_data_rc_channels->chan[i].raw; + + } else if (global_data_rc_channels->chan[i].raw != 0 && + global_data_rc_channels->chan[i].raw > 1500) { + max_values_servo[i] = global_data_rc_channels->chan[i].raw; + + } else { + max_values_servo[i] = 0; + min_values_servo[i] = 0; + } + } + + global_data_unlock(&global_data_rc_channels->access_conf); + return 0; + + } else + return -1; +} + + +void write_data_s(void) +{ + // global_data_lock(&global_data_rc_channels->access_conf); + // uint8_t i; + // for(i=0; i < NR_CHANNELS; i++){ + // //Write the data to global_data_rc_channels (if not 0) + // if(mid_values_servo[i]!=0 && min_values_servo[i]!=0 && max_values_servo[i]!=0){ + // global_data_rc_channels->chan[i].scaling_factor = + // 10000/((max_values_servo[i] - min_values_servo[i])/2); + // + // global_data_rc_channels->chan[i].mid = mid_values_servo[i]; + // } + // + // printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", + // i, + // global_data_rc_channels->function_name[global_data_rc_channels->function[i]], + // min_values_servo[i], max_values_servo[i], + // global_data_rc_channels->chan[i].scaling_factor, + // global_data_rc_channels->chan[i].mid); + // } + // global_data_unlock(&global_data_rc_channels->access_conf); + + //Write to the Parameter storage + + + + global_data_lock(&global_data_parameter_storage->access_conf); + + global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MIN] = min_values_servo[0]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MIN] = min_values_servo[1]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MIN] = min_values_servo[2]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MIN] = min_values_servo[3]; + + global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MAX] = max_values_servo[0]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MAX] = max_values_servo[1]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MAX] = max_values_servo[2]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MAX] = max_values_servo[3]; + + global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM] = mid_values_servo[0]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM] = mid_values_servo[1]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM] = mid_values_servo[2]; + global_data_parameter_storage->pm.param_values[PARAM_SERVO4_TRIM] = mid_values_servo[3]; + + global_data_unlock(&global_data_parameter_storage->access_conf); + + usleep(3e6); + uint8_t i; + + for (i = 0; i < NR_CHANNELS; i++) { + printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n", + i, + min_values_servo[i], max_values_servo[i], + global_data_rc_channels->chan[i].scaling_factor, + global_data_rc_channels->chan[i].mid); + } + +} + +static void *servo_loop(void *arg) +{ + + // Set thread name + prctl(1, "calibration servo", getpid()); + + // initialize servos + int fd; + servo_position_t data[PWM_SERVO_MAX_CHANNELS]; + + fd = open("/dev/pwm_servo", O_RDWR); + + if (fd < 0) { + printf("failed opening /dev/pwm_servo\n"); + } + + ioctl(fd, PWM_SERVO_ARM, 0); + + while (1) { + int i; + + for (i = 0; i < 4; i++) { + data[i] = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].raw; + } + + int result = write(fd, &data, sizeof(data)); + + if (result != sizeof(data)) { + printf("failed bulk-reading channel values\n"); + } + + // 5Hz loop + usleep(200000); + } + + return NULL; +} + + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +int servo_cal(int argc, char *argv[]) +{ + // pthread_attr_t servo_loop_attr; + // pthread_attr_init(&servo_loop_attr); + // pthread_attr_setstacksize(&servo_loop_attr, 1024); + pthread_create(&servo_calib_thread, NULL, servo_loop, NULL); + pthread_join(servo_calib_thread, NULL); + + printf("The servo calibration routine assumes you already did the channel\n"); + printf("assignment with 'calibration channels'\n"); + printf("This routine chooses the minimum, maximum and middle\n"); + printf("value for each channel separately. The ROLL, PITCH and YAW function\n"); + printf("get their middle value from the RC direct, for the rest it is\n"); + printf("calculated out of the min and max values.\n"); + press_enter(); + + printf("Hold both sticks in lower left corner and continue\n "); + press_enter(); + usleep(500000); + + while (get_value_s()); + + printf("Hold both sticks in upper right corner and continue\n"); + press_enter(); + usleep(500000); + + while (get_value_s()); + + printf("Set the trim to 0 and leave the sticks in the neutral position\n"); + press_enter(); + + //Loop until successfull + while (set_mid_s()); + + //write the values to global_data_rc_channels + write_data_s(); + + return 0; + +} + diff --git a/apps/systemcmds/perf/.context b/apps/systemcmds/perf/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/systemcmds/perf/.context diff --git a/apps/systemcmds/perf/Makefile b/apps/systemcmds/perf/Makefile new file mode 100644 index 000000000..0134c9948 --- /dev/null +++ b/apps/systemcmds/perf/Makefile @@ -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. +# +############################################################################ + +# +# perf_counter reporting tool +# + +APPNAME = perf +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/perf/perf.c b/apps/systemcmds/perf/perf.c new file mode 100644 index 000000000..72406e9c7 --- /dev/null +++ b/apps/systemcmds/perf/perf.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2012 Lorenz Meier. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> + +#include "systemlib/perf_counter.h" + + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int perf_main(int argc, char *argv[]); + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int perf_main(int argc, char *argv[]) +{ + perf_print_all(); + fflush(stdout); + return 0; +} + + diff --git a/apps/systemcmds/reboot/.context b/apps/systemcmds/reboot/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/systemcmds/reboot/.context diff --git a/apps/systemcmds/reboot/Makefile b/apps/systemcmds/reboot/Makefile new file mode 100644 index 000000000..9609a24fd --- /dev/null +++ b/apps/systemcmds/reboot/Makefile @@ -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. +# +############################################################################ + +# +# Reboot command. +# + +APPNAME = reboot +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/reboot/reboot.c b/apps/systemcmds/reboot/reboot.c new file mode 100644 index 000000000..2a4cb18d0 --- /dev/null +++ b/apps/systemcmds/reboot/reboot.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * apps/reboot.c + * + * Copyright (C) 2012 Lorenz Meier. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> + +#include "systemlib/systemlib.h" + + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int reboot_main(int argc, char *argv[]); + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int reboot_main(int argc, char *argv[]) +{ + reboot(); + return 0; +} + + diff --git a/apps/systemcmds/top/.context b/apps/systemcmds/top/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/systemcmds/top/.context diff --git a/apps/systemcmds/top/Makefile b/apps/systemcmds/top/Makefile new file mode 100644 index 000000000..c45775f4b --- /dev/null +++ b/apps/systemcmds/top/Makefile @@ -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. +# +############################################################################ + +# +# realtime system performance display +# + +APPNAME = top +PRIORITY = SCHED_PRIORITY_DEFAULT - 10 +STACKSIZE = 3000 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/top/top.c b/apps/systemcmds/top/top.c new file mode 100644 index 000000000..af1bb23d9 --- /dev/null +++ b/apps/systemcmds/top/top.c @@ -0,0 +1,245 @@ +/**************************************************************************** + * apps/reboot.c + * + * Copyright (C) 2012 Lorenz Meier. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + + +#include <nuttx/config.h> +#include <stdio.h> +#include <fcntl.h> +#include <stdbool.h> +#include <unistd.h> +#include <string.h> +#include <poll.h> + +#include <arch/board/up_cpuload.h> +#include <arch/board/up_hrt.h> + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +__EXPORT int top_main(int argc, char *argv[]); + +/**************************************************************************** + * user_start + ****************************************************************************/ + +extern struct system_load_s system_load; + +bool top_sigusr1_rcvd = false; + +int top_main(int argc, char *argv[]) +{ + int t; + + uint64_t total_user_time = 0; + + int running_count = 0; + int blocked_count = 0; + + uint64_t new_time = hrt_absolute_time(); + uint64_t interval_start_time = new_time; + + uint64_t last_times[CONFIG_MAX_TASKS]; + float curr_loads[CONFIG_MAX_TASKS]; + + for (t = 0; t < CONFIG_MAX_TASKS; t++) + last_times[t] = 0; + + float interval_time_ms_inv = 0.f; + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + + while (true) +// for (t = 0; t < 10; t++) + { + int i; + + uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU); + unsigned int curr_time_s = curr_time_ms / 1000LLU; + + uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU); + unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU; + + if (new_time > interval_start_time) + interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); + + running_count = 0; + blocked_count = 0; + total_user_time = 0; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0; + + last_times[i] = system_load.tasks[i].total_runtime; + + if (system_load.tasks[i].valid && (new_time > interval_start_time)) { + curr_loads[i] = interval_runtime * interval_time_ms_inv; + + if (i > 0) + total_user_time += interval_runtime; + + } else + curr_loads[i] = 0; + } + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && (new_time > interval_start_time)) { + if (system_load.tasks[i].tcb->pid == 0) { + float idle = curr_loads[0]; + float task_load = (float)(total_user_time) * interval_time_ms_inv; + + if (task_load > (1.f - idle)) task_load = (1.f - idle); /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */ + + float sched_load = 1.f - idle - task_load; + + /* print system information */ + printf("\033[H"); /* cursor home */ + printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count); + printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100)); + printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000)); + + /* 34 chars command name length (32 chars plus two spaces) */ + char header_spaces[CONFIG_TASK_NAME_SIZE + 1]; + memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE); + header_spaces[CONFIG_TASK_NAME_SIZE] = '\0'; +#if CONFIG_RR_INTERVAL > 0 + printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces); +#else + printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tMIN STACK USE\tCURR (BASE) PRIO\n", header_spaces); +#endif + + } else { + enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state; + + if (task_state == TSTATE_TASK_PENDING || + task_state == TSTATE_TASK_READYTORUN || + task_state == TSTATE_TASK_RUNNING) { + running_count++; + } + + if (task_state == TSTATE_TASK_INACTIVE || /* BLOCKED - Initialized but not yet activated */ + task_state == TSTATE_WAIT_SEM /* BLOCKED - Waiting for a semaphore */ +#ifndef CONFIG_DISABLE_SIGNALS + || task_state == TSTATE_WAIT_SIG /* BLOCKED - Waiting for a signal */ +#endif +#ifndef CONFIG_DISABLE_MQUEUE + || task_state == TSTATE_WAIT_MQNOTEMPTY /* BLOCKED - Waiting for a MQ to become not empty. */ + || task_state == TSTATE_WAIT_MQNOTFULL /* BLOCKED - Waiting for a MQ to become not full. */ +#endif +#ifdef CONFIG_PAGING + || task_state == TSTATE_WAIT_PAGEFILL /* BLOCKED - Waiting for page fill */ +#endif + ) { + blocked_count++; + } + + char spaces[CONFIG_TASK_NAME_SIZE + 2]; + + /* count name len */ + int namelen = 0; + + while (namelen < CONFIG_TASK_NAME_SIZE) { + if (system_load.tasks[i].tcb->name[namelen] == '\0') break; + + namelen++; + } + + int s = 0; + + for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) { + spaces[s] = ' '; + } + + spaces[s] = '\0'; + + char *runtime_spaces = " "; + + if ((system_load.tasks[i].total_runtime / 1000) < 99) { + runtime_spaces = ""; + } + + printf("\033[K % 2d\t%s%s\t% 8lld ms%s \t % 2d.%03d \t % 6d B", (int)system_load.tasks[i].tcb->pid, system_load.tasks[i].tcb->name, spaces, (system_load.tasks[i].total_runtime / 1000), runtime_spaces, (int)(curr_loads[i] * 100), (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100), (uint32_t)system_load.tasks[i].tcb->adj_stack_ptr - (uint32_t)system_load.tasks[i].tcb->xcp.regs[REG_R13]); + /* Print scheduling info with RR time slice */ +#if CONFIG_RR_INTERVAL > 0 + printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice); +#else + /* Print scheduling info without time slice*/ + printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority); +#endif + } + } + } + + printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J"); + fflush(stdout); + + interval_start_time = new_time; + + char c; + + /* Sleep 200 ms waiting for user input four times */ + /* XXX use poll ... */ + for (int k = 0; k < 4; k++) { + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + printf("Abort\n"); + close(console); + return OK; + } + } + + usleep(200000); + } + + new_time = hrt_absolute_time(); + } + + close(console); + + return OK; +} diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile new file mode 100644 index 000000000..cb126825d --- /dev/null +++ b/apps/systemlib/Makefile @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# System utility library +# + +CSRCS = hx_stream.c \ + mixer.c \ + perf_counter.c + +# +# XXX this really should be a CONFIG_* test +# +ifeq ($(TARGET),px4fmu) +CSRCS += systemlib.c +endif + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemlib/hx_stream.c b/apps/systemlib/hx_stream.c new file mode 100644 index 000000000..e7e974998 --- /dev/null +++ b/apps/systemlib/hx_stream.c @@ -0,0 +1,244 @@ +/**************************************************************************** + * + * 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 A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <crc32.h> +#include <unistd.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include "perf_counter.h" + +#include "hx_stream.h" + + +struct hx_stream { + uint8_t buf[HX_STREAM_MAX_FRAME + 4]; + unsigned frame_bytes; + bool escaped; + bool txerror; + + int fd; + hx_stream_rx_callback callback; + void *callback_arg; + + perf_counter_t pc_tx_frames; + perf_counter_t pc_rx_frames; + perf_counter_t pc_rx_errors; +}; + +/* + * Protocol magic numbers, straight out of HDLC. + */ +#define FBO 0x7e /**< Frame Boundary Octet */ +#define CEO 0x7c /**< Control Escape Octet */ + +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static int hx_rx_frame(hx_stream_t stream); + +static void +hx_tx_raw(hx_stream_t stream, uint8_t c) +{ + if (write(stream->fd, &c, 1) != 1) + stream->txerror = true; +} + +static void +hx_tx_byte(hx_stream_t stream, uint8_t c) +{ + switch (c) { + case FBO: + case CEO: + hx_tx_raw(stream, CEO); + c ^= 0x20; + break; + } + + hx_tx_raw(stream, c); +} + +static int +hx_rx_frame(hx_stream_t stream) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + unsigned length = stream->frame_bytes; + + /* reset the stream */ + stream->frame_bytes = 0; + stream->escaped = false; + + /* not a real frame - too short */ + if (length < 4) { + if (length > 1) + perf_count(stream->pc_rx_errors); + + return 0; + } + + length -= 4; + + /* compute expected CRC */ + u.w = crc32(&stream->buf[0], length); + + /* compare computed and actual CRC */ + for (unsigned i = 0; i < 4; i++) { + if (u.b[i] != stream->buf[length + i]) { + perf_count(stream->pc_rx_errors); + return 0; + } + } + + /* frame is good */ + perf_count(stream->pc_rx_frames); + stream->callback(stream->callback_arg, &stream->buf[0], length); + return 1; +} + +hx_stream_t +hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg) +{ + hx_stream_t stream; + + stream = (hx_stream_t)malloc(sizeof(struct hx_stream)); + + if (stream != NULL) { + memset(stream, 0, sizeof(struct hx_stream)); + stream->fd = fd; + stream->callback = callback; + stream->callback_arg = arg; + } + + return stream; +} + +void +hx_stream_free(hx_stream_t stream) +{ + free(stream); +} + +void +hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors) +{ + stream->pc_tx_frames = tx_frames; + stream->pc_rx_frames = rx_frames; + stream->pc_rx_errors = rx_errors; +} + +int +hx_stream_send(hx_stream_t stream, + const void *data, + size_t count) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + const uint8_t *p = (const uint8_t *)data; + unsigned resid = count; + + if (resid > HX_STREAM_MAX_FRAME) { + errno = EINVAL; + return -1; + } + + /* start the frame */ + hx_tx_raw(stream, FBO); + + /* transmit the data */ + while (resid--) + hx_tx_byte(stream, *p++); + + /* compute the CRC */ + u.w = crc32(data, count); + + /* send the CRC */ + p = &u.b[0]; + resid = 4; + + while (resid--) + hx_tx_byte(stream, *p++); + + /* and the trailing frame separator */ + hx_tx_raw(stream, FBO); + + /* check for transmit error */ + if (stream->txerror) { + stream->txerror = false; + return -1; + } + + return -1; +} + +void +hx_stream_rx(hx_stream_t stream, uint8_t c) +{ + /* frame end? */ + if (c == FBO) { + hx_rx_frame(stream); + return; + } + + /* escaped? */ + if (stream->escaped) { + stream->escaped = false; + c ^= 0x20; + + } else if (c == CEO) { + /* now escaped, ignore the byte */ + stream->escaped = true; + return; + } + + /* save for later */ + if (stream->frame_bytes < sizeof(stream->buf)) + stream->buf[stream->frame_bytes++] = c; +} diff --git a/apps/systemlib/hx_stream.h b/apps/systemlib/hx_stream.h new file mode 100644 index 000000000..591bb1291 --- /dev/null +++ b/apps/systemlib/hx_stream.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#ifndef _SYSTEMLIB_HX_STREAM_H +#define _SYSTEMLIB_HX_STREAM_H + +#include <sys/types.h> + +#include "perf_counter.h" + +struct hx_stream; +typedef struct hx_stream *hx_stream_t; + +#define HX_STREAM_MAX_FRAME 64 + +typedef void (* hx_stream_rx_callback)(void *arg, const void *data, size_t length); + +__BEGIN_DECLS + +/** + * Allocate a new hx_stream object. + * + * @param fd The file handle over which the protocol will + * communicate. + * @param callback Called when a frame is received. + * @param callback_arg Passed to the callback. + * @return A handle to the stream, or NULL if memory could + * not be allocated. + */ +__EXPORT extern hx_stream_t hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg); + +/** + * Free a hx_stream object. + * + * @param stream A handle returned from hx_stream_init. + */ +__EXPORT extern void hx_stream_free(hx_stream_t stream); + +/** + * Set performance counters for the stream. + * + * Any counter may be set to NULL to disable counting that datum. + * + * @param tx_frames Counter for transmitted frames. + * @param rx_frames Counter for received frames. + * @param rx_errors Counter for short and corrupt received frames. + */ +__EXPORT extern void hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors); + +/** + * Send a frame. + * + * This function will block until all frame bytes are sent if + * the descriptor passed to hx_stream_init is marked blocking, + * otherwise it will return -1 (but may transmit a + * runt frame at the same time). + * + * @todo Handling of non-blocking streams needs to be better. + * + * @param stream A handle returned from hx_stream_init. + * @param data Pointer to the data to send. + * @param count The number of bytes to send. + * @return Zero on success, nonzero with errno + * set on error. + */ +__EXPORT extern int hx_stream_send(hx_stream_t stream, + const void *data, + size_t count); + +/** + * Handle a byte from the stream. + * + * @param stream A handle returned from hx_stream_init. + * @param c The character to process. + */ +__EXPORT extern void hx_stream_rx(hx_stream_t stream, + uint8_t c); + +__END_DECLS + +#endif diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c new file mode 100644 index 000000000..f372d04bc --- /dev/null +++ b/apps/systemlib/mixer.c @@ -0,0 +1,79 @@ +/** + * @file Generic control value mixing library. + * + * This library implements a generic mixer function that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * See mixer.h for more details. + */ + +#include "mixer.h" + +static int +scale_check(struct MixScaler *scale) +{ + if (scale->offset > 1.0f) + return -1; + if (scale->offset > 1.0f) + return -1; + if (scale->lower_limit > scale->upper_limit) + return -1; + if (scale->lower_limit < -1.0f) + return -1; + if (scale->upper_limit > 1.0f) + return -1; + return 0; +} + +int +mixer_check(struct MixMixer *mixer, unsigned control_count) +{ + if (mixer->control_count < 1) + return -1; + if (mixer->control_count > control_count) + return -1; + if (!scale_check(&mixer->output_scaler)) + return -1; + + for (unsigned i = 0; i < mixer->control_count; i++) { + if (mixer->control_scaler[i].control >= control_count) + return -1; + if (!scale_check(&mixer->control_scaler[i])) + return -1; + } + return 0; +} + +static float +scale(struct MixScaler *scaler, float input) +{ + float output; + + if (input < 0.0f) { + output = (input * scaler->negative_scale) + scaler->offset; + } else { + output = (input * scaler->positive_scale) + scaler->offset; + } + if (output > scaler->upper_limit) { + output = scaler->upper_limit; + } else if (output < scaler->lower_limit) { + output = scaler->lower_limit; + } + + return output; +} + +float +mixer_mix(struct MixMixer *mixer, float *controls) +{ + struct MixScaler *scaler; + float sum = 0.0f; + + for (unsigned i = 0; i < mixer->control_count; i++) { + scaler = &mixer->control_scaler[i]; + sum += scale(scaler, controls[scaler->control]); + } + + return scale(&mixer->output_scaler, sum); +} diff --git a/apps/systemlib/mixer.h b/apps/systemlib/mixer.h new file mode 100644 index 000000000..fe95204a6 --- /dev/null +++ b/apps/systemlib/mixer.h @@ -0,0 +1,130 @@ +/** + * @file Generic control value mixing library. + * + * This library implements a generic mixer function that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * Terminology + * =========== + * + * control + * A mixer input value, typically provided by some controlling + * component of the system. + * + * actuator + * The mixer output value. + * + * Mixing basics + * ============= + * + * An actuator derives its value from the combination of one or more + * control values. Each of the control values is scaled according to + * the actuator's configuration and then combined to produce the + * actuator value, which may then be further scaled to suit the specific + * output type. + * + * Internally, all scaling is performed using floating point values. + * Inputs and outputs are clamped to the range -1.0 to 1.0. + * + * control control control + * | | | + * v v v + * scale scale scale + * | | | + * | v | + * +-------> mix <------+ + * | + * scale + * | + * v + * out + * + * Scaling + * ------- + * + * Each scaler allows the input value to be scaled independently for + * inputs greater/less than zero. An offset can be applied to the output, + * as well as lower and upper boundary constraints. + * Negative scaling factors cause the output to be inverted (negative input + * produces positive output). + * + * Scaler pseudocode: + * + * if (input < 0) + * output = (input * NEGATIVE_SCALE) + OFFSET + * else + * output = (input * POSITIVE_SCALE) + OFFSET + * + * if (output < LOWER_LIMIT) + * output = LOWER_LIMIT + * if (output > UPPER_LIMIT) + * output = UPPER_LIMIT + * + * + * Mixing + * ------ + * + * Mixing is performed by summing the scaled control values. + * + * + * Controls + * -------- + * + * Each mixer is presented with an array of controls from which it + * selects the set that will be mixed for each actuator. + * + * The precise assignment of controls may vary depending on the + * application, but the following assignments should be used + * when appropriate. + * + * control | standard meaning + * --------+----------------------- + * 0 | roll + * 1 | pitch + * 2 | yaw + * 3 | primary thrust + */ + + struct MixScaler + { + unsigned control; /**< control consumed by this scaler */ + float negative_scale; /**< scale for inputs < 0 */ + float positive_scale; /**< scale for inputs > 0 */ + float offset; /**< bias applied to output */ + float lower_limit; /**< minimum output value */ + float upper_limit; /**< maximum output value */ + }; + + struct MixMixer + { + unsigned control_count; /**< number of control scalers */ + struct MixScaler output_scaler; /**< scaler applied to mixer output */ + struct MixScaler control_scaler[0]; /**< array of control scalers */ + }; + +__BEGIN_DECLS + +/** + * Perform a mixer calculation. + * + * Note that the controls array is assumed to be sufficiently large for any control + * index in the mixer. + * + * @param mixer Mixer configuration. + * @param controls Array of input control values. + * @return The mixed output. + */ + __EXPORT float mixer_mix(struct MixMixer *mixer, float *controls); + + /** + * Check a mixer configuration for sanity. + * + * @param mixer The mixer configuration to be checked. + * @param control_count The number of controls in the system. + * @return Zero if the mixer configuration is sane, + * nonzero otherwise. + */ + __EXPORT int mixer_check(struct MixMixer *mixer, unsigned control_count); + +__END_DECLS diff --git a/apps/systemlib/perf_counter.c b/apps/systemlib/perf_counter.c new file mode 100644 index 000000000..14bd3c436 --- /dev/null +++ b/apps/systemlib/perf_counter.c @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * 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 perf_counter.c + * + * @brief Performance measuring tools. + */ + +#include <stdlib.h> +#include <stdio.h> +#include <sys/queue.h> +#include <arch/board/up_hrt.h> + +#include "perf_counter.h" + +/** + * Header common to all counters. + */ +struct perf_ctr_header { + sq_entry_t link; /**< list linkage */ + enum perf_counter_type type; /**< counter type */ + const char *name; /**< counter name */ +}; + +/** + * PC_EVENT counter. + */ +struct perf_ctr_count { + struct perf_ctr_header hdr; + uint64_t event_count; +}; + +/** + * PC_ELAPSED counter. + */ +struct perf_ctr_elapsed { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_start; + uint64_t time_total; + uint64_t time_least; + uint64_t time_most; +}; + +/** + * List of all known counters. + */ +static sq_queue_t perf_counters; + + +perf_counter_t +perf_alloc(enum perf_counter_type type, const char *name) +{ + perf_counter_t ctr = NULL; + + switch (type) { + case PC_COUNT: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1); + break; + + case PC_ELAPSED: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1); + break; + + default: + break; + } + + if (ctr != NULL) { + ctr->type = type; + ctr->name = name; + sq_addfirst(&ctr->link, &perf_counters); + } + + return ctr; +} + +void +perf_free(perf_counter_t handle) +{ + if (handle == NULL) + return; + + sq_rem(&handle->link, &perf_counters); + free(handle); +} + +void +perf_count(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + ((struct perf_ctr_count *)handle)->event_count++; + break; + + default: + break; + } +} + +void +perf_begin(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: + ((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time(); + break; + + default: + break; + } +} + +void +perf_end(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; + + pce->event_count++; + pce->time_total += elapsed; + + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + } + + default: + break; + } +} + +void +perf_print_counter(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + printf("%s: %llu events\n", + handle->name, + ((struct perf_ctr_count *)handle)->event_count); + break; + + case PC_ELAPSED: + printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + handle->name, + ((struct perf_ctr_elapsed *)handle)->event_count, + ((struct perf_ctr_elapsed *)handle)->time_total, + ((struct perf_ctr_elapsed *)handle)->time_least, + ((struct perf_ctr_elapsed *)handle)->time_most); + + default: + break; + } +} + +void +perf_print_all(void) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + perf_print_counter(handle); + handle = (perf_counter_t)sq_next(&handle->link); + } +} diff --git a/apps/systemlib/perf_counter.h b/apps/systemlib/perf_counter.h new file mode 100644 index 000000000..2ea6591c5 --- /dev/null +++ b/apps/systemlib/perf_counter.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 Performance measuring tools. + */ + +#ifndef _SYSTEMLIB_PERF_COUNTER_H +#define _SYSTEMLIB_PERF_COUNTER_H value + +/** + * Counter types. + */ +enum perf_counter_type { + PC_COUNT, /**< count the number of times an event occurs */ + PC_ELAPSED /**< measure the time elapsed performing an event */ +}; + +struct perf_ctr_header; +typedef struct perf_ctr_header *perf_counter_t; + +__BEGIN_DECLS + +/** + * Create a new counter. + * + * @param type The type of the new counter. + * @param name The counter name. + * @return Handle for the new counter, or NULL if a counter + * could not be allocated. + */ +__EXPORT extern perf_counter_t perf_alloc(enum perf_counter_type type, const char *name); + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +__EXPORT extern void perf_free(perf_counter_t handle); + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_count(perf_counter_t handle); + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_begin(perf_counter_t handle); + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_end(perf_counter_t handle); + + +/** + * Print one performance counter. + * + * @param handle The counter to print. + */ +__EXPORT extern void perf_print_counter(perf_counter_t handle); + +/** + * Print all of the performance counters. + */ +__EXPORT extern void perf_print_all(void); + +__END_DECLS + +#endif diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c new file mode 100644 index 000000000..6bde8b8ae --- /dev/null +++ b/apps/systemlib/systemlib.c @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** @file implementation of commonly used low-level system-call like functions */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <fcntl.h> +#include <sched.h> +#include <signal.h> +#include <sys/stat.h> +#include <unistd.h> +#include <arch/board/drv_eeprom.h> +#include <uORB/parameter_storage.h> +#include <float.h> +#include <string.h> + +#include "systemlib.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +const struct __multiport_info multiport_info = { + .port_names = {"MULT_0_US2_RXTX", "MULT_1_US2_FLOW", "MULT_2_GPIO_12"} +}; + +#define EEPROM_OFFSET 64 + +#define EEPROM_PARAM_MAGIC_BYTE 0xAF + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void kill_task(FAR _TCB *tcb, FAR void *arg); + +/**************************************************************************** + * user_start + ****************************************************************************/ + +int reboot(void) +{ + sched_lock(); + // print text + printf("\r\nRebooting system - ending tasks and performing hard reset\r\n\r\n"); + fflush(stdout); + //usleep(5000); + + /* Sending kill signal to other tasks */ + //killall(); + + /* Waiting maximum time for all to exit */ + //usleep(5000); + //sched_lock(); + + /* Resetting CPU */ + // FIXME Need check for ARM architecture here +#ifndef NVIC_AIRCR +#define NVIC_AIRCR (*((uint32_t*)0xE000ED0C)) +#endif + + /* Set the SYSRESETREQ bit to force a reset */ + NVIC_AIRCR = 0x05fa0004; + + /* Spinning until the board is really reset */ + while (true); + + /* Should never reach here */ + return 0; +} + +void killall() +{ +// printf("Sending SIGUSR1 to all processes now\n"); + + /* iterate through all tasks and send kill signal */ + sched_foreach(kill_task, NULL); +} + +void kill_task(FAR _TCB *tcb, FAR void *arg) +{ + kill(tcb->pid, SIGUSR1); +} + +int store_params_in_eeprom(struct global_data_parameter_storage_t *params) +{ + int ret = ERROR; + int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); + int lseek_res = lseek(fd, EEPROM_OFFSET, SEEK_SET); //don't touch first 64 bytes + int write_res; + + if (fd < 0) { + fprintf(stderr, "onboard eeprom: open fail\n"); + ret = ERROR; + + } else if (lseek_res < 0) { + fprintf(stderr, "onboard eeprom: set offet fail\n"); + ret = ERROR; + + } else { + /*Write start magic byte */ + uint8_t mb = EEPROM_PARAM_MAGIC_BYTE; + write_res = write(fd, &mb, sizeof(mb)); + + if (write_res != sizeof(mb)) { + ret = ERROR; + + } else { + for (int i = 0; i < params->pm.size; i++) { + write_res = write(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i])); + + if (write_res != sizeof(params->pm.param_values[i])) return ERROR; + } + + /*Write end magic byte */ + write_res = write(fd, &mb, sizeof(mb)); + + if (write_res != sizeof(mb)) { + ret = ERROR; + + } else { + ret = OK; + } + } + + } + + close(fd); + + return ret; +} + +int get_params_from_eeprom(struct global_data_parameter_storage_t *params) +{ + int ret = ERROR; + uint8_t magic_byte = 0; + int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); + int lseek_res = lseek(fd, EEPROM_OFFSET, SEEK_SET); //don't touch first 64 bytes + + if (fd < 0) { + fprintf(stderr, "onboard eeprom: open fail\n"); + ret = ERROR; + + } else if (lseek_res < 0) { + fprintf(stderr, "onboard eeprom: set offet fail\n"); + ret = ERROR; + + } else { + /*Get start magic byte */ + magic_byte = 0; + int read_res = read(fd, &magic_byte, sizeof(uint8_t)); + + if (read_res != sizeof(uint8_t)) { + ret = ERROR; + + } else { + if (magic_byte != EEPROM_PARAM_MAGIC_BYTE) { + ret = ERROR; + fprintf(stderr, "onboard eeprom: parameters: start magic byte wrong\n"); + + } else { + /*get end magic byte */ + lseek_res = lseek(fd, EEPROM_OFFSET + 1 + params->pm.size * sizeof(float), SEEK_SET); // jump to 2nd magic byte + + if (lseek_res == OK) { + /*Get end magic */ + read_res = read(fd, &magic_byte, sizeof(uint8_t)); + + if (read_res != sizeof(uint8_t)) { + ret = ERROR; + + } else { + if (magic_byte != EEPROM_PARAM_MAGIC_BYTE) { + ret = ERROR; + printf("onboard eeprom: parameters: end magic byte wrong\n"); + + } else { + lseek_res = lseek(fd, EEPROM_OFFSET + 1, SEEK_SET); + + /* read data */ + if (lseek_res == OK) { + + for (int i = 0; i < params->pm.size; i++) { + read_res = read(fd, params->pm.param_values + i, sizeof(params->pm.param_values[i])); + + if (read_res != sizeof(params->pm.param_values[i])) return ERROR; + } + + ret = OK; + + } else { + /* lseek #2 failed */ + ret = ERROR; + } + } + } + + } else { + /* lseek #1 failed */ + ret = ERROR; + } + } + } + + } + + close(fd); + + return ret; +} + +#define PX4_BOARD_ID_FMU (5) + +int fmu_get_board_info(struct fmu_board_info_s *info) +{ + /* Check which FMU version we're on */ + struct stat sb; + int statres; + + /* Copy version-specific fields */ + statres = stat("/dev/bma280", &sb); + + if (statres == OK) { + /* BMA280 indicates a v1.7+ board */ + strcpy(info->board_name, "FMU v1.7"); + info->board_version = 17; + + } else { + statres = stat("/dev/bma180", &sb); + + if (statres == OK) { + /* BMA180 indicates a v1.5-v1.6 board */ + strcpy(info->board_name, "FMU v1.6"); + info->board_version = 16; + + } else { + /* If no BMA pressure sensor is present, it is a v1.3 board */ + strcpy(info->board_name, "FMU v1.3"); + info->board_version = 13; + } + } + + /* Copy general FMU fields */ + memcpy(info->header, "PX4", 3); + info->board_id = PX4_BOARD_ID_FMU; + + return sizeof(struct fmu_board_info_s); +} + +int carrier_store_board_info(const struct carrier_board_info_s *info) +{ + int ret; + int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK); + + if (fd < 0) fprintf(stderr, "[boardinfo carrier] ERROR opening carrier eeprom\n"); + + /* Enforce correct header */ + ret = write(fd, info, sizeof(struct carrier_board_info_s)); + //ret = write(fd, "PX4", 3); + close(fd); + + return ret; +} + +int carrier_get_board_info(struct carrier_board_info_s *info) +{ + int ret; + int fd = open("/dev/eeprom", O_RDONLY | O_NONBLOCK); + + if (fd < 0) fprintf(stderr, "[boardinfo carrier] ERROR opening carrier eeprom\n"); + + ret = read(fd, info, sizeof(struct carrier_board_info_s)); + + /* Enforce NUL termination of human-readable string */ + if (ret == sizeof(struct carrier_board_info_s)) { + info->board_name[sizeof(info->board_name) - 1] = '\0'; + } + + close(fd); + + return ret; +} diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h new file mode 100644 index 000000000..e75f6ec9c --- /dev/null +++ b/apps/systemlib/systemlib.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** @file definition of commonly used low-level system-call like functions */ + +#ifndef SYSTEMLIB_H_ +#define SYSTEMLIB_H_ +#include <float.h> +#include <stdint.h> + +__BEGIN_DECLS + +/** Reboots the board */ +__EXPORT int reboot(void); + +/** Sends SIGUSR1 to all processes */ +__EXPORT void killall(void); + +struct global_data_parameter_storage_t; + +__EXPORT int store_params_in_eeprom(struct global_data_parameter_storage_t *params); + +__EXPORT int get_params_from_eeprom(struct global_data_parameter_storage_t *params); + +enum MULT_PORTS { + MULT_0_US2_RXTX = 0, + MULT_1_US2_FLOW, + MULT_2_GPIO_12, + MULT_COUNT +}; + +/* Check max multi port count */ +#if (MULT_COUNT > 33) +#error "MULT_COUNT HAS TO BE LESS THAN OR EQUAL 33" +#endif + +/* FMU board info, to be stored in the first 64 bytes of the FMU EEPROM */ +#pragma pack(push,1) +struct fmu_board_info_s { + char header[3]; /**< {'P', 'X', '4'} */ + char board_name[20]; /**< Human readable board name, \0 terminated */ + uint8_t board_id; /**< board ID, constantly increasing number per board */ + uint8_t board_version; /**< board version, major * 10 + minor: v1.7 = 17 */ + uint8_t multi_port_config[MULT_COUNT]; /**< Configuration of multi ports 1-3 */ + uint8_t reserved[33 - MULT_COUNT]; /**< Reserved space for more multi ports */ + uint16_t production_year; + uint8_t production_month; + uint8_t production_day; + uint8_t production_fab; + uint8_t production_tester; +}; /**< stores autopilot board information meta data from EEPROM */ +#pragma pack(pop) + +/* Carrier board info, to be stored in the 128 byte board info EEPROM */ +#pragma pack(push,1) +struct carrier_board_info_s { + char header[3]; /**< {'P', 'X', '4'} */ + char board_name[20]; /**< Human readable board name, \0 terminated */ + uint8_t board_id; /**< board ID, constantly increasing number per board */ + uint8_t board_version; /**< board version, major * 10 + minor: v1.7 = 17 */ + uint8_t multi_port_config[MULT_COUNT]; /**< Configuration of multi ports 1-3 */ + uint8_t reserved[33 - MULT_COUNT]; /**< Reserved space for more multi ports */ + uint16_t production_year; + uint8_t production_month; + uint8_t production_day; + uint8_t production_fab; + uint8_t production_tester; + char board_custom_data[64]; +}; /**< stores carrier board information meta data from EEPROM */ +#pragma pack(pop) + +struct __multiport_info { + const char *port_names[MULT_COUNT]; +}; +__EXPORT extern const struct __multiport_info multiport_info; + +__EXPORT int carrier_store_board_info(const struct carrier_board_info_s *info); +__EXPORT int carrier_get_board_info(struct carrier_board_info_s *info); + +__EXPORT int fmu_get_board_info(struct fmu_board_info_s *info); + +__END_DECLS + +#endif /* SYSTEMLIB_H_ */ diff --git a/apps/systemlib/visibility.h b/apps/systemlib/visibility.h new file mode 100644 index 000000000..9bd20e2a8 --- /dev/null +++ b/apps/systemlib/visibility.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 Definitions controlling symbol naming and visibility. + * + * This file is normally included automatically by the build system. + */ + +#ifndef __SYSTEMLIB_VISIBILITY_H +#define __SYSTEMLIB_VISIBILITY_H + +#ifdef __EXPORT +# undef __EXPORT +#endif +#define __EXPORT __attribute__ ((visibility ("default"))) + +#ifdef __PRIVATE +# undef __PRIVATE +#endif +#define __PRIVATE __attribute__ ((visibility ("hidden"))) + +#ifdef __cplusplus +# define __BEGIN_DECLS extern "C" { +# define __END_DECLS } +#else +# define __BEGIN_DECLS +# define __END_DECLS +#endif +#endif
\ No newline at end of file diff --git a/apps/uORB/.context b/apps/uORB/.context new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/apps/uORB/.context diff --git a/apps/uORB/Makefile b/apps/uORB/Makefile new file mode 100644 index 000000000..0766a66eb --- /dev/null +++ b/apps/uORB/Makefile @@ -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. +# +############################################################################ + +# +# Makefile to build uORB +# + +APPNAME = uorb +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp new file mode 100644 index 000000000..8d84ff3c2 --- /dev/null +++ b/apps/uORB/objects_common.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 Common object definitions without a better home. + */ + +#include <nuttx/config.h> + +#include <drivers/drv_orb_dev.h> + +#include <drivers/drv_mag.h> +ORB_DEFINE(sensor_mag, struct mag_report); + +#include <drivers/drv_accel.h> +ORB_DEFINE(sensor_accel, struct accel_report); + +#include <drivers/drv_gyro.h> +ORB_DEFINE(sensor_gyro, struct gyro_report); + +#include <drivers/drv_baro.h> +ORB_DEFINE(sensor_baro, struct baro_report); + +#include <drivers/drv_pwm_output.h> +ORB_DEFINE(output_pwm, struct pwm_output_values); + +#include <drivers/drv_rc_input.h> +ORB_DEFINE(input_rc, struct rc_input_values); + +// XXX need to check wether these should be here +#include "topics/vehicle_attitude.h" +ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); + +#include "topics/sensor_combined.h" +ORB_DEFINE(sensor_combined, struct sensor_combined_s); + +#include "topics/vehicle_gps_position.h" +ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); + +#include "topics/vehicle_status.h" +ORB_DEFINE(vehicle_status, struct vehicle_status_s); + +#include "topics/vehicle_global_position.h" +ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); + +#include "topics/vehicle_local_position.h" +ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s); + +#include "topics/ardrone_control.h" +ORB_DEFINE(ardrone_control, struct ardrone_control_s); + +#include "topics/ardrone_motors_setpoint.h" +ORB_DEFINE(ardrone_motors_setpoint, struct ardrone_motors_setpoint_s); + +#include "topics/rc_channels.h" +ORB_DEFINE(rc_channels, struct rc_channels_s); + +#include "topics/fixedwing_control.h" +ORB_DEFINE(fixedwing_control, struct fixedwing_control_s); + +#include "topics/vehicle_command.h" +ORB_DEFINE(vehicle_command, struct vehicle_command_s); + +#include "topics/vehicle_local_position_setpoint.h" +ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); + +#include "topics/vehicle_global_position_setpoint.h" +ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); + +#include "topics/vehicle_attitude_setpoint.h" +ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s); diff --git a/apps/uORB/parameter_storage.c b/apps/uORB/parameter_storage.c new file mode 100644 index 000000000..f4bd1c712 --- /dev/null +++ b/apps/uORB/parameter_storage.c @@ -0,0 +1,379 @@ + +#include "parameter_storage.h" +#include <stdbool.h> + + +/* Global symbols / flags */ + +struct global_data_parameter_storage_t global_data_parameter_storage_d = { /*.counter = 0, .timestamp = 0,*/ .pm = {.size = PARAM_MAX_COUNT, + .param_values[PARAM_SYSTEM_ID] = 12, + .param_names[PARAM_SYSTEM_ID] = "SYS_ID", + .param_needs_write[PARAM_SYSTEM_ID] = false, + + .param_values[PARAM_COMP_ID] = 200, + .param_names[PARAM_COMP_ID] = "COMP_ID", + .param_needs_write[PARAM_COMP_ID] = false, + + .param_values[PARAM_FLIGHT_ENV] = (float)PX4_FLIGHT_ENVIRONMENT_INDOOR, + .param_names[PARAM_FLIGHT_ENV] = "FLIGHT_ENV", + .param_needs_write[PARAM_FLIGHT_ENV] = false, + + .param_values[PARAM_BATTERYVOLTAGE_CONVERSION] = -1.f, + .param_names[PARAM_BATTERYVOLTAGE_CONVERSION] = "BATVOLTAG_CONV", + .param_needs_write[PARAM_BATTERYVOLTAGE_CONVERSION] = false, + + .param_values[PARAM_PID_YAWPOS_P] = 0.3f, + .param_names[PARAM_PID_YAWPOS_P] = "PID_YAWPOS_P", + .param_needs_write[PARAM_PID_YAWPOS_P] = false, + + .param_values[PARAM_PID_YAWPOS_I] = 0.15f, + .param_names[PARAM_PID_YAWPOS_I] = "PID_YAWPOS_I", + .param_needs_write[PARAM_PID_YAWPOS_I] = false, + + .param_values[PARAM_PID_YAWPOS_D] = 0.0f, + .param_names[PARAM_PID_YAWPOS_D] = "PID_YAWPOS_D", + .param_needs_write[PARAM_PID_YAWPOS_D] = false, + + .param_values[PARAM_PID_YAWPOS_AWU] = 1.0f, + .param_names[PARAM_PID_YAWPOS_AWU] = "PID_YAWPOS_AWU", + .param_needs_write[PARAM_PID_YAWPOS_AWU] = false, + + .param_values[PARAM_PID_YAWPOS_LIM] = 3.0f, + .param_names[PARAM_PID_YAWPOS_LIM] = "PID_YAWPOS_LIM", + .param_needs_write[PARAM_PID_YAWPOS_LIM] = false, + + .param_values[PARAM_PID_YAWSPEED_P] = 0.1f, + .param_names[PARAM_PID_YAWSPEED_P] = "PID_YAWSPD_P", + .param_needs_write[PARAM_PID_YAWSPEED_P] = false, + + .param_values[PARAM_PID_YAWSPEED_I] = 0.02f, + .param_names[PARAM_PID_YAWSPEED_I] = "PID_YAWSPD_I", + .param_needs_write[PARAM_PID_YAWSPEED_I] = false, + + .param_values[PARAM_PID_YAWSPEED_D] = 0.0f, + .param_names[PARAM_PID_YAWSPEED_D] = "PID_YAWSPD_D", + .param_needs_write[PARAM_PID_YAWSPEED_D] = false, + + .param_values[PARAM_PID_YAWSPEED_AWU] = 0.02f, + .param_names[PARAM_PID_YAWSPEED_AWU] = "PID_YAWSPD_AWU", + .param_needs_write[PARAM_PID_YAWSPEED_AWU] = false, + + .param_values[PARAM_PID_YAWSPEED_LIM] = 0.1f, + .param_names[PARAM_PID_YAWSPEED_LIM] = "PID_YAWSPD_LIM", + .param_needs_write[PARAM_PID_YAWSPEED_LIM] = false, + + .param_values[PARAM_PID_ATT_P] = 0.3f, + .param_names[PARAM_PID_ATT_P] = "PID_ATT_P", + .param_needs_write[PARAM_PID_ATT_P] = false, + + .param_values[PARAM_PID_ATT_I] = 0.0f, + .param_names[PARAM_PID_ATT_I] = "PID_ATT_I", + .param_needs_write[PARAM_PID_ATT_I] = false, + + .param_values[PARAM_PID_ATT_D] = 0.1f, + .param_names[PARAM_PID_ATT_D] = "PID_ATT_D", + .param_needs_write[PARAM_PID_ATT_D] = false, + + .param_values[PARAM_PID_ATT_AWU] = 0.05f, + .param_names[PARAM_PID_ATT_AWU] = "PID_ATT_AWU", + .param_needs_write[PARAM_PID_ATT_AWU] = false, + + .param_values[PARAM_PID_ATT_LIM] = 0.3f, + .param_names[PARAM_PID_ATT_LIM] = "PID_ATT_LIM", + .param_needs_write[PARAM_PID_ATT_LIM] = false, + + .param_values[PARAM_PID_POS_P] = 40.0f, + .param_names[PARAM_PID_POS_P] = "PID_POS_P", + .param_needs_write[PARAM_PID_POS_P] = false, + + .param_values[PARAM_PID_POS_I] = 0.0f, + .param_names[PARAM_PID_POS_I] = "PID_POS_I", + .param_needs_write[PARAM_PID_POS_I] = false, + + .param_values[PARAM_PID_POS_D] = 0.0f, + .param_names[PARAM_PID_POS_D] = "PID_POS_D", + .param_needs_write[PARAM_PID_POS_D] = false, + + .param_values[PARAM_PID_POS_AWU] = 5.0f, + .param_names[PARAM_PID_POS_AWU] = "PID_POS_AWU", + .param_needs_write[PARAM_PID_POS_AWU] = false, + + .param_values[PARAM_PID_POS_LIM] = 0.3f, + .param_names[PARAM_PID_POS_LIM] = "PID_POS_LIM", + .param_needs_write[PARAM_PID_POS_LIM] = false, + + .param_values[PARAM_PID_POS_Z_P] = 10.0f, + .param_names[PARAM_PID_POS_Z_P] = "PID_POS_Z_P", + .param_needs_write[PARAM_PID_POS_Z_P] = false, + + .param_values[PARAM_PID_POS_Z_I] = 0.0f, + .param_names[PARAM_PID_POS_Z_I] = "PID_POS_Z_I", + .param_needs_write[PARAM_PID_POS_Z_I] = false, + + .param_values[PARAM_PID_POS_Z_D] = 0.0f, + .param_names[PARAM_PID_POS_Z_D] = "PID_POS_Z_D", + .param_needs_write[PARAM_PID_POS_Z_D] = false, + + .param_values[PARAM_PID_POS_Z_AWU] = 3.0f, + .param_names[PARAM_PID_POS_Z_AWU] = "PID_POS_Z_AWU", + .param_needs_write[PARAM_PID_POS_Z_AWU] = false, + + .param_values[PARAM_PID_POS_Z_LIM] = 0.3f, + .param_names[PARAM_PID_POS_Z_LIM] = "PID_POS_Z_LIM", + .param_needs_write[PARAM_PID_POS_Z_LIM] = false, + + .param_values[PARAM_AIRSPEED] = 30.0f, + .param_names[PARAM_AIRSPEED] = "AIRSPEED", + .param_needs_write[PARAM_AIRSPEED] = false, + + .param_values[PARAM_WPLON] = -120.0f, + .param_names[PARAM_WPLON] = "WPLON", + .param_needs_write[PARAM_WPLON] = false, + + .param_values[PARAM_WPLAT] = 38.0f, + .param_names[PARAM_WPLAT] = "WPLAT", + .param_needs_write[PARAM_WPLAT] = false, + + .param_values[PARAM_WPALT] = 500.0f, + .param_names[PARAM_WPALT] = "WPALT", + .param_needs_write[PARAM_WPALT] = false, + + .param_values[PARAM_FLIGHTMODE] = CRUISE, + .param_names[PARAM_FLIGHTMODE] = "FLIGHTMODE", + .param_needs_write[PARAM_FLIGHTMODE] = false, + + .param_values[PARAM_SENSOR_GYRO_XOFFSET] = 700.f, + .param_names[PARAM_SENSOR_GYRO_XOFFSET] = "SENSOR_GYRO_XOF", + .param_needs_write[PARAM_SENSOR_GYRO_XOFFSET] = false, + + .param_values[PARAM_SENSOR_GYRO_YOFFSET] = 1400.0f, + .param_names[PARAM_SENSOR_GYRO_YOFFSET] = "SENSOR_GYRO_YOF", + .param_needs_write[PARAM_SENSOR_GYRO_YOFFSET] = false, + + .param_values[PARAM_SENSOR_GYRO_ZOFFSET] = 0.0f, + .param_names[PARAM_SENSOR_GYRO_ZOFFSET] = "SENSOR_GYRO_ZOF", + .param_needs_write[PARAM_SENSOR_GYRO_ZOFFSET] = false, + + .param_values[PARAM_SENSOR_MAG_XOFFSET] = 422.0f, + .param_names[PARAM_SENSOR_MAG_XOFFSET] = "SENSOR_MAG_XOF", + .param_needs_write[PARAM_SENSOR_MAG_XOFFSET] = false, + + .param_values[PARAM_SENSOR_MAG_YOFFSET] = -85.0f, + .param_names[PARAM_SENSOR_MAG_YOFFSET] = "SENSOR_MAG_YOF", + .param_needs_write[PARAM_SENSOR_MAG_YOFFSET] = false, + + .param_values[PARAM_SENSOR_MAG_ZOFFSET] = -370.0f, + .param_names[PARAM_SENSOR_MAG_ZOFFSET] = "SENSOR_MAG_ZOF", + .param_needs_write[PARAM_SENSOR_MAG_ZOFFSET] = false, + + .param_values[PARAM_ATT_XOFFSET] = 0.0f, + .param_names[PARAM_ATT_XOFFSET] = "ATT_XOFF", + .param_needs_write[PARAM_ATT_XOFFSET] = false, + + .param_values[PARAM_ATT_YOFFSET] = 0.0f, + .param_names[PARAM_ATT_YOFFSET] = "ATT_YOFF", + .param_needs_write[PARAM_ATT_YOFFSET] = false, + + .param_values[PARAM_RC1_MIN] = 1000.0f, + .param_names[PARAM_RC1_MIN] = "RC1_MIN", + .param_needs_write[PARAM_RC1_MIN] = false, + + .param_values[PARAM_RC1_MAX] = 2000.0f, + .param_names[PARAM_RC1_MAX] = "RC1_MAX", + .param_needs_write[PARAM_RC1_MAX] = false, + + .param_values[PARAM_RC1_TRIM] = 1500.0f, + .param_names[PARAM_RC1_TRIM] = "RC1_TRIM", + .param_needs_write[PARAM_RC1_TRIM] = false, + + .param_values[PARAM_RC1_REV] = 1.0f, + .param_names[PARAM_RC1_REV] = "RC1_REV", + .param_needs_write[PARAM_RC1_REV] = false, + + .param_values[PARAM_RC2_MIN] = 1000.0f, + .param_names[PARAM_RC2_MIN] = "RC2_MIN", + .param_needs_write[PARAM_RC2_MIN] = false, + + .param_values[PARAM_RC2_MAX] = 2000.0f, + .param_names[PARAM_RC2_MAX] = "RC2_MAX", + .param_needs_write[PARAM_RC2_MAX] = false, + + .param_values[PARAM_RC2_TRIM] = 1500.0f, + .param_names[PARAM_RC2_TRIM] = "RC2_TRIM", + .param_needs_write[PARAM_RC2_TRIM] = false, + + .param_values[PARAM_RC2_REV] = 1.0f, + .param_names[PARAM_RC2_REV] = "RC2_REV", + .param_needs_write[PARAM_RC2_REV] = false, + + .param_values[PARAM_RC3_MIN] = 1000.0f, + .param_names[PARAM_RC3_MIN] = "RC3_MIN", + .param_needs_write[PARAM_RC3_MIN] = false, + + .param_values[PARAM_RC3_MAX] = 2000.0f, + .param_names[PARAM_RC3_MAX] = "RC3_MAX", + .param_needs_write[PARAM_RC3_MAX] = false, + + .param_values[PARAM_RC3_TRIM] = 1500.0f, + .param_names[PARAM_RC3_TRIM] = "RC3_TRIM", + .param_needs_write[PARAM_RC3_TRIM] = false, + + .param_values[PARAM_RC3_REV] = 1.0f, + .param_names[PARAM_RC3_REV] = "RC3_REV", + .param_needs_write[PARAM_RC3_REV] = false, + + .param_values[PARAM_RC4_MIN] = 1000.0f, + .param_names[PARAM_RC4_MIN] = "RC4_MIN", + .param_needs_write[PARAM_RC4_MIN] = false, + + .param_values[PARAM_RC4_MAX] = 2000.0f, + .param_names[PARAM_RC4_MAX] = "RC4_MAX", + .param_needs_write[PARAM_RC4_MAX] = false, + + .param_values[PARAM_RC4_TRIM] = 1500.0f, + .param_names[PARAM_RC4_TRIM] = "RC4_TRIM", + .param_needs_write[PARAM_RC4_TRIM] = false, + + .param_values[PARAM_RC4_REV] = 1.0f, + .param_names[PARAM_RC4_REV] = "RC4_REV", + .param_needs_write[PARAM_RC4_MIN] = false, + + .param_values[PARAM_RC5_MIN] = 1000.0f, + .param_names[PARAM_RC5_MIN] = "RC5_MIN", + .param_needs_write[PARAM_RC5_MIN] = false, + + .param_values[PARAM_RC5_MAX] = 2000.0f, + .param_names[PARAM_RC5_MAX] = "RC5_MAX", + .param_needs_write[PARAM_RC5_MAX] = false, + + .param_values[PARAM_RC5_TRIM] = 1500.0f, + .param_names[PARAM_RC5_TRIM] = "RC5_TRIM", + .param_needs_write[PARAM_RC5_TRIM] = false, + + .param_values[PARAM_RC5_REV] = 1.0f, + .param_names[PARAM_RC5_REV] = "RC5_REV", + .param_needs_write[PARAM_RC5_REV] = false, + + .param_values[PARAM_RC6_MIN] = 1000.0f, + .param_names[PARAM_RC6_MIN] = "RC6_MIN", + .param_needs_write[PARAM_RC6_MIN] = false, + + .param_values[PARAM_RC6_MAX] = 2000.0f, + .param_names[PARAM_RC6_MAX] = "RC6_MAX", + .param_needs_write[PARAM_RC6_MAX] = false, + + .param_values[PARAM_RC6_TRIM] = 1500.0f, + .param_names[PARAM_RC6_TRIM] = "RC6_TRIM", + .param_needs_write[PARAM_RC6_TRIM] = false, + + .param_values[PARAM_RC6_REV] = 1.0f, + .param_names[PARAM_RC6_REV] = "RC6_REV", + .param_needs_write[PARAM_RC6_REV] = false, + + .param_values[PARAM_RC7_MIN] = 1000, + .param_names[PARAM_RC7_MIN] = "RC7_MIN", + .param_needs_write[PARAM_RC7_MIN] = false, + + .param_values[PARAM_RC7_MAX] = 2000, + .param_names[PARAM_RC7_MAX] = "RC7_MAX", + .param_needs_write[PARAM_RC7_MAX] = false, + + .param_values[PARAM_RC7_TRIM] = 1500, + .param_names[PARAM_RC7_TRIM] = "RC7_TRIM", + .param_needs_write[PARAM_RC7_TRIM] = false, + + .param_values[PARAM_RC7_REV] = 1.0f, + .param_names[PARAM_RC7_REV] = "RC7_REV", + .param_needs_write[PARAM_RC7_REV] = false, + + .param_values[PARAM_RC8_MIN] = 1000, + .param_names[PARAM_RC8_MIN] = "RC8_MIN", + .param_needs_write[PARAM_RC8_MIN] = false, + + .param_values[PARAM_RC8_MAX] = 2000, + .param_names[PARAM_RC8_MAX] = "RC8_MAX", + .param_needs_write[PARAM_RC8_MAX] = false, + + .param_values[PARAM_RC8_TRIM] = 1500, + .param_names[PARAM_RC8_TRIM] = "RC8_TRIM", + .param_needs_write[PARAM_RC8_TRIM] = false, + + .param_values[PARAM_RC8_REV] = 1.0f, + .param_names[PARAM_RC8_REV] = "RC8_REV", + .param_needs_write[PARAM_RC8_REV] = false, + + .param_values[PARAM_ROLL_CHAN] = 1, + .param_names[PARAM_ROLL_CHAN] = "ROLL_CHAN", + .param_needs_write[PARAM_ROLL_CHAN] = false, + + .param_values[PARAM_PITCH_CHAN] = 2, + .param_names[PARAM_PITCH_CHAN] = "PITCH_CHAN", + .param_needs_write[PARAM_PITCH_CHAN] = false, + + .param_values[PARAM_THROTTLE_CHAN] = 3, + .param_names[PARAM_THROTTLE_CHAN] = "THROTTLE_CHAN", + .param_needs_write[PARAM_THROTTLE_CHAN] = false, + + .param_values[PARAM_YAW_CHAN] = 4, + .param_names[PARAM_YAW_CHAN] = "YAW_CHAN", + .param_needs_write[PARAM_YAW_CHAN] = false, + + .param_values[PARAM_OVERRIDE_CHAN] = 5, + .param_names[PARAM_OVERRIDE_CHAN] = "OVERRIDE_CHAN", + .param_needs_write[PARAM_OVERRIDE_CHAN] = false, + + .param_values[PARAM_SERVO1_MIN] = 1000.0f, + .param_names[PARAM_SERVO1_MIN] = "SERVO1_MIN", + .param_needs_write[PARAM_SERVO1_MIN] = false, + + .param_values[PARAM_SERVO1_MAX] = 2000.0f, + .param_names[PARAM_SERVO1_MAX] = "SERVO1_MAX", + .param_needs_write[PARAM_SERVO1_MAX] = false, + + .param_values[PARAM_SERVO1_TRIM] = 1500.0f, + .param_names[PARAM_SERVO1_TRIM] = "SERVO1_TRIM", + .param_needs_write[PARAM_SERVO1_TRIM] = false, + + .param_values[PARAM_SERVO2_MIN] = 1000.0f, + .param_names[PARAM_SERVO2_MIN] = "SERVO2_MIN", + .param_needs_write[PARAM_SERVO2_MIN] = false, + + .param_values[PARAM_SERVO2_MAX] = 2000.0f, + .param_names[PARAM_SERVO2_MAX] = "SERVO2_MAX", + .param_needs_write[PARAM_SERVO2_MAX] = false, + + .param_values[PARAM_SERVO2_TRIM] = 1500.0f, + .param_names[PARAM_SERVO2_TRIM] = "SERVO2_TRIM", + .param_needs_write[PARAM_SERVO2_TRIM] = false, + + .param_values[PARAM_SERVO3_MIN] = 1000.0f, + .param_names[PARAM_SERVO3_MIN] = "SERVO3_MIN", + .param_needs_write[PARAM_SERVO3_MIN] = false, + + .param_values[PARAM_SERVO3_MAX] = 2000.0f, + .param_names[PARAM_SERVO3_MAX] = "SERVO3_MAX", + .param_needs_write[PARAM_SERVO3_MAX] = false, + + .param_values[PARAM_SERVO3_TRIM] = 1500.0f, + .param_names[PARAM_SERVO3_TRIM] = "SERVO3_TRIM", + .param_needs_write[PARAM_SERVO3_TRIM] = false, + + .param_values[PARAM_SERVO4_MIN] = 1000.0f, + .param_names[PARAM_SERVO4_MIN] = "SERVO4_MIN", + .param_needs_write[PARAM_SERVO4_MIN] = false, + + .param_values[PARAM_SERVO4_MAX] = 2000.0f, + .param_names[PARAM_SERVO4_MAX] = "SERVO4_MAX", + .param_needs_write[PARAM_SERVO4_MAX] = false, + + .param_values[PARAM_SERVO4_TRIM] = 1500.0f, + .param_names[PARAM_SERVO4_TRIM] = "SERVO4_TRIM", + .param_needs_write[PARAM_SERVO4_TRIM] = false, + + .param_values[PARAM_SERVO_SCALE] = 20.0f, + .param_names[PARAM_SERVO_SCALE] = "SERVO_SCALE", + .param_needs_write[PARAM_SERVO_SCALE] = false + } +}; + +struct global_data_parameter_storage_t *global_data_parameter_storage = &global_data_parameter_storage_d; diff --git a/apps/uORB/parameter_storage.h b/apps/uORB/parameter_storage.h new file mode 100644 index 000000000..54ae63cb9 --- /dev/null +++ b/apps/uORB/parameter_storage.h @@ -0,0 +1,148 @@ +/* Structure for storage of parameters */ + +#ifndef GLOBAL_DATA_PARAMETER_STORAGE_T_H_ //adjust this line! +#define GLOBAL_DATA_PARAMETER_STORAGE_T_H_ //adjust this line! + +#define PX4_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 +#define MAX_PARAM_NAME_LEN 16 + +#include <stdbool.h> +#include <stdint.h> + +enum MODE { + TAKEOFF = 1, + CRUISE, + LOITER, + LAND +}; + +enum PARAM { + PARAM_SYSTEM_ID = 0, ///< System ID used for communication + PARAM_COMP_ID, ///< Component ID + PARAM_FLIGHT_ENV, ///< Flight environment, indoor or outdoor + PARAM_BATTERYVOLTAGE_CONVERSION, ///< Conversion factor from adc measurement to millivolts. if not set the sensor app will use the default value for the ardrone board + PARAM_PID_YAWPOS_P, + PARAM_PID_YAWPOS_I, + PARAM_PID_YAWPOS_D, + PARAM_PID_YAWPOS_AWU, + PARAM_PID_YAWPOS_LIM, + PARAM_PID_YAWSPEED_P, + PARAM_PID_YAWSPEED_I, + PARAM_PID_YAWSPEED_D, + PARAM_PID_YAWSPEED_AWU, + PARAM_PID_YAWSPEED_LIM, + PARAM_PID_ATT_P, + PARAM_PID_ATT_I, + PARAM_PID_ATT_D, + PARAM_PID_ATT_AWU, + PARAM_PID_ATT_LIM, + PARAM_PID_POS_P, + PARAM_PID_POS_I, + PARAM_PID_POS_D, + PARAM_PID_POS_AWU, + PARAM_PID_POS_LIM, + PARAM_PID_POS_Z_P, + PARAM_PID_POS_Z_I, + PARAM_PID_POS_Z_D, + PARAM_PID_POS_Z_AWU, + PARAM_PID_POS_Z_LIM, + PARAM_AIRSPEED, + PARAM_WPLON, + PARAM_WPLAT, + PARAM_WPALT, + PARAM_FLIGHTMODE, + PARAM_SENSOR_GYRO_XOFFSET, + PARAM_SENSOR_GYRO_YOFFSET, + PARAM_SENSOR_GYRO_ZOFFSET, + PARAM_SENSOR_MAG_XOFFSET, + PARAM_SENSOR_MAG_YOFFSET, + PARAM_SENSOR_MAG_ZOFFSET, + PARAM_ATT_XOFFSET, + PARAM_ATT_YOFFSET, + PARAM_RC1_MIN, + PARAM_RC1_MAX, + PARAM_RC1_TRIM, + PARAM_RC1_REV, + PARAM_RC2_MIN, + PARAM_RC2_MAX, + PARAM_RC2_TRIM, + PARAM_RC2_REV, + PARAM_RC3_MIN, + PARAM_RC3_MAX, + PARAM_RC3_TRIM, + PARAM_RC3_REV, + PARAM_RC4_MIN, + PARAM_RC4_MAX, + PARAM_RC4_TRIM, + PARAM_RC4_REV, + PARAM_RC5_MIN, + PARAM_RC5_MAX, + PARAM_RC5_TRIM, + PARAM_RC5_REV, + PARAM_RC6_MIN, + PARAM_RC6_MAX, + PARAM_RC6_TRIM, + PARAM_RC6_REV, + PARAM_RC7_MIN, + PARAM_RC7_MAX, + PARAM_RC7_TRIM, + PARAM_RC7_REV, + PARAM_RC8_MIN, + PARAM_RC8_MAX, + PARAM_RC8_TRIM, + PARAM_RC8_REV, + PARAM_THROTTLE_CHAN, + PARAM_ROLL_CHAN, + PARAM_PITCH_CHAN, + PARAM_YAW_CHAN, + PARAM_OVERRIDE_CHAN, + PARAM_SERVO1_MIN, + PARAM_SERVO1_MAX, + PARAM_SERVO1_TRIM, + PARAM_SERVO2_MIN, + PARAM_SERVO2_MAX, + PARAM_SERVO2_TRIM, + PARAM_SERVO3_MIN, + PARAM_SERVO3_MAX, + PARAM_SERVO3_TRIM, + PARAM_SERVO4_MIN, + PARAM_SERVO4_MAX, + PARAM_SERVO4_TRIM, + PARAM_SERVO_SCALE, + PARAM_MAX_COUNT ///< LEAVE THIS IN PLACE AS LAST ELEMENT! +}; + +struct px4_parameter_storage_t { + float param_values[PARAM_MAX_COUNT]; ///< Parameter values + const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names + bool param_needs_write[PARAM_MAX_COUNT]; + uint16_t next_param; + uint16_t size; +}; + + +#define PX4_FLIGHT_ENVIRONMENT_INDOOR 0 +#define PX4_FLIGHT_ENVIRONMENT_OUTDOOR 1 +#define PX4_FLIGHT_ENVIRONMENT_TESTING 2 //NO check for position fix in this environment + +struct global_data_parameter_storage_t { + + /* use of a counter and timestamp recommended (but not necessary) */ + +// uint16_t counter; //incremented by the writing thread everytime new data is stored +// uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data + + /* Actual data, this is specific to the type of data which is stored in this struct */ + + //***** Start: Add your variables here ***** + + /* Parameters (set by a param_set mavlink message */ + struct px4_parameter_storage_t pm; + + //*****END: Add your variables here ***** + +}; + +__attribute__ ((visibility ("default"))) extern struct global_data_parameter_storage_t *global_data_parameter_storage; //adjust this line! + +#endif diff --git a/apps/uORB/topics/UNPORTED_subsystem_info.h b/apps/uORB/topics/UNPORTED_subsystem_info.h new file mode 100644 index 000000000..24f973a02 --- /dev/null +++ b/apps/uORB/topics/UNPORTED_subsystem_info.h @@ -0,0 +1,68 @@ +/* Structure for storage of shared variables (extended( */ + +/* global_data_subsystem_info stores a buffer of the sensor info/status messages (sent by sensors or gps app), this is then handled by the commander. (The commander then writes to global_data_sys_status which is read by mavlink) */ + +/* (any app) --> global_data_subsystem_info (buffered) --> (commander app) --> global_data_sys_status --> (mavlink app) */ + +#ifndef GLOBAL_DATA_SUBSYSTEM_INFO_T_H_ //adjust this line! +#define GLOBAL_DATA_SUBSYSTEM_INFO_T_H_ //adjust this line! + +#define SUBSYSTEM_INFO_BUFFER_SIZE 10 + +#include "global_data_access.h" + +typedef enum //values correspond to bitmasks of mavlink message sys_status, this is crucial for the underlying bitmask to work +{ + SUBSYSTEM_TYPE_GYRO = 0, + SUBSYSTEM_TYPE_ACC = 1, + SUBSYSTEM_TYPE_MAG = 2, + SUBSYSTEM_TYPE_ABSPRESSURE = 3, + SUBSYSTEM_TYPE_DIFFPRESSURE = 4, + SUBSYSTEM_TYPE_GPS = 5, + SUBSYSTEM_TYPE_OPTICALFLOW = 6, + SUBSYSTEM_TYPE_CVPOSITION = 7, + SUBSYSTEM_TYPE_LASERPOSITION = 8, + SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 9, + SUBSYSTEM_TYPE_ANGULARRATECONTROL = 10, + SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 11, + SUBSYSTEM_TYPE_YAWPOSITION = 12, + SUBSYSTEM_TYPE_ALTITUDECONTROL = 13, + SUBSYSTEM_TYPE_POSITIONCONTROL = 14, + SUBSYSTEM_TYPE_MOTORCONTROL = 15 + +} subsystem_type_t; + +typedef struct +{ + uint8_t present; + uint8_t enabled; + uint8_t health; + + subsystem_type_t subsystem_type; + +} __attribute__((__packed__)) subsystem_info_t; + +typedef struct +{ + /* Struct which stores the access configuration, this is the same for all shared structs */ + + access_conf_t access_conf; //don't remove this line! + + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; //incremented by the writing thread everytime new data is stored + uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data + + /* Actual data, this is specific to the type of data which is stored in this struct */ + + //***** Start: Add your variables here ***** + subsystem_info_t info[SUBSYSTEM_INFO_BUFFER_SIZE]; + uint8_t current_info; + + //*****END: Add your variables here ***** + +} global_data_subsystem_info_t; //adjust this line! + +extern global_data_subsystem_info_t* global_data_subsystem_info; //adjust this line! + +#endif diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h new file mode 100644 index 000000000..03c0c7b7d --- /dev/null +++ b/apps/uORB/topics/actuator_controls.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 Actuator control topic - mixer inputs. + * + * Values published to this topic are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + */ + +#ifndef TOPIC_ACTUATOR_CONTROLS_H +#define TOPIC_ACTUATOR_CONTROLS_H + +#include <stdint.h> +#include "../uORB.h" + +#define NUM_ACTUATOR_CONTROLS 16 + +struct actuator_controls +{ + float control[NUM_ACTUATOR_CONTROLS]; +}; + +ORB_DECLARE(actuator_controls); + +struct actuator_armed +{ + bool armed; +}; + +ORB_DECLARE(actuator_armed); + +#endif
\ No newline at end of file diff --git a/apps/uORB/topics/ardrone_control.h b/apps/uORB/topics/ardrone_control.h new file mode 100644 index 000000000..1f582f52a --- /dev/null +++ b/apps/uORB/topics/ardrone_control.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file ardrone_control.c + * Definition of the ardrone_control uORB topic. + */ + +#ifndef TOPIC_ARDRONE_CONTROL_H_ +#define TOPIC_ARDRONE_CONTROL_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct ardrone_control_s +{ + uint16_t counter; /**< incremented by the publishing thread everytime new data is stored. */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data. */ + + float setpoint_rate_cast[3]; + float setpoint_thrust_cast; /**< LOGME */ + float setpoint_attitude_rate[3]; + float setpoint_attitude[3]; /**< LOGME */ + float attitude_control_output[3]; /**< roll, pitch, yaw. */ + float position_control_output[3]; /**< x, y, z. */ + float attitude_setpoint_navigationframe_from_positioncontroller[3]; /**< LOGME */ + float gyro_scaled[3]; + float gyro_filtered[3]; + float gyro_filtered_offset[3]; + float zcompensation; + uint16_t motor_front_nw; + uint16_t motor_right_ne; + uint16_t motor_back_se; + uint16_t motor_left_sw; + +}; /**< ardrone control status */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(ardrone_control); + +#endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/apps/uORB/topics/ardrone_motors_setpoint.h b/apps/uORB/topics/ardrone_motors_setpoint.h new file mode 100644 index 000000000..54d81b706 --- /dev/null +++ b/apps/uORB/topics/ardrone_motors_setpoint.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ardrone_motors_setpoint.h + * Definition of the ardrone_motors_setpoint uORB topic. + */ + +#ifndef TOPIC_ARDRONE_MOTORS_SETPOINT_H_ +#define TOPIC_ARDRONE_MOTORS_SETPOINT_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct ardrone_motors_setpoint_s +{ + uint16_t counter; //incremented by the writing thread everytime new data is stored + uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data + + uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration + uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration + uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration + uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration + +}; /**< AR.Drone low level motors */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(ardrone_motors_setpoint); + +#endif diff --git a/apps/uORB/topics/fixedwing_control.h b/apps/uORB/topics/fixedwing_control.h new file mode 100644 index 000000000..e904709b6 --- /dev/null +++ b/apps/uORB/topics/fixedwing_control.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fixedwing_control.h + * Definition of the fixedwing_control uORB topic. + */ + +#ifndef TOPIC_FIXEDWING_CONTROL_H_ +#define TOPIC_FIXEDWING_CONTROL_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * fixed wing control status. + */ +struct fixedwing_control_s +{ + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; //incremented by the writing thread everytime new data is stored + uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data + + float setpoint_rate_cast[3]; + float setpoint_attitude_rate[3]; + float setpoint_attitude[3]; + int16_t attitude_control_output[4]; /**< roll, pitch, yaw, throttle */ + float position_control_output[4]; + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(fixedwing_control); + +#endif //GLOBAL_DATA_FIXEDWING_CONTROL_T_H_ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h new file mode 100644 index 000000000..5bd2adeec --- /dev/null +++ b/apps/uORB/topics/rc_channels.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Nils Wenzler <wenzlern@student.ethz.ch> + * @author Ivan Ovinnikov <oivan@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_channels.h + * Definition of the rc_channels uORB topic. + */ + +#ifndef RC_CHANNELS_H_ +#define RC_CHANNELS_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +enum RC_CHANNELS_STATUS +{ + UNKNOWN = 0, + KNOWN = 1, + SIGNAL = 2, + TIMEOUT = 3 +}; + +/** + * This defines the mapping of the RC functions. + * The value assigned to the specific function corresponds to the entry of + * the channel array chan[]. + * Ex. To read out the scaled Pitch value: + * pitch = global_data_rc_channels->chan[PITCH].scale; + * The override is on channel 8, since we don't usually have a 12 channel RC + * and channel 5/6 (GRAUPNER/FUTABA) are mapped to the second ROLL servo, which + * can only be disabled on more advanced RC sets. + */ +enum RC_CHANNELS_FUNCTION +{ + THROTTLE = 0, + ROLL = 1, + PITCH = 2, + YAW = 3, + OVERRIDE = 4, + FUNC_0 = 5, + FUNC_1 = 6, + FUNC_2 = 7, + FUNC_3 = 8, + FUNC_4 = 9, + FUNC_5 = 10, + FUNC_6 = 11, + RC_CHANNELS_FUNCTION_MAX = 12 +}; + +struct rc_channels_s { + + uint64_t timestamp; /**< In microseconds since boot time. */ + uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ + struct { + uint16_t mid; /**< midpoint (0). */ + float scaling_factor; /**< scaling factor from raw counts to 0..1 */ + uint16_t raw; /**< current raw value */ + int16_t scale; + float scaled; /**< Scaled */ + uint16_t override; + enum RC_CHANNELS_STATUS status; /**< status of the channel */ + } chan[RC_CHANNELS_FUNCTION_MAX]; + uint8_t chan_count; /**< maximum number of valid channels */ + + /*String array to store the names of the functions*/ + const char function_name[RC_CHANNELS_FUNCTION_MAX][20]; + uint8_t function[RC_CHANNELS_FUNCTION_MAX]; + uint8_t rssi; /**< Overall receive signal strength */ +}; /**< radio control channels. */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(rc_channels); + +#endif diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h new file mode 100644 index 000000000..ecf5ea81d --- /dev/null +++ b/apps/uORB/topics/sensor_combined.h @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sensor_combined.h + * Definition of the sensor_combined uORB topic. + */ + +#ifndef SENSOR_COMBINED_H_ +#define SENSOR_COMBINED_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Sensor readings in raw and SI-unit form. + * + * These values are read from the sensors. Raw values are in sensor-specific units, + * the scaled values are in SI-units, as visible from the ending of the variable + * or the comments. The use of the SI fields is in general advised, as these fields + * are scaled and offset-compensated where possible and do not change with board + * revisions and sensor updates. + * + */ +struct sensor_combined_s { + + /* + * Actual data, this is specific to the type of data which is stored in this struct + * A line containing L0GME will be added by the Python logging code generator to the + * logged dataset. + */ + + /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ + + uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */ + + int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */ + uint16_t gyro_raw_counter; /**< Number of raw measurments taken LOGME */ + float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */ + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */ + uint16_t accelerometer_raw_counter; /**< Number of raw acc measurements taken LOGME */ + float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */ + int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame LOGME */ + float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss LOGME */ + uint16_t magnetometer_raw_counter; /**< Number of raw mag measurements taken LOGME */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */ + float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */ + float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */ + float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */ + float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */ + uint16_t baro_raw_counter; /**< Number of raw baro measurements taken LOGME */ + uint16_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */ + bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(sensor_combined); + +#endif diff --git a/apps/uORB/topics/vehicle_attitude.h b/apps/uORB/topics/vehicle_attitude.h new file mode 100644 index 000000000..e365a6557 --- /dev/null +++ b/apps/uORB/topics/vehicle_attitude.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_attitude.h + * Definition of the attitude uORB topic. + */ + +#ifndef VEHICLE_ATTITUDE_H_ +#define VEHICLE_ATTITUDE_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Attitude in NED body frame in SI units. + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct vehicle_attitude_s { + + /* + * Actual data, this is specific to the type of data which is stored in this struct + * A line containing L0GME will be added by the Python logging code generator to the + * logged dataset. + */ + uint64_t timestamp; /**< in microseconds since system start */ + + /* This is similar to the mavlink message ATTITUDE, but for onboard use */ + + /* @warning Roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */ + + float roll; /**< Roll angle (rad, Tait-Bryan, NED) LOGME */ + float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) LOGME */ + float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) LOGME */ + float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) LOGME */ + float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) LOGME */ + float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) LOGME */ + float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ + float q[4]; /**< Quaternion (NED) */ + bool R_valid; /**< Rotation matrix valid */ + bool q_valid; /**< Quaternion valid */ + uint16_t counter; /**< Counter of all attitude messages (wraps) */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_attitude); + +#endif diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h new file mode 100644 index 000000000..6b3757402 --- /dev/null +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_attitude_setpoint.h + * Definition of the vehicle attitude setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ +#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ + +#include <stdint.h> +#include <stdbool.h> +#include <uORB.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * vehicle attitude setpoint. + */ +struct vehicle_attitude_setpoint_s +{ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */ + float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */ + float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ + float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ + + float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + bool R_valid; /**< Set to true if rotation matrix is valid */ + float thrust; /**< Thrust in Newton the power system should generate */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_attitude_setpoint); + +#endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/apps/uORB/topics/vehicle_command.h b/apps/uORB/topics/vehicle_command.h new file mode 100644 index 000000000..dca928efd --- /dev/null +++ b/apps/uORB/topics/vehicle_command.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_command.h + * Definition of the vehicle command uORB topic. + */ + +#ifndef TOPIC_VEHICLE_COMMAND_H_ +#define TOPIC_VEHICLE_COMMAND_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_command_s +{ + float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */ + float param2; /**< Parameter 2, as defined by MAVLink MAV_CMD enum. */ + float param3; /**< Parameter 3, as defined by MAVLink MAV_CMD enum. */ + float param4; /**< Parameter 4, as defined by MAVLink MAV_CMD enum. */ + float param5; /**< Parameter 5, as defined by MAVLink MAV_CMD enum. */ + float param6; /**< Parameter 6, as defined by MAVLink MAV_CMD enum. */ + float param7; /**< Parameter 7, as defined by MAVLink MAV_CMD enum. */ + uint16_t command; /**< Command ID, as defined MAVLink by MAV_CMD enum. */ + uint8_t target_system; /**< System which should execute the command */ + uint8_t target_component; /**< Component which should execute the command, 0 for all components */ + uint8_t source_system; /**< System sending the command */ + uint8_t source_component; /**< Component sending the command */ + uint8_t confirmation; /**< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) */ +}; /**< command sent to vehicle */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_command); + + + +#endif diff --git a/apps/uORB/topics/vehicle_global_position.h b/apps/uORB/topics/vehicle_global_position.h new file mode 100644 index 000000000..f036c7223 --- /dev/null +++ b/apps/uORB/topics/vehicle_global_position.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_position.h + * Definition of the global fused WGS84 position uORB topic. + */ + +#ifndef VEHICLE_GLOBAL_POSITION_T_H_ +#define VEHICLE_GLOBAL_POSITION_T_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + + /** + * Fused global position in WGS84. + * + * This struct contains the system's believ about its position. It is not the raw GPS + * measurement (@see vehicle_gps_position). This topic is usually published by the position + * estimator, which will take more sources of information into account than just GPS, + * e.g. control inputs of the vehicle in a Kalman-filter implementation. + */ +struct vehicle_global_position_s +{ + uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + bool valid; /**< true if position satisfies validity criteria of estimator */ + + int32_t lat; /**< Latitude in 1E7 degrees LOGME */ + int32_t lon; /**< Longitude in 1E7 degrees LOGME */ + float alt; /**< Altitude in meters LOGME */ + float relative_alt; /**< Altitude above home position in meters, LOGME */ + float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */ + float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */ + float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */ + float hdg; /**< Compass heading in radians -PI..+PI. */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_position); + +#endif diff --git a/apps/uORB/topics/vehicle_global_position_setpoint.h b/apps/uORB/topics/vehicle_global_position_setpoint.h new file mode 100644 index 000000000..b73e2a363 --- /dev/null +++ b/apps/uORB/topics/vehicle_global_position_setpoint.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_position_setpoint.h + * Definition of the global WGS84 position setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ +#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Global position setpoint in WGS84 coordinates. + * + * This is the position the MAV is heading towards. If it of type loiter, + * the MAV is circling around it with the given loiter radius in meters. + */ +struct vehicle_global_position_setpoint_s +{ + bool altitude_is_relative; /**< true if altitude is relative from start point */ + int32_t lat; /**< latitude in degrees * 1E7 */ + int32_t lon; /**< longitude in degrees * 1E7 */ + float altitude; /**< altitude in meters */ + float yaw; /**< in radians NED -PI..+PI */ + float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ + bool is_loiter; /**< true if loitering is enabled */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_position_setpoint); + +#endif diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h new file mode 100644 index 000000000..60d01a831 --- /dev/null +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_gps_position.h + * Definition of the GPS WGS84 uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GPS_H_ +#define TOPIC_VEHICLE_GPS_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GPS position in WGS84 coordinates. + */ +struct vehicle_gps_position_s +{ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + uint32_t counter; /**< Count of GPS messages */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + + int32_t lat; /**< Latitude in 1E7 degrees //LOGME */ + int32_t lon; /**< Longitude in 1E7 degrees //LOGME */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL //LOGME */ + uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */ + uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */ + uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ + uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */ + uint16_t cog; /**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ + + uint8_t satellite_prn[20]; /**< Global satellite ID */ + uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ + uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ + uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_gps_position); + +#endif diff --git a/apps/uORB/topics/vehicle_local_position.h b/apps/uORB/topics/vehicle_local_position.h new file mode 100644 index 000000000..76eddeacd --- /dev/null +++ b/apps/uORB/topics/vehicle_local_position.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_local_position.h + * Definition of the local fused NED position uORB topic. + */ + +#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ +#define TOPIC_VEHICLE_LOCAL_POSITION_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Fused local position in NED. + */ +struct vehicle_local_position_s +{ + uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ + bool valid; /**< true if position satisfies validity criteria of estimator */ + + float x; /**< X positin in meters in NED earth-fixed frame */ + float y; /**< X positin in meters in NED earth-fixed frame */ + float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ + float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ + float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ + float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ + float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ + float hdg; /**< Compass heading in radians -PI..+PI. */ + + // TODO Add covariances here + + /* Reference position in GPS / WGS84 frame */ + uint64_t home_timestamp;/**< Time when home position was set */ + int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ + int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ + float home_alt; /**< Altitude in meters LOGME */ + float home_hdg; /**< Compass heading in radians -PI..+PI. */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_local_position); + +#endif diff --git a/apps/uORB/topics/vehicle_local_position_setpoint.h b/apps/uORB/topics/vehicle_local_position_setpoint.h new file mode 100644 index 000000000..d24d81e3a --- /dev/null +++ b/apps/uORB/topics/vehicle_local_position_setpoint.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_local_position_setpoint.h + * Definition of the local NED position setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ +#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_local_position_setpoint_s +{ + float x; /**< in meters NED */ + float y; /**< in meters NED */ + float z; /**< in meters NED */ + float yaw; /**< in radians NED -PI..+PI */ +}; /**< Local position in NED frame to go to */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_local_position_setpoint); + +#endif diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h new file mode 100644 index 000000000..d92b80046 --- /dev/null +++ b/apps/uORB/topics/vehicle_status.h @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_status.h + * Definition of the vehicle_status uORB topic. + * + * Published the state machine and the system status bitfields + * (see SYS_STATUS mavlink message), published only by commander app. + * + * All apps should write to subsystem_info: + * + * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) + */ + +#ifndef VEHICLE_STATUS_H_ +#define VEHICLE_STATUS_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + +/* State Machine */ +typedef enum +{ + SYSTEM_STATE_PREFLIGHT = 0, + SYSTEM_STATE_STANDBY = 1, + SYSTEM_STATE_GROUND_READY = 2, + SYSTEM_STATE_MANUAL = 3, + SYSTEM_STATE_STABILIZED = 4, + SYSTEM_STATE_AUTO = 5, + SYSTEM_STATE_MISSION_ABORT = 6, + SYSTEM_STATE_EMCY_LANDING = 7, + SYSTEM_STATE_EMCY_CUTOFF = 8, + SYSTEM_STATE_GROUND_ERROR = 9, + SYSTEM_STATE_REBOOT= 10, + +} commander_state_machine_t; + +enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, + VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 +}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +struct vehicle_status_s +{ + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + commander_state_machine_t state_machine; + uint8_t mode; + + bool control_manual_enabled; /**< true if manual input is mixed in */ + bool control_rates_enabled; /**< true if rates are stabilized */ + bool control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool control_speed_enabled; /**< true if speed (implies direction) is controlled */ + bool control_position_enabled; /**< true if position is controlled */ + + bool preflight_gyro_calibration; /**< true if gyro calibration is requested */ + bool preflight_mag_calibration; /**< true if mag calibration is requested */ + + /* see SYS_STATUS mavlink message for the following */ + uint32_t onboard_control_sensors_present; + uint32_t onboard_control_sensors_enabled; + uint32_t onboard_control_sensors_health; + uint16_t load; + uint16_t voltage_battery; + int16_t current_battery; + int8_t battery_remaining; + uint16_t drop_rate_comm; + uint16_t errors_comm; + uint16_t errors_count1; + uint16_t errors_count2; + uint16_t errors_count3; + uint16_t errors_count4; + +// bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */ + bool gps_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_status); + +#endif diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp new file mode 100644 index 000000000..270e3861c --- /dev/null +++ b/apps/uORB/uORB.cpp @@ -0,0 +1,969 @@ +/**************************************************************************** + * + * 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 A lightweight object broker. + */ + +#include <nuttx/config.h> + +#include <drivers/device/device.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <string.h> +#include <stdlib.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/up_hrt.h> + +#include <drivers/drv_orb_dev.h> + +#include "uORB.h" + +/** + * Utility functions. + */ +namespace +{ + +static const unsigned orb_maxpath = 64; + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +enum Flavor { + PUBSUB, + PARAM +}; + +int +node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) +{ + unsigned len; + + len = snprintf(buf, orb_maxpath, "/%s/%s", + (f == PUBSUB) ? "obj" : "param", + meta->o_name); + + if (len >= orb_maxpath) + return -ENAMETOOLONG; + + return OK; +} + +} + +/** + * Per-object device instance. + */ +class ORBDevNode : public device::CDev +{ +public: + ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path); + ~ORBDevNode(); + + virtual int open(struct file *filp); + virtual int close(struct file *filp); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +protected: + virtual pollevent_t poll_state(struct file *filp); + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + +private: + struct SubscriberData { + unsigned generation; /**< last generation the subscriber has seen */ + unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ + void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ + bool update_reported; /**< true if we have reported the update via poll/check */ + }; + + const struct orb_metadata *_meta; /**< object metadata information */ + uint8_t *_data; /**< allocated object buffer */ + hrt_abstime _last_update; /**< time the object was last updated */ + volatile unsigned _generation; /**< object generation count */ + pid_t _publisher; /**< if nonzero, current publisher */ + + SubscriberData *filp_to_sd(struct file *filp) { + SubscriberData *sd = (SubscriberData *)(filp->f_priv); + return sd; + } + + /** + * Perform a deferred update for a rate-limited subscriber. + */ + void update_deferred(); + + /** + * Bridge from hrt_call to update_deferred + * + * void *arg ORBDevNode pointer for which the deferred update is performed. + */ + static void update_deferred_trampoline(void *arg); + + /** + * Check whether a topic appears updated to a subscriber. + * + * @param sd The subscriber for whom to check. + * @return True if the topic should appear updated to the subscriber + */ + bool appears_updated(SubscriberData *sd); +}; + +ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) : + CDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0) +{ + // enable debug() calls + _debug_enabled = true; +} + +ORBDevNode::~ORBDevNode() +{ + if (_data != nullptr) + delete[] _data; +} + +int +ORBDevNode::open(struct file *filp) +{ + int ret; + + /* is this a publisher? */ + if (filp->f_oflags == O_WRONLY) { + + /* become the publisher if we can */ + lock(); + + if (_publisher == 0) { + _publisher = getpid(); + ret = OK; + + } else { + ret = -EBUSY; + } + + unlock(); + + /* now complete the open */ + if (ret == OK) { + ret = CDev::open(filp); + + /* open failed - not the publisher anymore */ + if (ret != OK) + _publisher = 0; + } + + return ret; + } + + /* is this a new subscriber? */ + if (filp->f_oflags == O_RDONLY) { + + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; + + if (nullptr == sd) + return -ENOMEM; + + memset(sd, 0, sizeof(*sd)); + + /* default to no pending update */ + sd->generation = _generation; + + filp->f_priv = (void *)sd; + + ret = CDev::open(filp); + + if (ret != OK) + free(sd); + + return ret; + } + + /* can only be pub or sub, not both */ + return -EINVAL; +} + +int +ORBDevNode::close(struct file *filp) +{ + /* is this the publisher closing? */ + if (getpid() == _publisher) { + _publisher = 0; + + } else { + SubscriberData *sd = filp_to_sd(filp); + + if (sd != nullptr) + delete sd; + } + + return CDev::close(filp); +} + +ssize_t +ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) +{ + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + + /* if the object has not been written yet, return zero */ + if (_data == nullptr) + return 0; + + /* if the caller's buffer is the wrong size, that's an error */ + if (buflen != _meta->o_size) + return -EIO; + + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + /* if the caller doesn't want the data, don't give it to them */ + if (nullptr != buffer) + memcpy(buffer, _data, _meta->o_size); + + /* track the last generation that the file has seen */ + sd->generation = _generation; + + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; + + irqrestore(flags); + + return _meta->o_size; +} + +ssize_t +ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) +{ + /* + * Writes are legal from interrupt context as long as the + * object has already been initialised from thread context. + * + * Writes outside interrupt context will allocate the object + * if it has not yet been allocated. + */ + if (nullptr == _data) { + if (!up_interrupt_context()) { + + lock(); + + /* re-check size */ + if (nullptr == _data) + _data = new uint8_t[_meta->o_size]; + + unlock(); + } + + /* failed or could not allocate */ + if (nullptr == _data) + return -ENOMEM; + } + + /* If write size does not match, that is an error */ + if (_meta->o_size != buflen) + return -EIO; + + /* Perform an atomic copy. */ + irqstate_t flags = irqsave(); + memcpy(_data, buffer, _meta->o_size); + irqrestore(flags); + + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; + + /* notify any poll waiters */ + poll_notify(POLLIN); + + return _meta->o_size; +} + +int +ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + SubscriberData *sd = filp_to_sd(filp); + + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return OK; + + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return OK; + + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +pollevent_t +ORBDevNode::poll_state(struct file *filp) +{ + SubscriberData *sd = filp_to_sd(filp); + + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) + return POLLIN; + + return 0; +} + +void +ORBDevNode::poll_notify_one(struct pollfd *fds, pollevent_t events) +{ + SubscriberData *sd = filp_to_sd((struct file *)fds->priv); + + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) + CDev::poll_notify_one(fds, events); +} + +bool +ORBDevNode::appears_updated(SubscriberData *sd) +{ + /* assume it doesn't look updated */ + bool ret = false; + + /* avoid racing between interrupt and non-interrupt context calls */ + irqstate_t state = irqsave(); + + /* + * If the subscriber's generation count matches the update generation + * count, there has been no update from their perspective; if they + * don't match then we might have a visible update. + */ + while (sd->generation != _generation) { + + /* + * Handle non-rate-limited subscribers. + */ + if (sd->update_interval == 0) { + ret = true; + break; + } + + /* + * If we have previously told the subscriber that there is data, + * and they have not yet collected it, continue to tell them + * that there has been an update. This mimics the non-rate-limited + * behaviour where checking / polling continues to report an update + * until the topic is read. + */ + if (sd->update_reported) { + ret = true; + break; + } + + /* + * If the interval timer is still running, the topic should not + * appear updated, even though at this point we know that it has. + * We have previously been through here, so the subscriber + * must have collected the update we reported, otherwise + * update_reported would still be true. + */ + if (!hrt_called(&sd->update_call)) + break; + + /* + * Make sure that we don't consider the topic to be updated again + * until the interval has passed once more by restarting the interval + * timer and thereby re-scheduling a poll notification at that time. + */ + hrt_call_after(&sd->update_call, + sd->update_interval, + &ORBDevNode::update_deferred_trampoline, + (void *)this); + + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; + + break; + } + + irqrestore(state); + + /* consider it updated */ + return ret; +} + +void +ORBDevNode::update_deferred() +{ + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); +} + +void +ORBDevNode::update_deferred_trampoline(void *arg) +{ + ORBDevNode *node = (ORBDevNode *)arg; + + node->update_deferred(); +} + +/** + * Master control device for ObjDev. + * + * Used primarily to create new objects via the ORBIOCCREATE + * ioctl. + */ +class ORBDevMaster : public device::CDev +{ +public: + ORBDevMaster(Flavor f); + ~ORBDevMaster(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); +private: + Flavor _flavor; +}; + +ORBDevMaster::ORBDevMaster(Flavor f) : + CDev((f == PUBSUB) ? "obj_master" : "param_master", + (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), + _flavor(f) +{ + // enable debug() calls + _debug_enabled = true; + +} + +ORBDevMaster::~ORBDevMaster() +{ +} + +int +ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret; + + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_metadata *meta = (const struct orb_metadata *)arg; + const char *objname; + char nodepath[orb_maxpath]; + ORBDevNode *node; + + /* construct a path to the node - this also checks the node name */ + ret = node_mkpath(nodepath, _flavor, meta); + + if (ret != OK) + return ret; + + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) + return -ENOMEM; + + /* construct the new node */ + node = new ORBDevNode(meta, objname, nodepath); + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + if (node != nullptr) + ret = node->init(); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) + return -ENOMEM; + + /* if init failed, discard the node and its name */ + if (ret != OK) { + delete node; + free((void *)objname); + } + + return ret; + } + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + + +/** + * Local functions in support of the shell command. + */ + +namespace +{ + +ORBDevMaster *g_dev; + +struct orb_test { + int val; +}; + +ORB_DEFINE(orb_test, struct orb_test); + +int +test_fail(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "FAIL: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return ERROR; +} + +int +test_note(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "note: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return OK; +} + +ORB_DECLARE(sensor_combined); + +int +test() +{ + struct orb_test t, u; + int pfd, sfd; + bool updated; + + t.val = 0; + pfd = orb_advertise(ORB_ID(orb_test), &t); + + if (pfd < 0) + return test_fail("advertise failed: %d", errno); + + test_note("publish fd %d", pfd); + sfd = orb_subscribe(ORB_ID(orb_test)); + + if (sfd < 0) + return test_fail("subscribe failed: %d", errno); + + test_note("subscribe fd %d", sfd); + u.val = 1; + + if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + return test_fail("copy(1) failed: %d", errno); + + if (u.val != t.val) + return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + + if (OK != orb_check(sfd, &updated)) + return test_fail("check(1) failed"); + + if (updated) + return test_fail("spurious updated flag"); + + t.val = 2; + test_note("try publish"); + + if (OK != orb_publish(ORB_ID(orb_test), pfd, &t)) + return test_fail("publish failed"); + + if (OK != orb_check(sfd, &updated)) + return test_fail("check(2) failed"); + + if (!updated) + return test_fail("missing updated flag"); + + if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + return test_fail("copy(2) failed: %d", errno); + + if (u.val != t.val) + return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); + + orb_unsubscribe(sfd); + close(pfd); + +#if 0 + /* this is a hacky test that exploits the sensors app to test rate-limiting */ + + sfd = orb_subscribe(ORB_ID(sensor_combined)); + + hrt_abstime start, end; + unsigned count; + + start = hrt_absolute_time(); + count = 0; + + do { + orb_check(sfd, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sfd, nullptr); + count++; + } + } while (count < 100); + + end = hrt_absolute_time(); + test_note("full-speed, 100 updates in %llu", end - start); + + orb_set_interval(sfd, 10); + + start = hrt_absolute_time(); + count = 0; + + do { + orb_check(sfd, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sfd, nullptr); + count++; + } + } while (count < 100); + + end = hrt_absolute_time(); + test_note("100Hz, 100 updates in %llu", end - start); + + orb_unsubscribe(sfd); +#endif + + return test_note("PASS"); +} + +int +info() +{ + return OK; +} + + +} // namespace + +/* + * uORB server 'main'. + */ +extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } + +int +uorb_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + fprintf(stderr, "[uorb] already loaded\n"); + return -EBUSY; + } + + /* create the driver */ + g_dev = new ORBDevMaster(PUBSUB); + + if (g_dev == nullptr) { + fprintf(stderr, "[uorb] driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "[uorb] driver init failed\n"); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + printf("[uorb] ready\n"); + return OK; + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + return test(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + return info(); + + fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n"); + return -EINVAL; +} + +/* + * Library functions. + */ +namespace +{ + +void debug(const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + usleep(100000); +} + +/** + * Advertise a node; don't consider it an error if the node has + * already been advertised. + * + * @todo verify that the existing node is the same as the one + * we tried to advertise. + */ +int +node_advertise(const struct orb_metadata *meta) +{ + int fd = -1; + int ret = ERROR; + + /* open the control device */ + fd = open(TOPIC_MASTER_DEVICE_PATH, 0); + + if (fd < 0) + goto out; + + /* advertise the object */ + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta); + + /* it's OK if it already exists */ + if ((OK != ret) && (EEXIST == errno)) + ret = OK; + +out: + + if (fd >= 0) + close(fd); + + return ret; +} + +/** + * Common implementation for orb_advertise and orb_subscribe. + * + * Handles creation of the object and the initial publication for + * advertisers. + */ +int +node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser) +{ + char path[orb_maxpath]; + int fd, ret; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return ERROR; + } + + /* + * Advertiser must publish an initial value. + */ + if (advertiser && (data == nullptr)) { + errno = EINVAL; + return ERROR; + } + + /* + * Generate the path to the node and try to open it. + */ + ret = node_mkpath(path, f, meta); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + + /* we may need to advertise the node... */ + if (fd < 0) { + + /* try to create the node */ + ret = node_advertise(meta); + + /* on success, try the open again */ + if (ret == OK) + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + } + + if (fd < 0) { + errno = EIO; + return ERROR; + } + + /* the advertiser must perform an initial publish to initialise the object */ + if (advertiser) { + ret = orb_publish(meta, fd, data); + + if (ret != OK) { + /* save errno across the close */ + ret = errno; + close(fd); + errno = ret; + return ERROR; + } + } + + /* everything has been OK, we can return the handle now */ + return fd; +} + +} // namespace + +int +orb_advertise(const struct orb_metadata *meta, const void *data) +{ + return node_open(PUBSUB, meta, data, true); +} + +int +orb_subscribe(const struct orb_metadata *meta) +{ + return node_open(PUBSUB, meta, nullptr, false); +} + +int +orb_unsubscribe(int handle) +{ + return close(handle); +} + +int +orb_publish(const struct orb_metadata *meta, int handle, const void *data) +{ + int ret; + + ret = write(handle, data, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + +int +orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +{ + int ret; + + ret = read(handle, buffer, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + +int +orb_check(int handle, bool *updated) +{ + return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); +} + +int +orb_stat(int handle, uint64_t *time) +{ + return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); +} + +int +orb_set_interval(int handle, unsigned interval) +{ + return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); +} + diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h new file mode 100644 index 000000000..c36d0044f --- /dev/null +++ b/apps/uORB/uORB.h @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef _UORB_UORB_H +#define _UORB_UORB_H + +/** + * @file API for the uORB lightweight object broker. + */ + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> + // Hack until parameter storage is cleaned up +#include "parameter_storage.h" + +// Hack until everything is using this header +#include <systemlib/visibility.h> + +/** + * Object metadata. + */ +struct orb_metadata { + const char *o_name; /**< unique object name */ + const size_t o_size; /**< object size */ +}; + +/** + * Generates a pointer to the uORB metadata structure for + * a given topic. + * + * The topic must have been declared previously in scope + * with ORB_DECLARE(). + * + * @param _name The name of the topic. + */ +#define ORB_ID(_name) &__orb_##_name + +/** + * Declare (prototype) the uORB metadata for a topic. + * + * Note that optional topics are declared weak; this allows a potential + * subscriber to attempt to subscribe to a topic that is not known to the + * system at runtime. The ORB_ID() macro will return NULL/nullptr for + * such a topic, and attempts to advertise or subscribe to it will + * return -1/ENOENT (see below). + * + * @param _name The name of the topic. + */ +#if defined(__cplusplus) +# define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT +# define ORB_DECLARE_OPTIONAL(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak)) +#else +# define ORB_DECLARE(_name) extern const struct orb_metadata __orb_##_name __EXPORT +# define ORB_DECLARE_OPTIONAL(_name) extern const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak)) +#endif + +/** + * Define (instantiate) the uORB metadata for a topic. + * + * The uORB metadata is used to help ensure that updates and + * copies are accessing the right data. + * + * Note that there must be no more than one instance of this macro + * for each topic. + * + * @param _name The name of the topic. + * @param _struct The structure the topic provides. + */ +#define ORB_DEFINE(_name, _struct) \ + const struct orb_metadata __orb_##_name = { \ + #_name, \ + sizeof(_struct) \ + }; struct hack + +__BEGIN_DECLS + +/** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern int orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; + +/** + * Publish new data to a topic. + * + * The data is atomically published to the topic and any waiting subscribers + * will be notified. Subscribers that are not waiting can check the topic + * for updates using orb_check and/or orb_stat. + * + * @handle The handle returned from orb_advertise. + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the data to be published. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_publish(const struct orb_metadata *meta, int handle, const void *data) __EXPORT; + +/** + * Subscribe to a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; + +/** + * Unsubscribe from a topic. + * + * @param handle A handle returned from orb_subscribe. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_unsubscribe(int handle) __EXPORT; + +/** + * Fetch data from a topic. + * + * This is the only operation that will reset the internal marker that + * indicates that a topic has been updated for a subscriber. Once poll + * or check return indicating that an updaet is available, this call + * must be used to update the subscription. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param handle A handle returned from orb_subscribe. + * @param buffer Pointer to the buffer receiving the data, or NULL + * if the caller wants to clear the updated flag without + * using the data. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) __EXPORT; + +/** + * Check whether a topic has been published to since the last orb_copy. + * + * This check can be used to determine whether to copy the topic when + * not using poll(), or to avoid the overhead of calling poll() when the + * topic is likely to have updated. + * + * Updates are tracked on a per-handle basis; this call will continue to + * return true until orb_copy is called using the same handle. This interface + * should be preferred over calling orb_stat due to the race window between + * stat and copy that can lead to missed updates. + * + * @param handle A handle returned from orb_subscribe. + * @param updated Set to true if the topic has been updated since the + * last time it was copied using this handle. + * @return OK if the check was successful, ERROR otherwise with + * errno set accordingly. + */ +extern int orb_check(int handle, bool *updated) __EXPORT; + +/** + * Return the last time that the topic was updated. + * + * @param handle A handle returned from orb_subscribe. + * @param time Returns the absolute time that the topic was updated, or zero if it has + * never been updated. Time is measured in microseconds. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_stat(int handle, uint64_t *time) __EXPORT; + +/** + * Set the minimum interval between which updates are seen for a subscription. + * + * If this interval is set, the subscriber will not see more than one update + * within the period. + * + * Specifically, the first time an update is reported to the subscriber a timer + * is started. The update will continue to be reported via poll and orb_check, but + * once fetched via orb_copy another update will not be reported until the timer + * expires. + * + * This feature can be used to pace a subscriber that is watching a topic that + * would otherwise update too quickly. + * + * @param handle A handle returned from orb_subscribe. + * @param interval An interval period in milliseconds. + * @return OK on success, ERROR otherwise with ERRNO set accordingly. + */ +extern int orb_set_interval(int handle, unsigned interval) __EXPORT; + +__END_DECLS + +#endif /* _UORB_UORB_H */ |