From 69bebbcaf827113a6a7fd7d90aad34c79d820e6d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 18 Dec 2012 15:46:19 +1100 Subject: [PATCH] SITL: removed old SITL code --- libraries/Desktop/Desktop.mk | 446 ----- libraries/Desktop/Makefile.desktop | 27 - libraries/Desktop/README | 33 - libraries/Desktop/include/HardwareSerial.h | 76 - libraries/Desktop/include/Print.h | 66 - libraries/Desktop/include/SPI.h | 70 - libraries/Desktop/include/Stream.h | 16 - libraries/Desktop/include/WConstants.h | 1 - libraries/Desktop/include/WProgram.h | 29 - libraries/Desktop/include/WString.h | 112 -- libraries/Desktop/include/Wire.h | 67 - libraries/Desktop/include/avr/eeprom.h | 24 - libraries/Desktop/include/avr/interrupt.h | 14 - libraries/Desktop/include/avr/io.h | 25 - libraries/Desktop/include/avr/iom2560.h | 94 -- libraries/Desktop/include/avr/iomxx0_1.h | 1563 ------------------ libraries/Desktop/include/avr/pgmspace.h | 57 - libraries/Desktop/include/avr/wdt.h | 4 - libraries/Desktop/include/avr/wiring.h | 0 libraries/Desktop/include/binary.h | 515 ------ libraries/Desktop/include/pins_arduino.h | 88 - libraries/Desktop/include/wiring.h | 136 -- libraries/Desktop/support/Arduino.cpp | 214 --- libraries/Desktop/support/DataFlash_APM1.cpp | 125 -- libraries/Desktop/support/FastSerial.cpp | 378 ----- libraries/Desktop/support/Print.cpp | 220 --- libraries/Desktop/support/SPI.cpp | 61 - libraries/Desktop/support/WMath.cpp | 60 - libraries/Desktop/support/WString.cpp | 443 ----- libraries/Desktop/support/desktop.h | 41 - libraries/Desktop/support/digital.c | 22 - libraries/Desktop/support/eeprom.c | 87 - libraries/Desktop/support/main.cpp | 115 -- libraries/Desktop/support/sitl.cpp | 367 ---- libraries/Desktop/support/sitl_adc.cpp | 151 -- libraries/Desktop/support/sitl_adc.h | 78 - libraries/Desktop/support/sitl_barometer.cpp | 46 - libraries/Desktop/support/sitl_compass.cpp | 74 - libraries/Desktop/support/sitl_gps.cpp | 243 --- libraries/Desktop/support/sitl_rc.h | 52 - libraries/Desktop/support/util.cpp | 53 - libraries/Desktop/support/util.h | 17 - 42 files changed, 6310 deletions(-) delete mode 100644 libraries/Desktop/Desktop.mk delete mode 100644 libraries/Desktop/Makefile.desktop delete mode 100644 libraries/Desktop/README delete mode 100644 libraries/Desktop/include/HardwareSerial.h delete mode 100644 libraries/Desktop/include/Print.h delete mode 100644 libraries/Desktop/include/SPI.h delete mode 100644 libraries/Desktop/include/Stream.h delete mode 100644 libraries/Desktop/include/WConstants.h delete mode 100644 libraries/Desktop/include/WProgram.h delete mode 100644 libraries/Desktop/include/WString.h delete mode 100644 libraries/Desktop/include/Wire.h delete mode 100644 libraries/Desktop/include/avr/eeprom.h delete mode 100644 libraries/Desktop/include/avr/interrupt.h delete mode 100644 libraries/Desktop/include/avr/io.h delete mode 100644 libraries/Desktop/include/avr/iom2560.h delete mode 100644 libraries/Desktop/include/avr/iomxx0_1.h delete mode 100644 libraries/Desktop/include/avr/pgmspace.h delete mode 100644 libraries/Desktop/include/avr/wdt.h delete mode 100644 libraries/Desktop/include/avr/wiring.h delete mode 100644 libraries/Desktop/include/binary.h delete mode 100644 libraries/Desktop/include/pins_arduino.h delete mode 100644 libraries/Desktop/include/wiring.h delete mode 100644 libraries/Desktop/support/Arduino.cpp delete mode 100644 libraries/Desktop/support/DataFlash_APM1.cpp delete mode 100644 libraries/Desktop/support/FastSerial.cpp delete mode 100644 libraries/Desktop/support/Print.cpp delete mode 100644 libraries/Desktop/support/SPI.cpp delete mode 100644 libraries/Desktop/support/WMath.cpp delete mode 100644 libraries/Desktop/support/WString.cpp delete mode 100644 libraries/Desktop/support/desktop.h delete mode 100644 libraries/Desktop/support/digital.c delete mode 100644 libraries/Desktop/support/eeprom.c delete mode 100644 libraries/Desktop/support/main.cpp delete mode 100644 libraries/Desktop/support/sitl.cpp delete mode 100644 libraries/Desktop/support/sitl_adc.cpp delete mode 100644 libraries/Desktop/support/sitl_adc.h delete mode 100644 libraries/Desktop/support/sitl_barometer.cpp delete mode 100644 libraries/Desktop/support/sitl_compass.cpp delete mode 100644 libraries/Desktop/support/sitl_gps.cpp delete mode 100644 libraries/Desktop/support/sitl_rc.h delete mode 100644 libraries/Desktop/support/util.cpp delete mode 100644 libraries/Desktop/support/util.h diff --git a/libraries/Desktop/Desktop.mk b/libraries/Desktop/Desktop.mk deleted file mode 100644 index d52cf86158..0000000000 --- a/libraries/Desktop/Desktop.mk +++ /dev/null @@ -1,446 +0,0 @@ -# -# Copyright (c) 2010 Andrew Tridgell. All rights reserved. -# based on Arduino.mk, Copyright (c) 2010 Michael Smith -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS -# OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY -# OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF -# SUCH DAMAGE. -# - -# -# Save the system type for later use. -# -SYSTYPE := $(shell uname) - -# force LANG to C so awk works sanely on MacOS -export LANG=C - -# -# Locate the sketch sources based on the initial Makefile's path -# -SRCROOT := $(PWD) - -# -# We need to know the location of the sketchbook. If it hasn't been overridden, -# try the parent of the current directory. If there is no libraries directory -# there, assume that we are in a library's examples directory and try backing up -# further. -# -ifeq ($(SKETCHBOOK),) - SKETCHBOOK := $(shell cd $(SRCROOT)/.. && pwd) - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - SKETCHBOOK := $(shell cd $(SRCROOT)/../.. && pwd) - endif - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - SKETCHBOOK := $(shell cd $(SRCROOT)/../../.. && pwd) - endif - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - SKETCHBOOK := $(shell cd $(SRCROOT)/../../../.. && pwd) - endif - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - $(error ERROR: cannot determine sketchbook location - please specify on the commandline with SKETCHBOOK=) - endif -else - ifeq ($(wildcard $(SKETCHBOOK)/libraries),) - $(warning WARNING: sketchbook directory $(SKETCHBOOK) contains no libraries) - endif -endif - -# -# Work out the sketch name from the name of the source directory. -# -SKETCH := $(lastword $(subst /, ,$(SRCROOT))) -# Workaround a $(lastword ) bug on cygwin -ifeq ($(SKETCH),) - WORDLIST := $(subst /, ,$(SRCROOT)) - SKETCH := $(word $(words $(WORDLIST)),$(WORDLIST)) -endif - -# -# Work out where we are going to be building things -# -TMPDIR ?= /tmp -BUILDROOT := $(abspath $(TMPDIR)/$(SKETCH).build) -ifneq ($(findstring CYGWIN, $(SYSTYPE)),) - # Workaround a $(abspath ) bug on cygwin - ifeq ($(BUILDROOT),) - BUILDROOT := C:$(TMPDIR)/$(SKETCH).build - $(warning your abspath function is not working) - $(warning > setting BUILDROOT to $(BUILDROOT)) - endif -endif - -# Jump over the next makefile sections when runing a "make configure" -ifneq ($(MAKECMDGOALS),configure) - -HARDWARE=desktop -BOARD=desktop - -ifeq ($(SYSTYPE),Darwin) - AWK := awk - CXX := c++ - CC := cc - AS := cc - AR := ar -endif - -CXX ?= g++ -CC ?= gcc -AS ?= gcc -AR ?= ar -LD := $(CXX) - -# Find awk -AWK ?= gawk -ifeq ($(shell which $(AWK)),) -$(error ERROR: cannot find $(AWK) - you may need to install GNU awk) -endif - -# -# Tool options -# -DEFINES = $(EXTRAFLAGS) -DSKETCH=\"$(SKETCH)\" -OPTFLAGS = -g -Wformat -Wall -Wshadow -Wpointer-arith -Wcast-align -Wwrite-strings -Wformat=2 -Wno-reorder -DEPFLAGS = -MD -MT $@ - -# XXX warning options TBD -CXXOPTS = -fno-exceptions -I$(SKETCHBOOK)/libraries/Desktop/include -DDESKTOP_BUILD=1 -COPTS = -I$(SKETCHBOOK)/libraries/Desktop/include -DDESKTOP_BUILD=1 -ASOPTS = -assembler-with-cpp - -CXXFLAGS = -g $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS) -CFLAGS = -g $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(COPTS) -ASFLAGS = -g $(DEFINES) $(DEPFLAGS) $(ASOPTS) -LDFLAGS = -g $(OPTFLAGS) - -LIBS = -lm - -SRCSUFFIXES = *.cpp *.c - -ifeq ($(VERBOSE),) -v = @ -else -v = -endif - - -################################################################################ -# Sketch -# - -# Sketch source files -SKETCHPDESRCS := $(wildcard $(SRCROOT)/*.pde) -SKETCHSRCS := $(wildcard $(addprefix $(SRCROOT)/,$(SRCSUFFIXES))) -SKETCHPDE := $(wildcard $(SRCROOT)/$(SKETCH).pde) -SKETCHCPP := $(BUILDROOT)/$(SKETCH).cpp -ifeq ($(SKETCHPDE),) -$(error ERROR: sketch $(SKETCH) is missing $(SKETCH).pde) -endif - -# Sketch object files -SKETCHOBJS := $(subst $(SRCROOT),$(BUILDROOT),$(SKETCHSRCS)) $(SKETCHCPP) -SKETCHOBJS := $(addsuffix .o,$(basename $(SKETCHOBJS))) - -# List of input files to the sketch.cpp file in the order they should -# be appended to create it -SKETCHCPP_SRC := $(SKETCHPDE) $(sort $(filter-out $(SKETCHPDE),$(SKETCHPDESRCS))) - -################################################################################ -# Libraries -# -# Pick libraries to add to the include path and to link with based on -# #include directives in the sketchfiles. -# -# For example: -# -# #include -# -# implies that there might be a Foo library. -# -# Note that the # and $ require special treatment to avoid upsetting -# make. -# -SEXPR = 's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p' -ifeq ($(SYSTYPE),Darwin) - LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nEe $(SEXPR))) -else - LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) -endif - -# these are library objects we don't want in the desktop build (maybe we'll add them later) -NODESKTOP := I2C/I2C.cpp DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp AP_Baro/AP_Baro_BMP085.cpp AP_RangeFinder/AP_RangeFinder_MaxsonarI2CXL.cpp - -# -# Find sketchbook libraries referenced by the sketch. -# -# Include paths for sketch libraries -# -SKETCHLIBS := $(wildcard $(addprefix $(SKETCHBOOK)/libraries/,$(LIBTOKENS))) -SKETCHLIBNAMES := $(notdir $(SKETCHLIBS)) -SKETCHLIBSRCDIRS := $(SKETCHLIBS) $(SKETCHBOOK)/libraries/Desktop/support $(addsuffix /utility,$(SKETCHLIBS)) -SKETCHLIBSRCS := $(wildcard $(foreach suffix,$(SRCSUFFIXES),$(addsuffix /$(suffix),$(SKETCHLIBSRCDIRS)))) -FILTEREDSRCS := $(filter-out $(addprefix $(SKETCHBOOK)/libraries/,$(NODESKTOP)),$(SKETCHLIBSRCS)) -SKETCHLIBOBJS := $(addsuffix .o,$(basename $(subst $(SKETCHBOOK),$(BUILDROOT),$(FILTEREDSRCS)))) -SKETCHLIBINCLUDES := $(addprefix -I,$(SKETCHLIBS)) - -# -# Find Arduino libraries referenced by the sketch. Exclude any that -# are overloaded by versions in the sketchbook. -# -ARDUINOLIBS := $(wildcard $(addprefix $(ARDUINO)/libraries/,$(filter-out $(SKETCHLIBNAMES),$(LIBTOKENS)))) -ARDUINOLIBNAMES := $(notdir $(ARDUINOLIBS)) -ARDUINOLIBSRCDIRS := $(ARDUINOLIBS) $(addsuffix /utility,$(ARDUINOLIBS)) -ARDUINOLIBSRCS := $(wildcard $(foreach suffix,$(SRCSUFFIXES),$(addsuffix /$(suffix),$(ARDUINOLIBSRCDIRS)))) -ARDUINOLIBOBJS := $(addsuffix .o,$(basename $(subst $(ARDUINO),$(BUILDROOT),$(ARDUINOLIBSRCS)))) -ARDUINOLIBINCLUDES := $(addprefix -I,$(ARDUINOLIBS)) -ARDUINOLIBINCLUDES := $(ARDUINOLIBINCLUDES) - -# Library object files -LIBOBJS := $(SKETCHLIBOBJS) - -################################################################################ -# Built products -# - -# The ELF file -SKETCHELF = $(BUILDROOT)/$(SKETCH).elf - -# Map file -SKETCHMAP = $(BUILDROOT)/$(SKETCH).map - -# The core library -CORELIB = - -# All of the objects that may be built -ALLOBJS = $(SKETCHOBJS) $(LIBOBJS) $(CORELIBOBJS) - -# All of the dependency files that may be generated -ALLDEPS = $(ALLOBJS:%.o=%.d) -endif - -################################################################################ -# Targets -# - -all: $(SKETCHELF) - -clean: - @rm -fr $(BUILDROOT) - -################################################################################ -# Rules -# - -# fetch dependency info from a previous build if any of it exists --include $(ALLDEPS) - -# common header for rules, prints what is being built -define RULEHDR - @echo %% $(subst $(BUILDROOT)/,,$@) - @mkdir -p $(dir $@) -endef - -showsources: - @echo "SKETCHLIBSRCDIRS=$(SKETCHLIBSRCDIRS)" - @echo "SKETCHLIBSRCS=$(SKETCHLIBSRCS)" - @echo "SKETCHOBJS=$(SKETCHOBJS)" - @echo "LIBOBJS=$(LIBOBJS)" - @echo "CORELIB=$(CORELIB)" - -# Link the final object -$(SKETCHELF): $(SKETCHOBJS) $(LIBOBJS) $(CORELIB) - $(RULEHDR) - $(v)$(LD) $(LDFLAGS) -o $@ $^ $(LIBS) - @echo Built $@ - -# -# Build sketch objects -# -SKETCH_INCLUDES = $(SKETCHLIBINCLUDES) $(ARDUINOLIBINCLUDES) $(COREINCLUDES) - -$(BUILDROOT)/%.o: $(BUILDROOT)/%.cpp - $(RULEHDR) - $(v)$(CXX) $(CXXFLAGS) -c -o $@ $< -I$(SRCROOT) $(SKETCH_INCLUDES) - -$(BUILDROOT)/%.o: $(SRCROOT)/%.cpp - $(RULEHDR) - $(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(SKETCH_INCLUDES) - -$(BUILDROOT)/%.o: $(SRCROOT)/%.c - $(RULEHDR) - $(v)$(CC) $(CFLAGS) -c -o $@ $< $(SKETCH_INCLUDES) - -$(BUILDROOT)/%.o: $(SRCROOT)/%.S - $(RULEHDR) - $(v)$(AS) $(ASFLAGS) -c -o $@ $< $(SKETCH_INCLUDES) - -# -# Build library objects from sources in the sketchbook -# -SLIB_INCLUDES = -I$(dir $<)/utility $(SKETCHLIBINCLUDES) $(ARDUINOLIBINCLUDES) $(COREINCLUDES) - -$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.cpp - $(RULEHDR) - $(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(SLIB_INCLUDES) - -$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.c - $(RULEHDR) - $(v)$(CC) $(CFLAGS) -c -o $@ $< $(SLIB_INCLUDES) - -$(BUILDROOT)/libraries/%.o: $(SKETCHBOOK)/libraries/%.S - $(RULEHDR) - $(v)$(AS) $(ASFLAGS) -c -o $@ $< $(SLIB_INCLUDES) - -# -# Build library objects from Ardiuno library sources -# -ALIB_INCLUDES = -I$(dir $<)/utility $(ARDUINOLIBINCLUDES) $(COREINCLUDES) - -$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.cpp - $(RULEHDR) - $(v)$(CXX) $(CXXFLAGS) -c -o $@ $< $(ALIB_INCLUDES) - -$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.c - $(RULEHDR) - $(v)$(CC) $(CFLAGS) -c -o $@ $< $(ALIB_INCLUDES) - -$(BUILDROOT)/libraries/%.o: $(ARDUINO)/libraries/%.S - $(RULEHDR) - $(v)$(AS) $(ASFLAGS) -c -o $@ $< $(ALIB_INCLUDES) - -# -# Build objects from the hardware core -# -$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.cpp - $(RULEHDR) - $(v)$(CXX) $(filter-out -W%,$(CXXFLAGS)) -c -o $@ $< -I$(CORESRC_DIR) - -$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.c - @mkdir -p $(dir $@) - $(v)$(CC) $(filter-out -W%,$(CFLAGS)) -c -o $@ $< -I$(CORESRC_DIR) - -$(BUILDROOT)/$(HARDWARE)/%.o: $(CORESRC_DIR)/%.S - $(RULEHDR) - $(v)$(AS) $(ASFLAGS) -c -o $@ $< -I$(CORESRC_DIR) - -# -# Build the core library -# -$(CORELIB): $(CORELIBOBJS) - $(RULEHDR) - $(v)$(AR) -rcs $@ $^ - -# -# Build the sketch.cpp file -# -# This process strives to be as faithful to the Arduino implementation as -# possible. Conceptually, the process is as follows: -# -# * All of the .pde files are concatenated, starting with the file named -# for the sketch and followed by the others in alphabetical order. -# * An insertion point is created in the concatenated file at -# the first statement that isn't a preprocessor directive or comment. -# * An include of "WProgram.h" is inserted at the insertion point. -# * The file following the insertion point is scanned for function definitions -# and prototypes for these functions are inserted at the insertion point. -# -# In addition, we add #line directives wherever the originating file changes -# to help backtrack from compiler messages and in the debugger. -# -$(SKETCHCPP): $(SKETCHCPP_SRC) - $(RULEHDR) - $(v)$(AWK) -v mode=header '$(SKETCH_SPLITTER)' $(SKETCHCPP_SRC) > $@ - $(v)echo "#line 1 \"autogenerated\"" >> $@ - $(v)echo "#include \"WProgram.h\"" >> $@ - $(v)$(AWK) '$(SKETCH_PROTOTYPER)' $(SKETCHCPP_SRC) >> $@ - $(v)$(AWK) -v mode=body '$(SKETCH_SPLITTER)' $(SKETCHCPP_SRC) >> $@ - -# delete the sketch.cpp file if a processing error occurs -.DELETE_ON_ERROR: $(SKETCHCPP) - -# -# The sketch splitter is an awk script used to split off the -# header and body of the concatenated .pde files. It also -# inserts #line directives to help in backtracking from compiler -# and debugger messages to the original source file. -# -# Note that # and $ require special treatment here to avoid upsetting -# make. -# -# This script requires BWK or GNU awk. -# -define SKETCH_SPLITTER - BEGIN { \ - scanning = 1; \ - printing = (mode ~ "header") ? 1 : 0; \ - } \ - { toggles = 1 } \ - (FNR == 1) && printing { \ - printf "#line %d \"%s\"\n", FNR, FILENAME; \ - } \ - /^[[:space:]]*\/\*/,/\*\// { \ - toggles = 0; \ - } \ - /^[[:space:]]*$$/ || /^[[:space:]]*\/\/.*/ || /^\#.*$$/ { \ - toggles = 0; \ - } \ - scanning && toggles { \ - scanning = 0; \ - printing = !printing; \ - if (printing) { \ - printf "#line %d \"%s\"\n", FNR, FILENAME; \ - } \ - } \ - printing -endef - -# -# The prototype scanner is an awk script used to generate function -# prototypes from the concantenated .pde files. -# -# Function definitions are expected to follow the form -# -# [...]([]){ -# -# with whitespace permitted between the various elements. The pattern -# is assembled from separate subpatterns to make adjustments easier. -# -# Note that $ requires special treatment here to avoid upsetting make, -# and backslashes are doubled in the partial patterns to satisfy -# escaping rules. -# -# This script requires BWK or GNU awk. -# -define SKETCH_PROTOTYPER - BEGIN { \ - RS="{"; \ - type = "((\\n)|(^))[[:space:]]*[[:alnum:]_]+[[:space:]]+"; \ - qualifiers = "([[:alnum:]_\\*&]+[[:space:]]*)*"; \ - name = "[[:alnum:]_]+[[:space:]]*"; \ - args = "\\([[:space:][:alnum:]_,&\\*\\[\\]]*\\)"; \ - bodycuddle = "[[:space:]]*$$"; \ - pattern = type qualifiers name args bodycuddle; \ - } \ - match($$0, pattern) { \ - proto = substr($$0, RSTART, RLENGTH); \ - gsub("\n", " ", proto); \ - printf "%s;\n", proto; \ - } -endef diff --git a/libraries/Desktop/Makefile.desktop b/libraries/Desktop/Makefile.desktop deleted file mode 100644 index 84d1ef0f04..0000000000 --- a/libraries/Desktop/Makefile.desktop +++ /dev/null @@ -1,27 +0,0 @@ -DESKTOP=$(PWD)/../libraries/Desktop - -include ../libraries/Desktop/Desktop.mk - -default: - make -f $(DESKTOP)/Makefile.desktop - -hil: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE" - -hilsen: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS" - -heli: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME" - -helihil: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME -DHIL_MODE=HIL_MODE_ATTITUDE" - -octa: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=OCTA_FRAME" - -hexa: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HEXA_FRAME" - -y6: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=Y6_FRAME" diff --git a/libraries/Desktop/README b/libraries/Desktop/README deleted file mode 100644 index 531688ab73..0000000000 --- a/libraries/Desktop/README +++ /dev/null @@ -1,33 +0,0 @@ -This provides some support files for building APM on normal desktop -systems. This makes it possible to use debugging tools (such as gdb -and valgrind) on the APM code - -The code can then run on the PC instead of on the Arduino board and -simulate the behaviour of the real system by integrating it with -X-Plane of FlightGear to build a Software-In-the-Loop (SIL) simulator. - -It will use TCP sockets to communicate between the several software -components (ArduPilot, GCS and Flight simulator). All the ArduPilot -serial ports that get initialised map to separate TCP ports, which -means you can separately test the telemetry port and the main serial -port. It also makes using a debugger easier, as the debugger can use -stdin/stdout. - -So the new usage is: - - 1) build with "make -f ../libraries/Desktop/Makefile.desktop hil" - - 2) start in a terminal like this: /tmp/ArduPlane.build/ArduPlane.elf - it will say something like this: - - Serial port 0 on TCP port 5760 - Waiting for connection .... - - 3) start a GCS, pointing it at localhost:5760. For the current - mavproxy, you would use: - - mavproxy.py --master=tcp:localhost:5760 - - MichaelO has also added support in the GCS mission planner for TCP. - You will see a TCP option in the drop down for the serial port, then - choose port 5760. diff --git a/libraries/Desktop/include/HardwareSerial.h b/libraries/Desktop/include/HardwareSerial.h deleted file mode 100644 index 3efa775f84..0000000000 --- a/libraries/Desktop/include/HardwareSerial.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - HardwareSerial.h - Hardware serial library for Wiring - Copyright (c) 2006 Nicholas Zambetti. All right reserved. - - 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - - Modified 28 September 2010 by Mark Sproul -*/ - -#ifndef HardwareSerial_h -#define HardwareSerial_h - -#include - -#include "Stream.h" - -struct ring_buffer; - -class HardwareSerial : public Stream -{ - private: - ring_buffer *_rx_buffer; - volatile uint8_t *_ubrrh; - volatile uint8_t *_ubrrl; - volatile uint8_t *_ucsra; - volatile uint8_t *_ucsrb; - volatile uint8_t *_udr; - uint8_t _rxen; - uint8_t _txen; - uint8_t _rxcie; - uint8_t _udre; - uint8_t _u2x; - public: - HardwareSerial(ring_buffer *rx_buffer, - volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, - volatile uint8_t *ucsra, volatile uint8_t *ucsrb, - volatile uint8_t *udr, - uint8_t rxen, uint8_t txen, uint8_t rxcie, uint8_t udre, uint8_t u2x); - void begin(long); - void end(); - virtual int available(void); - virtual int peek(void); - virtual int read(void); - virtual void flush(void); - virtual void write(uint8_t); - using Print::write; // pull in write(str) and write(buf, size) from Print -}; - -#if defined(UBRRH) || defined(UBRR0H) - extern HardwareSerial Serial; -#elif defined(USBCON) - #include "usb_api.h" -#endif -#if defined(UBRR1H) - extern HardwareSerial Serial1; -#endif -#if defined(UBRR2H) - extern HardwareSerial Serial2; -#endif -#if defined(UBRR3H) - extern HardwareSerial Serial3; -#endif - -#endif diff --git a/libraries/Desktop/include/Print.h b/libraries/Desktop/include/Print.h deleted file mode 100644 index b092ae51d1..0000000000 --- a/libraries/Desktop/include/Print.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - Print.h - Base class that provides print() and println() - Copyright (c) 2008 David A. Mellis. All right reserved. - - 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef Print_h -#define Print_h - -#include -#include // for size_t - -#include "WString.h" - -#define DEC 10 -#define HEX 16 -#define OCT 8 -#define BIN 2 -#define BYTE 0 - -class Print -{ - private: - void printNumber(unsigned long, uint8_t); - void printFloat(double, uint8_t); - public: - virtual void write(uint8_t) = 0; - virtual void write(const char *str); - virtual void write(const uint8_t *buffer, size_t size); - - void print(const String &); - void print(const char[]); - void print(char, int = BYTE); - void print(unsigned char, int = BYTE); - void print(int, int = DEC); - void print(unsigned int, int = DEC); - void print(long, int = DEC); - void print(unsigned long, int = DEC); - void print(double, int = 2); - - void println(const String &s); - void println(const char[]); - void println(char, int = BYTE); - void println(unsigned char, int = BYTE); - void println(int, int = DEC); - void println(unsigned int, int = DEC); - void println(long, int = DEC); - void println(unsigned long, int = DEC); - void println(double, int = 2); - void println(void); -}; - -#endif diff --git a/libraries/Desktop/include/SPI.h b/libraries/Desktop/include/SPI.h deleted file mode 100644 index 9808b08dcf..0000000000 --- a/libraries/Desktop/include/SPI.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright (c) 2010 by Cristian Maglie - * SPI Master library for arduino. - * - * This file is free software; you can redistribute it and/or modify - * it under the terms of either the GNU General Public License version 2 - * or the GNU Lesser General Public License version 2.1, both as - * published by the Free Software Foundation. - */ - -#ifndef _SPI_H_INCLUDED -#define _SPI_H_INCLUDED - -#include -#include -#include - -#define SPI_CLOCK_DIV4 0x00 -#define SPI_CLOCK_DIV16 0x01 -//#define SPI_CLOCK_DIV64 0x02 -#define SPI_CLOCK_DIV128 0x03 -#define SPI_CLOCK_DIV2 0x04 -#define SPI_CLOCK_DIV8 0x05 -#define SPI_CLOCK_DIV32 0x06 -#define SPI_CLOCK_DIV64 0x07 - -#define SPI_MODE0 0x00 -#define SPI_MODE1 0x04 -#define SPI_MODE2 0x08 -#define SPI_MODE3 0x0C - -#define SPI_MODE_MASK 0x0C // CPOL = bit 3, CPHA = bit 2 on SPCR -#define SPI_CLOCK_MASK 0x03 // SPR1 = bit 1, SPR0 = bit 0 on SPCR -#define SPI_2XCLOCK_MASK 0x01 // SPI2X = bit 0 on SPSR - -class SPIClass { -public: - inline static byte transfer(byte _data); - - // SPI Configuration methods - - inline static void attachInterrupt(); - inline static void detachInterrupt(); // Default - - static void begin(); // Default - static void end(); - - static void setBitOrder(uint8_t); - static void setDataMode(uint8_t); - static void setClockDivider(uint8_t); -}; - -extern SPIClass SPI; - -byte SPIClass::transfer(byte _data) { - SPDR = _data; - while (!(SPSR & _BV(SPIF))) - ; - return SPDR; -} - -void SPIClass::attachInterrupt() { - SPCR |= _BV(SPIE); -} - -void SPIClass::detachInterrupt() { - SPCR &= ~_BV(SPIE); -} - -#endif diff --git a/libraries/Desktop/include/Stream.h b/libraries/Desktop/include/Stream.h deleted file mode 100644 index 69278b7f7b..0000000000 --- a/libraries/Desktop/include/Stream.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef Stream_h -#define Stream_h - -#include -#include "Print.h" - -class Stream : public Print -{ - public: - virtual int available() = 0; - virtual int read() = 0; - virtual int peek() = 0; - virtual void flush() = 0; -}; - -#endif diff --git a/libraries/Desktop/include/WConstants.h b/libraries/Desktop/include/WConstants.h deleted file mode 100644 index 3e19ac44aa..0000000000 --- a/libraries/Desktop/include/WConstants.h +++ /dev/null @@ -1 +0,0 @@ -#include "wiring.h" diff --git a/libraries/Desktop/include/WProgram.h b/libraries/Desktop/include/WProgram.h deleted file mode 100644 index eb064535a3..0000000000 --- a/libraries/Desktop/include/WProgram.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef WProgram_h -#define WProgram_h - -#include "wiring.h" -#include -#include "HardwareSerial.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include -#include -#include - -typedef uint8_t byte; - -void delay(unsigned long msec); -char *itoa(int __val, char *__s, int __radix); -char *ltoa(long __val, char *__s, int __radix); -char *ultoa(unsigned long __val, char *__s, int __radix); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/libraries/Desktop/include/WString.h b/libraries/Desktop/include/WString.h deleted file mode 100644 index cadddb9476..0000000000 --- a/libraries/Desktop/include/WString.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - WString.h - String library for Wiring & Arduino - Copyright (c) 2009-10 Hernando Barragan. All right reserved. - - 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef String_h -#define String_h - -//#include "WProgram.h" -#include -#include -#include - -class String -{ - public: - // constructors - String( const char *value = "" ); - String( const String &value ); - String( const char ); - String( const unsigned char ); - String( const int, const int base=10); - String( const unsigned int, const int base=10 ); - String( const long, const int base=10 ); - String( const unsigned long, const int base=10 ); - ~String() { free(_buffer); _length = _capacity = 0;} //added _length = _capacity = 0; - - // operators - const String & operator = ( const String &rhs ); - const String & operator +=( const String &rhs ); - //const String & operator +=( const char ); - int operator ==( const String &rhs ) const; - int operator !=( const String &rhs ) const; - int operator < ( const String &rhs ) const; - int operator > ( const String &rhs ) const; - int operator <=( const String &rhs ) const; - int operator >=( const String &rhs ) const; - char operator []( unsigned int index ) const; - char& operator []( unsigned int index ); - //operator const char *() const { return _buffer; } - - // general methods - char charAt( unsigned int index ) const; - int compareTo( const String &anotherString ) const; - unsigned char endsWith( const String &suffix ) const; - unsigned char equals( const String &anObject ) const; - unsigned char equalsIgnoreCase( const String &anotherString ) const; - int indexOf( char ch ) const; - int indexOf( char ch, unsigned int fromIndex ) const; - int indexOf( const String &str ) const; - int indexOf( const String &str, unsigned int fromIndex ) const; - int lastIndexOf( char ch ) const; - int lastIndexOf( char ch, unsigned int fromIndex ) const; - int lastIndexOf( const String &str ) const; - int lastIndexOf( const String &str, unsigned int fromIndex ) const; - const unsigned int length( ) const { return _length; } - void setCharAt(unsigned int index, const char ch); - unsigned char startsWith( const String &prefix ) const; - unsigned char startsWith( const String &prefix, unsigned int toffset ) const; - String substring( unsigned int beginIndex ) const; - String substring( unsigned int beginIndex, unsigned int endIndex ) const; - String toLowerCase( ) const; - String toUpperCase( ) const; - String trim( ) const; - void getBytes(unsigned char *buf, unsigned int bufsize); - void toCharArray(char *buf, unsigned int bufsize); - long toInt( ); - const String& concat( const String &str ); - String replace( char oldChar, char newChar ); - String replace( const String& match, const String& replace ); - friend String operator + ( String lhs, const String &rhs ); - - protected: - char *_buffer; // the actual char array - unsigned int _capacity; // the array length minus one (for the '\0') - unsigned int _length; // the String length (not counting the '\0') - - void getBuffer(unsigned int maxStrLen); - - private: - -}; - -// allocate buffer space -inline void String::getBuffer(unsigned int maxStrLen) -{ - _capacity = maxStrLen; - _buffer = (char *) malloc(_capacity + 1); - if (_buffer == NULL) _length = _capacity = 0; -} - -inline String operator+( String lhs, const String &rhs ) -{ - return lhs += rhs; -} - - -#endif diff --git a/libraries/Desktop/include/Wire.h b/libraries/Desktop/include/Wire.h deleted file mode 100644 index a6c29c493f..0000000000 --- a/libraries/Desktop/include/Wire.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - TwoWire.h - TWI/I2C library for Arduino & Wiring - Copyright (c) 2006 Nicholas Zambetti. All right reserved. - - 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#ifndef TwoWire_h -#define TwoWire_h - -#include - -#define BUFFER_LENGTH 32 - -class TwoWire -{ - private: - static uint8_t rxBuffer[]; - static uint8_t rxBufferIndex; - static uint8_t rxBufferLength; - - static uint8_t txAddress; - static uint8_t txBuffer[]; - static uint8_t txBufferIndex; - static uint8_t txBufferLength; - - static uint8_t transmitting; - static void (*user_onRequest)(void); - static void (*user_onReceive)(int); - static void onRequestService(void); - static void onReceiveService(uint8_t*, int); - public: - TwoWire(); - void begin(); - void begin(uint8_t); - void begin(int); - void beginTransmission(uint8_t); - void beginTransmission(int); - uint8_t endTransmission(void); - uint8_t requestFrom(uint8_t, uint8_t); - uint8_t requestFrom(int, int); - void send(uint8_t); - void send(uint8_t*, uint8_t); - void send(int); - void send(char*); - uint8_t available(void); - uint8_t receive(void); - void onReceive( void (*)(int) ); - void onRequest( void (*)(void) ); -}; - -extern TwoWire Wire; - -#endif - diff --git a/libraries/Desktop/include/avr/eeprom.h b/libraries/Desktop/include/avr/eeprom.h deleted file mode 100644 index 626a25e602..0000000000 --- a/libraries/Desktop/include/avr/eeprom.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef _AVR_EEPROM_H_ -#define _AVR_EEPROM_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#include - -uint8_t eeprom_read_byte(const uint8_t *p); -uint16_t eeprom_read_word(const uint16_t *p); -uint32_t eeprom_read_dword(const uint32_t *p); -void eeprom_read_block(void *buf, void *ptr, uint8_t size); - -void eeprom_write_byte(uint8_t *p, uint8_t value); -void eeprom_write_word(uint16_t *p, uint16_t value); -void eeprom_write_dword(uint32_t *p, uint32_t value); -void eeprom_write_block(const void *buf, void *ptr, uint8_t size); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/libraries/Desktop/include/avr/interrupt.h b/libraries/Desktop/include/avr/interrupt.h deleted file mode 100644 index db18edeba0..0000000000 --- a/libraries/Desktop/include/avr/interrupt.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef _AVR_INTERRUPT_H_ -#define _AVR_INTERRUPT_H_ - -#include "io.h" - -#define ISR(vector,...) void vector(void); \ -void vector(void) - -extern "C" { -void cli(void); -void sei(void); -} - -#endif diff --git a/libraries/Desktop/include/avr/io.h b/libraries/Desktop/include/avr/io.h deleted file mode 100644 index fa58db23a1..0000000000 --- a/libraries/Desktop/include/avr/io.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef _AVR_IO_H_ -#define _AVR_IO_H_ - -#include - -#define F_CPU 16000000UL - -#define _VECTOR(N) __vector_ ## N -#define _BV(bit) (1 << (bit)) - -extern "C" volatile uint8_t __iomem[1024]; - -#define _SFR_MEM8(addr) __iomem[addr] -#define _SFR_MEM16(addr) (*(uint16_t *)&__iomem[addr]) - -#define _SFR_IO8(addr) __iomem[addr] - -extern "C" volatile uint8_t SREG; - -#define _interrupts_are_blocked() ((SREG&0x80)==0) - -#define __ATmegaxx0__ -#include "iom2560.h" - -#endif diff --git a/libraries/Desktop/include/avr/iom2560.h b/libraries/Desktop/include/avr/iom2560.h deleted file mode 100644 index 07039ad7e8..0000000000 --- a/libraries/Desktop/include/avr/iom2560.h +++ /dev/null @@ -1,94 +0,0 @@ -/* Copyright (c) 2005 Anatoly Sokolov - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in - the documentation and/or other materials provided with the - distribution. - - * Neither the name of the copyright holders nor the names of - contributors may be used to endorse or promote products derived - from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - POSSIBILITY OF SUCH DAMAGE. */ - -/* $Id */ - -/* avr/iom2560.h - definitions for ATmega2560 */ - -#ifndef _AVR_IOM2560_H_ -#define _AVR_IOM2560_H_ 1 - -#include - -/* Constants */ -#define SPM_PAGESIZE 256 -#define RAMEND 0x21FF -#define XRAMEND 0xFFFF -#define E2END 0xFFF -#define E2PAGESIZE 8 -#define FLASHEND 0x3FFFF - - -/* Fuses */ - -#define FUSE_MEMORY_SIZE 3 - -/* Low Fuse Byte */ -#define FUSE_CKSEL0 (unsigned char)~_BV(0) -#define FUSE_CKSEL1 (unsigned char)~_BV(1) -#define FUSE_CKSEL2 (unsigned char)~_BV(2) -#define FUSE_CKSEL3 (unsigned char)~_BV(3) -#define FUSE_SUT0 (unsigned char)~_BV(4) -#define FUSE_SUT1 (unsigned char)~_BV(5) -#define FUSE_CKOUT (unsigned char)~_BV(6) -#define FUSE_CKDIV8 (unsigned char)~_BV(7) -#define LFUSE_DEFAULT (FUSE_CKSEL0 & FUSE_CKSEL2 & FUSE_CKSEL3 & FUSE_SUT0 & FUSE_CKDIV8) - -/* High Fuse Byte */ -#define FUSE_BOOTRST (unsigned char)~_BV(0) -#define FUSE_BOOTSZ0 (unsigned char)~_BV(1) -#define FUSE_BOOTSZ1 (unsigned char)~_BV(2) -#define FUSE_EESAVE (unsigned char)~_BV(3) -#define FUSE_WDTON (unsigned char)~_BV(4) -#define FUSE_SPIEN (unsigned char)~_BV(5) -#define FUSE_JTAGEN (unsigned char)~_BV(6) -#define FUSE_OCDEN (unsigned char)~_BV(7) -#define HFUSE_DEFAULT (FUSE_BOOTSZ0 & FUSE_BOOTSZ1 & FUSE_SPIEN & FUSE_JTAGEN) - -/* Extended Fuse Byte */ -#define FUSE_BODLEVEL0 (unsigned char)~_BV(0) -#define FUSE_BODLEVEL1 (unsigned char)~_BV(1) -#define FUSE_BODLEVEL2 (unsigned char)~_BV(2) -#define EFUSE_DEFAULT (0xFF) - - -/* Lock Bits */ -#define __LOCK_BITS_EXIST -#define __BOOT_LOCK_BITS_0_EXIST -#define __BOOT_LOCK_BITS_1_EXIST - - -/* Signature */ -#define SIGNATURE_0 0x1E -#define SIGNATURE_1 0x98 -#define SIGNATURE_2 0x01 - - -#endif /* _AVR_IOM2560_H_ */ diff --git a/libraries/Desktop/include/avr/iomxx0_1.h b/libraries/Desktop/include/avr/iomxx0_1.h deleted file mode 100644 index 4cd54ff9fa..0000000000 --- a/libraries/Desktop/include/avr/iomxx0_1.h +++ /dev/null @@ -1,1563 +0,0 @@ -/* Copyright (c) 2005 Anatoly Sokolov - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in - the documentation and/or other materials provided with the - distribution. - - * Neither the name of the copyright holders nor the names of - contributors may be used to endorse or promote products derived - from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - POSSIBILITY OF SUCH DAMAGE. */ - -/* $Id: iomxx0_1.h,v 1.12 2007/12/12 14:00:49 arcanum Exp $ */ - -/* avr/iomxx0_1.h - definitions for ATmega640, Atmega1280, ATmega1281, - ATmega2560 and ATmega2561. */ - -#ifndef _AVR_IOMXX0_1_H_ -#define _AVR_IOMXX0_1_H_ 1 - -/* This file should only be included from , never directly. */ - -#ifndef _AVR_IO_H_ -# error "Include instead of this file." -#endif - -#ifndef _AVR_IOXXX_H_ -# define _AVR_IOXXX_H_ "iomxx0_1.h" -#else -# error "Attempt to include more than one file." -#endif - -#if defined(__AVR_ATmega640__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(DESKTOP_BUILD) -# define __ATmegaxx0__ -#elif defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) -# define __ATmegaxx1__ -#endif - -/* Registers and associated bit numbers */ - -#define PINA _SFR_IO8(0X00) -#define PINA7 7 -#define PINA6 6 -#define PINA5 5 -#define PINA4 4 -#define PINA3 3 -#define PINA2 2 -#define PINA1 1 -#define PINA0 0 - -#define DDRA _SFR_IO8(0X01) -#define DDA7 7 -#define DDA6 6 -#define DDA5 5 -#define DDA4 4 -#define DDA3 3 -#define DDA2 2 -#define DDA1 1 -#define DDA0 0 - -#define PORTA _SFR_IO8(0X02) -#define PA7 7 -#define PA6 6 -#define PA5 5 -#define PA4 4 -#define PA3 3 -#define PA2 2 -#define PA1 1 -#define PA0 0 - -#define PINB _SFR_IO8(0X03) -#define PINB7 7 -#define PINB6 6 -#define PINB5 5 -#define PINB4 4 -#define PINB3 3 -#define PINB2 2 -#define PINB1 1 -#define PINB0 0 - -#define DDRB _SFR_IO8(0x04) -#define DDB7 7 -#define DDB6 6 -#define DDB5 5 -#define DDB4 4 -#define DDB3 3 -#define DDB2 2 -#define DDB1 1 -#define DDB0 0 - -#define PORTB _SFR_IO8(0x05) -#define PB7 7 -#define PB6 6 -#define PB5 5 -#define PB4 4 -#define PB3 3 -#define PB2 2 -#define PB1 1 -#define PB0 0 - -#define PINC _SFR_IO8(0x06) -#define PINC7 7 -#define PINC6 6 -#define PINC5 5 -#define PINC4 4 -#define PINC3 3 -#define PINC2 2 -#define PINC1 1 -#define PINC0 0 - -#define DDRC _SFR_IO8(0x07) -#define DDC7 7 -#define DDC6 6 -#define DDC5 5 -#define DDC4 4 -#define DDC3 3 -#define DDC2 2 -#define DDC1 1 -#define DDC0 0 - -#define PORTC _SFR_IO8(0x08) -#define PC7 7 -#define PC6 6 -#define PC5 5 -#define PC4 4 -#define PC3 3 -#define PC2 2 -#define PC1 1 -#define PC0 0 - -#define PIND _SFR_IO8(0x09) -#define PIND7 7 -#define PIND6 6 -#define PIND5 5 -#define PIND4 4 -#define PIND3 3 -#define PIND2 2 -#define PIND1 1 -#define PIND0 0 - -#define DDRD _SFR_IO8(0x0A) -#define DDD7 7 -#define DDD6 6 -#define DDD5 5 -#define DDD4 4 -#define DDD3 3 -#define DDD2 2 -#define DDD1 1 -#define DDD0 0 - -#define PORTD _SFR_IO8(0x0B) -#define PD7 7 -#define PD6 6 -#define PD5 5 -#define PD4 4 -#define PD3 3 -#define PD2 2 -#define PD1 1 -#define PD0 0 - -#define PINE _SFR_IO8(0x0C) -#define PINE7 7 -#define PINE6 6 -#define PINE5 5 -#define PINE4 4 -#define PINE3 3 -#define PINE2 2 -#define PINE1 1 -#define PINE0 0 - -#define DDRE _SFR_IO8(0x0D) -#define DDE7 7 -#define DDE6 6 -#define DDE5 5 -#define DDE4 4 -#define DDE3 3 -#define DDE2 2 -#define DDE1 1 -#define DDE0 0 - -#define PORTE _SFR_IO8(0x0E) -#define PE7 7 -#define PE6 6 -#define PE5 5 -#define PE4 4 -#define PE3 3 -#define PE2 2 -#define PE1 1 -#define PE0 0 - -#define PINF _SFR_IO8(0x0F) -#define PINF7 7 -#define PINF6 6 -#define PINF5 5 -#define PINF4 4 -#define PINF3 3 -#define PINF2 2 -#define PINF1 1 -#define PINF0 0 - -#define DDRF _SFR_IO8(0x10) -#define DDF7 7 -#define DDF6 6 -#define DDF5 5 -#define DDF4 4 -#define DDF3 3 -#define DDF2 2 -#define DDF1 1 -#define DDF0 0 - -#define PORTF _SFR_IO8(0x11) -#define PF7 7 -#define PF6 6 -#define PF5 5 -#define PF4 4 -#define PF3 3 -#define PF2 2 -#define PF1 1 -#define PF0 0 - -#define PING _SFR_IO8(0x12) -#define PING5 5 -#define PING4 4 -#define PING3 3 -#define PING2 2 -#define PING1 1 -#define PING0 0 - -#define DDRG _SFR_IO8(0x13) -#define DDG5 5 -#define DDG4 4 -#define DDG3 3 -#define DDG2 2 -#define DDG1 1 -#define DDG0 0 - -#define PORTG _SFR_IO8(0x14) -#define PG5 5 -#define PG4 4 -#define PG3 3 -#define PG2 2 -#define PG1 1 -#define PG0 0 - -#define TIFR0 _SFR_IO8(0x15) -#define OCF0B 2 -#define OCF0A 1 -#define TOV0 0 - -#define TIFR1 _SFR_IO8(0x16) -#define ICF1 5 -#define OCF1C 3 -#define OCF1B 2 -#define OCF1A 1 -#define TOV1 0 - -#define TIFR2 _SFR_IO8(0x17) -#define OCF2B 2 -#define OCF2A 1 -#define TOV2 0 - -#define TIFR3 _SFR_IO8(0x18) -#define ICF3 5 -#define OCF3C 3 -#define OCF3B 2 -#define OCF3A 1 -#define TOV3 0 - -#define TIFR4 _SFR_IO8(0x19) -#define ICF4 5 -#define OCF4C 3 -#define OCF4B 2 -#define OCF4A 1 -#define TOV4 0 - -#define TIFR5 _SFR_IO8(0x1A) -#define ICF5 5 -#define OCF5C 3 -#define OCF5B 2 -#define OCF5A 1 -#define TOV5 0 - -#define PCIFR _SFR_IO8(0x1B) -#if defined(__ATmegaxx0__) -# define PCIF2 2 -#endif /* __ATmegaxx0__ */ -#define PCIF1 1 -#define PCIF0 0 - -#define EIFR _SFR_IO8(0x1C) -#define INTF7 7 -#define INTF6 6 -#define INTF5 5 -#define INTF4 4 -#define INTF3 3 -#define INTF2 2 -#define INTF1 1 -#define INTF0 0 - -#define EIMSK _SFR_IO8(0x1D) -#define INT7 7 -#define INT6 6 -#define INT5 5 -#define INT4 4 -#define INT3 3 -#define INT2 2 -#define INT1 1 -#define INT0 0 - -#define GPIOR0 _SFR_IO8(0x1E) - -#define EECR _SFR_IO8(0x1F) -#define EEPM1 5 -#define EEPM0 4 -#define EERIE 3 -#define EEMPE 2 -#define EEPE 1 -#define EERE 0 - -#define EEDR _SFR_IO8(0X20) - -/* Combine EEARL and EEARH */ -#define EEAR _SFR_IO16(0x21) - -#define EEARL _SFR_IO8(0x21) -#define EEARH _SFR_IO8(0X22) - -/* 6-char sequence denoting where to find the EEPROM registers in memory space. - Adresses denoted in hex syntax with uppercase letters. Used by the EEPROM - subroutines. - First two letters: EECR address. - Second two letters: EEDR address. - Last two letters: EEAR address. */ -#define __EEPROM_REG_LOCATIONS__ 1F2021 - -#define GTCCR _SFR_IO8(0x23) -#define TSM 7 -#define PSRASY 1 -#define PSRSYNC 0 - -#define TCCR0A _SFR_IO8(0x24) -#define COM0A1 7 -#define COM0A0 6 -#define COM0B1 5 -#define COM0B0 4 -#define WGM01 1 -#define WGM00 0 - -#define TCCR0B _SFR_IO8(0x25) -#define FOC0A 7 -#define FOC0B 6 -#define WGM02 3 -#define CS02 2 -#define CS01 1 -#define CS00 0 - -#define TCNT0 _SFR_IO8(0X26) - -#define OCR0A _SFR_IO8(0X27) - -#define OCR0B _SFR_IO8(0X28) - -/* Reserved [0x29] */ - -#define GPIOR1 _SFR_IO8(0x2A) - -#define GPIOR2 _SFR_IO8(0x2B) - -#define SPCR _SFR_IO8(0x2C) -#define SPIE 7 -#define SPE 6 -#define DORD 5 -#define MSTR 4 -#define CPOL 3 -#define CPHA 2 -#define SPR1 1 -#define SPR0 0 - -#define SPSR _SFR_IO8(0x2D) -#define SPIF 7 -#define WCOL 6 -#define SPI2X 0 - -#define SPDR _SFR_IO8(0X2E) - -/* Reserved [0x2F] */ - -#define ACSR _SFR_IO8(0x30) -#define ACD 7 -#define ACBG 6 -#define ACO 5 -#define ACI 4 -#define ACIE 3 -#define ACIC 2 -#define ACIS1 1 -#define ACIS0 0 - -#define MONDR _SFR_IO8(0x31) -#define OCDR _SFR_IO8(0x31) -#define IDRD 7 -#define OCDR7 7 -#define OCDR6 6 -#define OCDR5 5 -#define OCDR4 4 -#define OCDR3 3 -#define OCDR2 2 -#define OCDR1 1 -#define OCDR0 0 - -/* Reserved [0x32] */ - -#define SMCR _SFR_IO8(0x33) -#define SM2 3 -#define SM1 2 -#define SM0 1 -#define SE 0 - -#define MCUSR _SFR_IO8(0x34) -#define JTRF 4 -#define WDRF 3 -#define BORF 2 -#define EXTRF 1 -#define PORF 0 - -#define MCUCR _SFR_IO8(0X35) -#define JTD 7 -#define PUD 4 -#define IVSEL 1 -#define IVCE 0 - -/* Reserved [0x36] */ - -#define SPMCSR _SFR_IO8(0x37) -#define SPMIE 7 -#define RWWSB 6 -#define SIGRD 5 -#define RWWSRE 4 -#define BLBSET 3 -#define PGWRT 2 -#define PGERS 1 -#define SPMEN 0 - -/* Reserved [0x38..0x3A] */ - -#define RAMPZ _SFR_IO8(0X3B) -#define RAMPZ0 0 - -#define EIND _SFR_IO8(0X3C) -#define EIND0 0 - -/* SP [0x3D..0x3E] */ -/* SREG [0x3F] */ - -#define WDTCSR _SFR_MEM8(0x60) -#define WDIF 7 -#define WDIE 6 -#define WDP3 5 -#define WDCE 4 -#define WDE 3 -#define WDP2 2 -#define WDP1 1 -#define WDP0 0 - -#define CLKPR _SFR_MEM8(0x61) -#define CLKPCE 7 -#define CLKPS3 3 -#define CLKPS2 2 -#define CLKPS1 1 -#define CLKPS0 0 - -/* Reserved [0x62..0x63] */ - -#define PRR0 _SFR_MEM8(0x64) -#define PRTWI 7 -#define PRTIM2 6 -#define PRTIM0 5 -#define PRTIM1 3 -#define PRSPI 2 -#define PRUSART0 1 -#define PRADC 0 - -#define PRR1 _SFR_MEM8(0x65) -#define PRTIM5 5 -#define PRTIM4 4 -#define PRTIM3 3 -#define PRUSART3 2 -#define PRUSART2 1 -#define PRUSART1 0 - -#define OSCCAL _SFR_MEM8(0x66) - -/* Reserved [0x67] */ - -#define PCICR _SFR_MEM8(0x68) -#if defined(__ATmegaxx0__) -# define PCIE2 2 -#endif /* __ATmegaxx0__ */ -#define PCIE1 1 -#define PCIE0 0 - -#define EICRA _SFR_MEM8(0x69) -#define ISC31 7 -#define ISC30 6 -#define ISC21 5 -#define ISC20 4 -#define ISC11 3 -#define ISC10 2 -#define ISC01 1 -#define ISC00 0 - -#define EICRB _SFR_MEM8(0x6A) -#define ISC71 7 -#define ISC70 6 -#define ISC61 5 -#define ISC60 4 -#define ISC51 3 -#define ISC50 2 -#define ISC41 1 -#define ISC40 0 - -#define PCMSK0 _SFR_MEM8(0x6B) -#define PCINT7 7 -#define PCINT6 6 -#define PCINT5 5 -#define PCINT4 4 -#define PCINT3 3 -#define PCINT2 2 -#define PCINT1 1 -#define PCINT0 0 - -#define PCMSK1 _SFR_MEM8(0x6C) -#define PCINT15 7 -#define PCINT14 6 -#define PCINT13 5 -#define PCINT12 4 -#define PCINT11 3 -#define PCINT10 2 -#define PCINT9 1 -#define PCINT8 0 - -#if defined(__ATmegaxx0__) -# define PCMSK2 _SFR_MEM8(0x6D) -# define PCINT23 7 -# define PCINT22 6 -# define PCINT21 5 -# define PCINT20 4 -# define PCINT19 3 -# define PCINT18 2 -# define PCINT17 1 -# define PCINT16 0 -#endif /* __ATmegaxx0__ */ - -#define TIMSK0 _SFR_MEM8(0x6E) -#define OCIE0B 2 -#define OCIE0A 1 -#define TOIE0 0 - -#define TIMSK1 _SFR_MEM8(0x6F) -#define ICIE1 5 -#define OCIE1C 3 -#define OCIE1B 2 -#define OCIE1A 1 -#define TOIE1 0 - -#define TIMSK2 _SFR_MEM8(0x70) -#define OCIE2B 2 -#define OCIE2A 1 -#define TOIE2 0 - -#define TIMSK3 _SFR_MEM8(0x71) -#define ICIE3 5 -#define OCIE3C 3 -#define OCIE3B 2 -#define OCIE3A 1 -#define TOIE3 0 - -#define TIMSK4 _SFR_MEM8(0x72) -#define ICIE4 5 -#define OCIE4C 3 -#define OCIE4B 2 -#define OCIE4A 1 -#define TOIE4 0 - -#define TIMSK5 _SFR_MEM8(0x73) -#define ICIE5 5 -#define OCIE5C 3 -#define OCIE5B 2 -#define OCIE5A 1 -#define TOIE5 0 - -#define XMCRA _SFR_MEM8(0x74) -#define SRE 7 -#define SRL2 6 -#define SRL1 5 -#define SRL0 4 -#define SRW11 3 -#define SRW10 2 -#define SRW01 1 -#define SRW00 0 - -#define XMCRB _SFR_MEM8(0x75) -#define XMBK 7 -#define XMM2 2 -#define XMM1 1 -#define XMM0 0 - -/* Reserved [0x76..0x77] */ - -/* Combine ADCL and ADCH */ -#ifndef __ASSEMBLER__ -#define ADC _SFR_MEM16(0x78) -#endif -#define ADCW _SFR_MEM16(0x78) -#define ADCL _SFR_MEM8(0x78) -#define ADCH _SFR_MEM8(0x79) - -#define ADCSRA _SFR_MEM8(0x7A) -#define ADEN 7 -#define ADSC 6 -#define ADATE 5 -#define ADIF 4 -#define ADIE 3 -#define ADPS2 2 -#define ADPS1 1 -#define ADPS0 0 - -#define ADCSRB _SFR_MEM8(0x7B) -#define ACME 6 -#if defined(__ATmegaxx0__) -# define MUX5 3 -#endif /* __ATmegaxx0__ */ -#define ADTS2 2 -#define ADTS1 1 -#define ADTS0 0 - -#define ADMUX _SFR_MEM8(0x7C) -#define REFS1 7 -#define REFS0 6 -#define ADLAR 5 -#define MUX4 4 -#define MUX3 3 -#define MUX2 2 -#define MUX1 1 -#define MUX0 0 - -#define DIDR2 _SFR_MEM8(0x7D) -#define ADC15D 7 -#define ADC14D 6 -#define ADC13D 5 -#define ADC12D 4 -#define ADC11D 3 -#define ADC10D 2 -#define ADC9D 1 -#define ADC8D 0 - -#define DIDR0 _SFR_MEM8(0x7E) -#define ADC7D 7 -#define ADC6D 6 -#define ADC5D 5 -#define ADC4D 4 -#define ADC3D 3 -#define ADC2D 2 -#define ADC1D 1 -#define ADC0D 0 - -#define DIDR1 _SFR_MEM8(0x7F) -#define AIN1D 1 -#define AIN0D 0 - -#define TCCR1A _SFR_MEM8(0x80) -#define COM1A1 7 -#define COM1A0 6 -#define COM1B1 5 -#define COM1B0 4 -#define COM1C1 3 -#define COM1C0 2 -#define WGM11 1 -#define WGM10 0 - -#define TCCR1B _SFR_MEM8(0x81) -#define ICNC1 7 -#define ICES1 6 -#define WGM13 4 -#define WGM12 3 -#define CS12 2 -#define CS11 1 -#define CS10 0 - -#define TCCR1C _SFR_MEM8(0x82) -#define FOC1A 7 -#define FOC1B 6 -#define FOC1C 5 - -/* Reserved [0x83] */ - -/* Combine TCNT1L and TCNT1H */ -#define TCNT1 _SFR_MEM16(0x84) - -#define TCNT1L _SFR_MEM8(0x84) -#define TCNT1H _SFR_MEM8(0x85) - -/* Combine ICR1L and ICR1H */ -#define ICR1 _SFR_MEM16(0x86) - -#define ICR1L _SFR_MEM8(0x86) -#define ICR1H _SFR_MEM8(0x87) - -/* Combine OCR1AL and OCR1AH */ -#define OCR1A _SFR_MEM16(0x88) - -#define OCR1AL _SFR_MEM8(0x88) -#define OCR1AH _SFR_MEM8(0x89) - -/* Combine OCR1BL and OCR1BH */ -#define OCR1B _SFR_MEM16(0x8A) - -#define OCR1BL _SFR_MEM8(0x8A) -#define OCR1BH _SFR_MEM8(0x8B) - -/* Combine OCR1CL and OCR1CH */ -#define OCR1C _SFR_MEM16(0x8C) - -#define OCR1CL _SFR_MEM8(0x8C) -#define OCR1CH _SFR_MEM8(0x8D) - -/* Reserved [0x8E..0x8F] */ - -#define TCCR3A _SFR_MEM8(0x90) -#define COM3A1 7 -#define COM3A0 6 -#define COM3B1 5 -#define COM3B0 4 -#define COM3C1 3 -#define COM3C0 2 -#define WGM31 1 -#define WGM30 0 - -#define TCCR3B _SFR_MEM8(0x91) -#define ICNC3 7 -#define ICES3 6 -#define WGM33 4 -#define WGM32 3 -#define CS32 2 -#define CS31 1 -#define CS30 0 - -#define TCCR3C _SFR_MEM8(0x92) -#define FOC3A 7 -#define FOC3B 6 -#define FOC3C 5 - -/* Reserved [0x93] */ - -/* Combine TCNT3L and TCNT3H */ -#define TCNT3 _SFR_MEM16(0x94) - -#define TCNT3L _SFR_MEM8(0x94) -#define TCNT3H _SFR_MEM8(0x95) - -/* Combine ICR3L and ICR3H */ -#define ICR3 _SFR_MEM16(0x96) - -#define ICR3L _SFR_MEM8(0x96) -#define ICR3H _SFR_MEM8(0x97) - -/* Combine OCR3AL and OCR3AH */ -#define OCR3A _SFR_MEM16(0x98) - -#define OCR3AL _SFR_MEM8(0x98) -#define OCR3AH _SFR_MEM8(0x99) - -/* Combine OCR3BL and OCR3BH */ -#define OCR3B _SFR_MEM16(0x9A) - -#define OCR3BL _SFR_MEM8(0x9A) -#define OCR3BH _SFR_MEM8(0x9B) - -/* Combine OCR3CL and OCR3CH */ -#define OCR3C _SFR_MEM16(0x9C) - -#define OCR3CL _SFR_MEM8(0x9C) -#define OCR3CH _SFR_MEM8(0x9D) - -/* Reserved [0x9E..0x9F] */ - -#define TCCR4A _SFR_MEM8(0xA0) -#define COM4A1 7 -#define COM4A0 6 -#define COM4B1 5 -#define COM4B0 4 -#define COM4C1 3 -#define COM4C0 2 -#define WGM41 1 -#define WGM40 0 - -#define TCCR4B _SFR_MEM8(0xA1) -#define ICNC4 7 -#define ICES4 6 -#define WGM43 4 -#define WGM42 3 -#define CS42 2 -#define CS41 1 -#define CS40 0 - -#define TCCR4C _SFR_MEM8(0xA2) -#define FOC4A 7 -#define FOC4B 6 -#define FOC4C 5 - -/* Reserved [0xA3] */ - -/* Combine TCNT4L and TCNT4H */ -#define TCNT4 _SFR_MEM16(0xA4) - -#define TCNT4L _SFR_MEM8(0xA4) -#define TCNT4H _SFR_MEM8(0xA5) - -/* Combine ICR4L and ICR4H */ -#ifdef DESKTOP_BUILD -#include "../support/sitl_rc.h" -extern struct RC_ICR4 ICR4; -#else -#define ICR4 _SFR_MEM16(0xA6) -#endif - -#define ICR4L _SFR_MEM8(0xA6) -#define ICR4H _SFR_MEM8(0xA7) - -/* Combine OCR4AL and OCR4AH */ -#define OCR4A _SFR_MEM16(0xA8) - -#define OCR4AL _SFR_MEM8(0xA8) -#define OCR4AH _SFR_MEM8(0xA9) - -/* Combine OCR4BL and OCR4BH */ -#define OCR4B _SFR_MEM16(0xAA) - -#define OCR4BL _SFR_MEM8(0xAA) -#define OCR4BH _SFR_MEM8(0xAB) - -/* Combine OCR4CL and OCR4CH */ -#define OCR4C _SFR_MEM16(0xAC) - -#define OCR4CL _SFR_MEM8(0xAC) -#define OCR4CH _SFR_MEM8(0xAD) - -/* Reserved [0xAE..0xAF] */ - -#define TCCR2A _SFR_MEM8(0xB0) -#define COM2A1 7 -#define COM2A0 6 -#define COM2B1 5 -#define COM2B0 4 -#define WGM21 1 -#define WGM20 0 - -#define TCCR2B _SFR_MEM8(0xB1) -#define FOC2A 7 -#define FOC2B 6 -#define WGM22 3 -#define CS22 2 -#define CS21 1 -#define CS20 0 - -#define TCNT2 _SFR_MEM8(0xB2) - -#define OCR2A _SFR_MEM8(0xB3) - -#define OCR2B _SFR_MEM8(0xB4) - -/* Reserved [0xB5] */ - -#define ASSR _SFR_MEM8(0xB6) -#define EXCLK 6 -#define AS2 5 -#define TCN2UB 4 -#define OCR2AUB 3 -#define OCR2BUB 2 -#define TCR2AUB 1 -#define TCR2BUB 0 - -/* Reserved [0xB7] */ - -#define TWBR _SFR_MEM8(0xB8) - -#define TWSR _SFR_MEM8(0xB9) -#define TWS7 7 -#define TWS6 6 -#define TWS5 5 -#define TWS4 4 -#define TWS3 3 -#define TWPS1 1 -#define TWPS0 0 - -#define TWAR _SFR_MEM8(0xBA) -#define TWA6 7 -#define TWA5 6 -#define TWA4 5 -#define TWA3 4 -#define TWA2 3 -#define TWA1 2 -#define TWA0 1 -#define TWGCE 0 - -#define TWDR _SFR_MEM8(0xBB) - -#define TWCR _SFR_MEM8(0xBC) -#define TWINT 7 -#define TWEA 6 -#define TWSTA 5 -#define TWSTO 4 -#define TWWC 3 -#define TWEN 2 -#define TWIE 0 - -#define TWAMR _SFR_MEM8(0xBD) -#define TWAM6 7 -#define TWAM5 6 -#define TWAM4 5 -#define TWAM3 4 -#define TWAM2 3 -#define TWAM1 2 -#define TWAM0 1 - -/* Reserved [0xBE..0xBF] */ - -#define UCSR0A _SFR_MEM8(0xC0) -#define RXC0 7 -#define TXC0 6 -#define UDRE0 5 -#define FE0 4 -#define DOR0 3 -#define UPE0 2 -#define U2X0 1 -#define MPCM0 0 - -#define UCSR0B _SFR_MEM8(0XC1) -#define RXCIE0 7 -#define TXCIE0 6 -#define UDRIE0 5 -#define RXEN0 4 -#define TXEN0 3 -#define UCSZ02 2 -#define RXB80 1 -#define TXB80 0 - -#define UCSR0C _SFR_MEM8(0xC2) -#define UMSEL01 7 -#define UMSEL00 6 -#define UPM01 5 -#define UPM00 4 -#define USBS0 3 -#define UCSZ01 2 -#define UCSZ00 1 -#define UCPOL0 0 - -/* Reserved [0xC3] */ - -/* Combine UBRR0L and UBRR0H */ -#define UBRR0 _SFR_MEM16(0xC4) - -#define UBRR0L _SFR_MEM8(0xC4) -#define UBRR0H _SFR_MEM8(0xC5) - -#define UDR0 _SFR_MEM8(0XC6) - -/* Reserved [0xC7] */ - -#define UCSR1A _SFR_MEM8(0xC8) -#define RXC1 7 -#define TXC1 6 -#define UDRE1 5 -#define FE1 4 -#define DOR1 3 -#define UPE1 2 -#define U2X1 1 -#define MPCM1 0 - -#define UCSR1B _SFR_MEM8(0XC9) -#define RXCIE1 7 -#define TXCIE1 6 -#define UDRIE1 5 -#define RXEN1 4 -#define TXEN1 3 -#define UCSZ12 2 -#define RXB81 1 -#define TXB81 0 - -#define UCSR1C _SFR_MEM8(0xCA) -#define UMSEL11 7 -#define UMSEL10 6 -#define UPM11 5 -#define UPM10 4 -#define USBS1 3 -#define UCSZ11 2 -#define UCSZ10 1 -#define UCPOL1 0 - -/* Reserved [0xCB] */ - -/* Combine UBRR1L and UBRR1H */ -#define UBRR1 _SFR_MEM16(0xCC) - -#define UBRR1L _SFR_MEM8(0xCC) -#define UBRR1H _SFR_MEM8(0xCD) - -#define UDR1 _SFR_MEM8(0XCE) - -/* Reserved [0xCF] */ - -#if defined(__ATmegaxx0__) - -# define UCSR2A _SFR_MEM8(0xD0) -# define RXC2 7 -# define TXC2 6 -# define UDRE2 5 -# define FE2 4 -# define DOR2 3 -# define UPE2 2 -# define U2X2 1 -# define MPCM2 0 - -# define UCSR2B _SFR_MEM8(0XD1) -# define RXCIE2 7 -# define TXCIE2 6 -# define UDRIE2 5 -# define RXEN2 4 -# define TXEN2 3 -# define UCSZ22 2 -# define RXB82 1 -# define TXB82 0 - -# define UCSR2C _SFR_MEM8(0xD2) -# define UMSEL21 7 -# define UMSEL20 6 -# define UPM21 5 -# define UPM20 4 -# define USBS2 3 -# define UCSZ21 2 -# define UCSZ20 1 -# define UCPOL2 0 - -/* Reserved [0xD3] */ - -/* Combine UBRR2L and UBRR2H */ -# define UBRR2 _SFR_MEM16(0xD4) - -# define UBRR2L _SFR_MEM8(0xD4) -# define UBRR2H _SFR_MEM8(0xD5) - -#ifdef DESKTOP_BUILD -#include "../support/sitl_adc.h" -extern struct ADC_UDR2 UDR2; -#else -# define UDR2 _SFR_MEM8(0XD6) -#endif - -#endif /* __ATmegaxx0__ */ - -/* Reserved [0xD7..0xFF] */ - -#if defined(__ATmegaxx0__) - -# define PINH _SFR_MEM8(0x100) -# define PINH7 7 -# define PINH6 6 -# define PINH5 5 -# define PINH4 4 -# define PINH3 3 -# define PINH2 2 -# define PINH1 1 -# define PINH0 0 - -# define DDRH _SFR_MEM8(0x101) -# define DDH7 7 -# define DDH6 6 -# define DDH5 5 -# define DDH4 4 -# define DDH3 3 -# define DDH2 2 -# define DDH1 1 -# define DDH0 0 - -# define PORTH _SFR_MEM8(0x102) -# define PH7 7 -# define PH6 6 -# define PH5 5 -# define PH4 4 -# define PH3 3 -# define PH2 2 -# define PH1 1 -# define PH0 0 - -# define PINJ _SFR_MEM8(0x103) -# define PINJ7 7 -# define PINJ6 6 -# define PINJ5 5 -# define PINJ4 4 -# define PINJ3 3 -# define PINJ2 2 -# define PINJ1 1 -# define PINJ0 0 - -# define DDRJ _SFR_MEM8(0x104) -# define DDJ7 7 -# define DDJ6 6 -# define DDJ5 5 -# define DDJ4 4 -# define DDJ3 3 -# define DDJ2 2 -# define DDJ1 1 -# define DDJ0 0 - -# define PORTJ _SFR_MEM8(0x105) -# define PJ7 7 -# define PJ6 6 -# define PJ5 5 -# define PJ4 4 -# define PJ3 3 -# define PJ2 2 -# define PJ1 1 -# define PJ0 0 - -# define PINK _SFR_MEM8(0x106) -# define PINK7 7 -# define PINK6 6 -# define PINK5 5 -# define PINK4 4 -# define PINK3 3 -# define PINK2 2 -# define PINK1 1 -# define PINK0 0 - -# define DDRK _SFR_MEM8(0x107) -# define DDK7 7 -# define DDK6 6 -# define DDK5 5 -# define DDK4 4 -# define DDK3 3 -# define DDK2 2 -# define DDK1 1 -# define DDK0 0 - -# define PORTK _SFR_MEM8(0x108) -# define PK7 7 -# define PK6 6 -# define PK5 5 -# define PK4 4 -# define PK3 3 -# define PK2 2 -# define PK1 1 -# define PK0 0 - -# define PINL _SFR_MEM8(0x109) -# define PINL7 7 -# define PINL6 6 -# define PINL5 5 -# define PINL4 4 -# define PINL3 3 -# define PINL2 2 -# define PINL1 1 -# define PINL0 0 - -# define DDRL _SFR_MEM8(0x10A) -# define DDL7 7 -# define DDL6 6 -# define DDL5 5 -# define DDL4 4 -# define DDL3 3 -# define DDL2 2 -# define DDL1 1 -# define DDL0 0 - -# define PORTL _SFR_MEM8(0x10B) -# define PL7 7 -# define PL6 6 -# define PL5 5 -# define PL4 4 -# define PL3 3 -# define PL2 2 -# define PL1 1 -# define PL0 0 - -#endif /* __ATmegaxx0__ */ - -/* Reserved [0x10C..0x11F] */ - -#define TCCR5A _SFR_MEM8(0x120) -#define COM5A1 7 -#define COM5A0 6 -#define COM5B1 5 -#define COM5B0 4 -#define COM5C1 3 -#define COM5C0 2 -#define WGM51 1 -#define WGM50 0 - -#define TCCR5B _SFR_MEM8(0x121) -#define ICNC5 7 -#define ICES5 6 -#define WGM53 4 -#define WGM52 3 -#define CS52 2 -#define CS51 1 -#define CS50 0 - -#define TCCR5C _SFR_MEM8(0x122) -#define FOC5A 7 -#define FOC5B 6 -#define FOC5C 5 - -/* Reserved [0x123] */ - -/* Combine TCNT5L and TCNT5H */ -#define TCNT5 _SFR_MEM16(0x124) - -#define TCNT5L _SFR_MEM8(0x124) -#define TCNT5H _SFR_MEM8(0x125) - -/* Combine ICR5L and ICR5H */ -#define ICR5 _SFR_MEM16(0x126) - -#define ICR5L _SFR_MEM8(0x126) -#define ICR5H _SFR_MEM8(0x127) - -/* Combine OCR5AL and OCR5AH */ -#define OCR5A _SFR_MEM16(0x128) - -#define OCR5AL _SFR_MEM8(0x128) -#define OCR5AH _SFR_MEM8(0x129) - -/* Combine OCR5BL and OCR5BH */ -#define OCR5B _SFR_MEM16(0x12A) - -#define OCR5BL _SFR_MEM8(0x12A) -#define OCR5BH _SFR_MEM8(0x12B) - -/* Combine OCR5CL and OCR5CH */ -#define OCR5C _SFR_MEM16(0x12C) - -#define OCR5CL _SFR_MEM8(0x12C) -#define OCR5CH _SFR_MEM8(0x12D) - -/* Reserved [0x12E..0x12F] */ - -#if defined(__ATmegaxx0__) - -# define UCSR3A _SFR_MEM8(0x130) -# define RXC3 7 -# define TXC3 6 -# define UDRE3 5 -# define FE3 4 -# define DOR3 3 -# define UPE3 2 -# define U2X3 1 -# define MPCM3 0 - -# define UCSR3B _SFR_MEM8(0X131) -# define RXCIE3 7 -# define TXCIE3 6 -# define UDRIE3 5 -# define RXEN3 4 -# define TXEN3 3 -# define UCSZ32 2 -# define RXB83 1 -# define TXB83 0 - -# define UCSR3C _SFR_MEM8(0x132) -# define UMSEL31 7 -# define UMSEL30 6 -# define UPM31 5 -# define UPM30 4 -# define USBS3 3 -# define UCSZ31 2 -# define UCSZ30 1 -# define UCPOL3 0 - -/* Reserved [0x133] */ - -/* Combine UBRR3L and UBRR3H */ -# define UBRR3 _SFR_MEM16(0x134) - -# define UBRR3L _SFR_MEM8(0x134) -# define UBRR3H _SFR_MEM8(0x135) - -# define UDR3 _SFR_MEM8(0X136) - -#endif /* __ATmegaxx0__ */ - -/* Reserved [0x137..1FF] */ - -/* Interrupt vectors */ -/* Vector 0 is the reset vector */ -/* External Interrupt Request 0 */ -#define INT0_vect _VECTOR(1) -#define SIG_INTERRUPT0 _VECTOR(1) - -/* External Interrupt Request 1 */ -#define INT1_vect _VECTOR(2) -#define SIG_INTERRUPT1 _VECTOR(2) - -/* External Interrupt Request 2 */ -#define INT2_vect _VECTOR(3) -#define SIG_INTERRUPT2 _VECTOR(3) - -/* External Interrupt Request 3 */ -#define INT3_vect _VECTOR(4) -#define SIG_INTERRUPT3 _VECTOR(4) - -/* External Interrupt Request 4 */ -#define INT4_vect _VECTOR(5) -#define SIG_INTERRUPT4 _VECTOR(5) - -/* External Interrupt Request 5 */ -#define INT5_vect _VECTOR(6) -#define SIG_INTERRUPT5 _VECTOR(6) - -/* External Interrupt Request 6 */ -#define INT6_vect _VECTOR(7) -#define SIG_INTERRUPT6 _VECTOR(7) - -/* External Interrupt Request 7 */ -#define INT7_vect _VECTOR(8) -#define SIG_INTERRUPT7 _VECTOR(8) - -/* Pin Change Interrupt Request 0 */ -#define PCINT0_vect _VECTOR(9) -#define SIG_PIN_CHANGE0 _VECTOR(9) - -/* Pin Change Interrupt Request 1 */ -#define PCINT1_vect _VECTOR(10) -#define SIG_PIN_CHANGE1 _VECTOR(10) - -#if defined(__ATmegaxx0__) -/* Pin Change Interrupt Request 2 */ -#define PCINT2_vect _VECTOR(11) -#define SIG_PIN_CHANGE2 _VECTOR(11) - -#endif /* __ATmegaxx0__ */ - -/* Watchdog Time-out Interrupt */ -#define WDT_vect _VECTOR(12) -#define SIG_WATCHDOG_TIMEOUT _VECTOR(12) - -/* Timer/Counter2 Compare Match A */ -#define TIMER2_COMPA_vect _VECTOR(13) -#define SIG_OUTPUT_COMPARE2A _VECTOR(13) - -/* Timer/Counter2 Compare Match B */ -#define TIMER2_COMPB_vect _VECTOR(14) -#define SIG_OUTPUT_COMPARE2B _VECTOR(14) - -/* Timer/Counter2 Overflow */ -#define TIMER2_OVF_vect _VECTOR(15) -#define SIG_OVERFLOW2 _VECTOR(15) - -/* Timer/Counter1 Capture Event */ -#define TIMER1_CAPT_vect _VECTOR(16) -#define SIG_INPUT_CAPTURE1 _VECTOR(16) - -/* Timer/Counter1 Compare Match A */ -#define TIMER1_COMPA_vect _VECTOR(17) -#define SIG_OUTPUT_COMPARE1A _VECTOR(17) - -/* Timer/Counter1 Compare Match B */ -#define TIMER1_COMPB_vect _VECTOR(18) -#define SIG_OUTPUT_COMPARE1B _VECTOR(18) - -/* Timer/Counter1 Compare Match C */ -#define TIMER1_COMPC_vect _VECTOR(19) -#define SIG_OUTPUT_COMPARE1C _VECTOR(19) - -/* Timer/Counter1 Overflow */ -#define TIMER1_OVF_vect _VECTOR(20) -#define SIG_OVERFLOW1 _VECTOR(20) - -/* Timer/Counter0 Compare Match A */ -#define TIMER0_COMPA_vect _VECTOR(21) -#define SIG_OUTPUT_COMPARE0A _VECTOR(21) - -/* Timer/Counter0 Compare Match B */ -#define TIMER0_COMPB_vect _VECTOR(22) -#define SIG_OUTPUT_COMPARE0B _VECTOR(22) - -/* Timer/Counter0 Overflow */ -#define TIMER0_OVF_vect _VECTOR(23) -#define SIG_OVERFLOW0 _VECTOR(23) - -/* SPI Serial Transfer Complete */ -#define SPI_STC_vect _VECTOR(24) -#define SIG_SPI _VECTOR(24) - -/* USART0, Rx Complete */ -#define USART0_RX_vect _VECTOR(25) -#define SIG_USART0_RECV _VECTOR(25) - -/* USART0 Data register Empty */ -#define USART0_UDRE_vect _VECTOR(26) -#define SIG_USART0_DATA _VECTOR(26) - -/* USART0, Tx Complete */ -#define USART0_TX_vect _VECTOR(27) -#define SIG_USART0_TRANS _VECTOR(27) - -/* Analog Comparator */ -#define ANALOG_COMP_vect _VECTOR(28) -#define SIG_COMPARATOR _VECTOR(28) - -/* ADC Conversion Complete */ -#define ADC_vect _VECTOR(29) -#define SIG_ADC _VECTOR(29) - -/* EEPROM Ready */ -#define EE_READY_vect _VECTOR(30) -#define SIG_EEPROM_READY _VECTOR(30) - -/* Timer/Counter3 Capture Event */ -#define TIMER3_CAPT_vect _VECTOR(31) -#define SIG_INPUT_CAPTURE3 _VECTOR(31) - -/* Timer/Counter3 Compare Match A */ -#define TIMER3_COMPA_vect _VECTOR(32) -#define SIG_OUTPUT_COMPARE3A _VECTOR(32) - -/* Timer/Counter3 Compare Match B */ -#define TIMER3_COMPB_vect _VECTOR(33) -#define SIG_OUTPUT_COMPARE3B _VECTOR(33) - -/* Timer/Counter3 Compare Match C */ -#define TIMER3_COMPC_vect _VECTOR(34) -#define SIG_OUTPUT_COMPARE3C _VECTOR(34) - -/* Timer/Counter3 Overflow */ -#define TIMER3_OVF_vect _VECTOR(35) -#define SIG_OVERFLOW3 _VECTOR(35) - -/* USART1, Rx Complete */ -#define USART1_RX_vect _VECTOR(36) -#define SIG_USART1_RECV _VECTOR(36) - -/* USART1 Data register Empty */ -#define USART1_UDRE_vect _VECTOR(37) -#define SIG_USART1_DATA _VECTOR(37) - -/* USART1, Tx Complete */ -#define USART1_TX_vect _VECTOR(38) -#define SIG_USART1_TRANS _VECTOR(38) - -/* 2-wire Serial Interface */ -#define TWI_vect _VECTOR(39) -#define SIG_2WIRE_SERIAL _VECTOR(39) - -/* Store Program Memory Read */ -#define SPM_READY_vect _VECTOR(40) -#define SIG_SPM_READY _VECTOR(40) - -#if defined(__ATmegaxx0__) -/* Timer/Counter4 Capture Event */ -#define TIMER4_CAPT_vect _VECTOR(41) -#define SIG_INPUT_CAPTURE4 _VECTOR(41) - -#endif /* __ATmegaxx0__ */ - -/* Timer/Counter4 Compare Match A */ -#define TIMER4_COMPA_vect _VECTOR(42) -#define SIG_OUTPUT_COMPARE4A _VECTOR(42) - -/* Timer/Counter4 Compare Match B */ -#define TIMER4_COMPB_vect _VECTOR(43) -#define SIG_OUTPUT_COMPARE4B _VECTOR(43) - -/* Timer/Counter4 Compare Match C */ -#define TIMER4_COMPC_vect _VECTOR(44) -#define SIG_OUTPUT_COMPARE4C _VECTOR(44) - -/* Timer/Counter4 Overflow */ -#define TIMER4_OVF_vect _VECTOR(45) -#define SIG_OVERFLOW4 _VECTOR(45) - -#if defined(__ATmegaxx0__) -/* Timer/Counter5 Capture Event */ -#define TIMER5_CAPT_vect _VECTOR(46) -#define SIG_INPUT_CAPTURE5 _VECTOR(46) - -#endif /* __ATmegaxx0__ */ - -/* Timer/Counter5 Compare Match A */ -#define TIMER5_COMPA_vect _VECTOR(47) -#define SIG_OUTPUT_COMPARE5A _VECTOR(47) - -/* Timer/Counter5 Compare Match B */ -#define TIMER5_COMPB_vect _VECTOR(48) -#define SIG_OUTPUT_COMPARE5B _VECTOR(48) - -/* Timer/Counter5 Compare Match C */ -#define TIMER5_COMPC_vect _VECTOR(49) -#define SIG_OUTPUT_COMPARE5C _VECTOR(49) - -/* Timer/Counter5 Overflow */ -#define TIMER5_OVF_vect _VECTOR(50) -#define SIG_OVERFLOW5 _VECTOR(50) - -#if defined(__ATmegaxx1__) - -# define _VECTORS_SIZE 204 - -#else - -/* USART2, Rx Complete */ -#define USART2_RX_vect _VECTOR(51) -#define SIG_USART2_RECV _VECTOR(51) - -/* USART2 Data register Empty */ -#define USART2_UDRE_vect _VECTOR(52) -#define SIG_USART2_DATA _VECTOR(52) - -/* USART2, Tx Complete */ -#define USART2_TX_vect _VECTOR(53) -#define SIG_USART2_TRANS _VECTOR(53) - -/* USART3, Rx Complete */ -#define USART3_RX_vect _VECTOR(54) -#define SIG_USART3_RECV _VECTOR(54) - -/* USART3 Data register Empty */ -#define USART3_UDRE_vect _VECTOR(55) -#define SIG_USART3_DATA _VECTOR(55) - -/* USART3, Tx Complete */ -#define USART3_TX_vect _VECTOR(56) -#define SIG_USART3_TRANS _VECTOR(56) - -# define _VECTORS_SIZE 228 - -#endif /* __ATmegaxx1__ */ - -#if defined(__ATmegaxx0__) -# undef __ATmegaxx0__ -#endif - -#if defined(__ATmegaxx1__) -# undef __ATmegaxx1__ -#endif - -#endif /* _AVR_IOMXX0_1_H_ */ diff --git a/libraries/Desktop/include/avr/pgmspace.h b/libraries/Desktop/include/avr/pgmspace.h deleted file mode 100644 index 86e937738e..0000000000 --- a/libraries/Desktop/include/avr/pgmspace.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef _PGMSPACE_H -#define _PGMSPACE_H - -#include -#include -#include - -#ifndef __ATTR_PROGMEM__ -#define __ATTR_PROGMEM__ __attribute__(()) -#endif - -#ifndef __ATTR_PURE__ -#define __ATTR_PURE__ __attribute__((__pure__)) -#endif - -#undef PROGMEM -#define PROGMEM __ATTR_PROGMEM__ - -#ifndef PGM_P -#define PGM_P const prog_char * -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -typedef char PROGMEM prog_char; -extern int strcasecmp_P(const char *, PGM_P) __ATTR_PURE__; -extern int strcmp_P(const char *, PGM_P) __ATTR_PURE__; -extern int strncmp_P(const char *, PGM_P, size_t n) __ATTR_PURE__; -extern size_t strlcat_P (char *, PGM_P, size_t ); -extern size_t strnlen_P (PGM_P, size_t ); -extern size_t strlen_P (PGM_P); -extern size_t strlen_P (PGM_P); -extern char *strncpy_P(char *dest, PGM_P src, size_t n); -extern void *memcpy_P(void *dest, PGM_P src, size_t n); - -static inline uint8_t pgm_read_byte(PGM_P s) { return (uint8_t)*s; } - static inline uint8_t pgm_read_byte_far(const void *s) { return *(const uint8_t *)s; } -static inline uint16_t pgm_read_word(const void *s) { return *(const uint16_t *)s; } -static inline uint32_t pgm_read_dword(const void *s) { return *(const uint32_t *)s; } -static inline float pgm_read_float(const void *s) { return *(const float *)s; } - -#define GETBYTE(flag, mask, pnt) ({ \ - unsigned char __c; \ - __c = ((flag) & (mask)) \ - ? pgm_read_byte(pnt) : *pnt; \ - pnt++; \ - __c; \ -}) - - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/libraries/Desktop/include/avr/wdt.h b/libraries/Desktop/include/avr/wdt.h deleted file mode 100644 index d0a7f431c4..0000000000 --- a/libraries/Desktop/include/avr/wdt.h +++ /dev/null @@ -1,4 +0,0 @@ - -#define WDTO_15MS 15 - -#define wdt_enable(x) diff --git a/libraries/Desktop/include/avr/wiring.h b/libraries/Desktop/include/avr/wiring.h deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/libraries/Desktop/include/binary.h b/libraries/Desktop/include/binary.h deleted file mode 100644 index af1498033a..0000000000 --- a/libraries/Desktop/include/binary.h +++ /dev/null @@ -1,515 +0,0 @@ -#ifndef Binary_h -#define Binary_h - -#define B0 0 -#define B00 0 -#define B000 0 -#define B0000 0 -#define B00000 0 -#define B000000 0 -#define B0000000 0 -#define B00000000 0 -#define B1 1 -#define B01 1 -#define B001 1 -#define B0001 1 -#define B00001 1 -#define B000001 1 -#define B0000001 1 -#define B00000001 1 -#define B10 2 -#define B010 2 -#define B0010 2 -#define B00010 2 -#define B000010 2 -#define B0000010 2 -#define B00000010 2 -#define B11 3 -#define B011 3 -#define B0011 3 -#define B00011 3 -#define B000011 3 -#define B0000011 3 -#define B00000011 3 -#define B100 4 -#define B0100 4 -#define B00100 4 -#define B000100 4 -#define B0000100 4 -#define B00000100 4 -#define B101 5 -#define B0101 5 -#define B00101 5 -#define B000101 5 -#define B0000101 5 -#define B00000101 5 -#define B110 6 -#define B0110 6 -#define B00110 6 -#define B000110 6 -#define B0000110 6 -#define B00000110 6 -#define B111 7 -#define B0111 7 -#define B00111 7 -#define B000111 7 -#define B0000111 7 -#define B00000111 7 -#define B1000 8 -#define B01000 8 -#define B001000 8 -#define B0001000 8 -#define B00001000 8 -#define B1001 9 -#define B01001 9 -#define B001001 9 -#define B0001001 9 -#define B00001001 9 -#define B1010 10 -#define B01010 10 -#define B001010 10 -#define B0001010 10 -#define B00001010 10 -#define B1011 11 -#define B01011 11 -#define B001011 11 -#define B0001011 11 -#define B00001011 11 -#define B1100 12 -#define B01100 12 -#define B001100 12 -#define B0001100 12 -#define B00001100 12 -#define B1101 13 -#define B01101 13 -#define B001101 13 -#define B0001101 13 -#define B00001101 13 -#define B1110 14 -#define B01110 14 -#define B001110 14 -#define B0001110 14 -#define B00001110 14 -#define B1111 15 -#define B01111 15 -#define B001111 15 -#define B0001111 15 -#define B00001111 15 -#define B10000 16 -#define B010000 16 -#define B0010000 16 -#define B00010000 16 -#define B10001 17 -#define B010001 17 -#define B0010001 17 -#define B00010001 17 -#define B10010 18 -#define B010010 18 -#define B0010010 18 -#define B00010010 18 -#define B10011 19 -#define B010011 19 -#define B0010011 19 -#define B00010011 19 -#define B10100 20 -#define B010100 20 -#define B0010100 20 -#define B00010100 20 -#define B10101 21 -#define B010101 21 -#define B0010101 21 -#define B00010101 21 -#define B10110 22 -#define B010110 22 -#define B0010110 22 -#define B00010110 22 -#define B10111 23 -#define B010111 23 -#define B0010111 23 -#define B00010111 23 -#define B11000 24 -#define B011000 24 -#define B0011000 24 -#define B00011000 24 -#define B11001 25 -#define B011001 25 -#define B0011001 25 -#define B00011001 25 -#define B11010 26 -#define B011010 26 -#define B0011010 26 -#define B00011010 26 -#define B11011 27 -#define B011011 27 -#define B0011011 27 -#define B00011011 27 -#define B11100 28 -#define B011100 28 -#define B0011100 28 -#define B00011100 28 -#define B11101 29 -#define B011101 29 -#define B0011101 29 -#define B00011101 29 -#define B11110 30 -#define B011110 30 -#define B0011110 30 -#define B00011110 30 -#define B11111 31 -#define B011111 31 -#define B0011111 31 -#define B00011111 31 -#define B100000 32 -#define B0100000 32 -#define B00100000 32 -#define B100001 33 -#define B0100001 33 -#define B00100001 33 -#define B100010 34 -#define B0100010 34 -#define B00100010 34 -#define B100011 35 -#define B0100011 35 -#define B00100011 35 -#define B100100 36 -#define B0100100 36 -#define B00100100 36 -#define B100101 37 -#define B0100101 37 -#define B00100101 37 -#define B100110 38 -#define B0100110 38 -#define B00100110 38 -#define B100111 39 -#define B0100111 39 -#define B00100111 39 -#define B101000 40 -#define B0101000 40 -#define B00101000 40 -#define B101001 41 -#define B0101001 41 -#define B00101001 41 -#define B101010 42 -#define B0101010 42 -#define B00101010 42 -#define B101011 43 -#define B0101011 43 -#define B00101011 43 -#define B101100 44 -#define B0101100 44 -#define B00101100 44 -#define B101101 45 -#define B0101101 45 -#define B00101101 45 -#define B101110 46 -#define B0101110 46 -#define B00101110 46 -#define B101111 47 -#define B0101111 47 -#define B00101111 47 -#define B110000 48 -#define B0110000 48 -#define B00110000 48 -#define B110001 49 -#define B0110001 49 -#define B00110001 49 -#define B110010 50 -#define B0110010 50 -#define B00110010 50 -#define B110011 51 -#define B0110011 51 -#define B00110011 51 -#define B110100 52 -#define B0110100 52 -#define B00110100 52 -#define B110101 53 -#define B0110101 53 -#define B00110101 53 -#define B110110 54 -#define B0110110 54 -#define B00110110 54 -#define B110111 55 -#define B0110111 55 -#define B00110111 55 -#define B111000 56 -#define B0111000 56 -#define B00111000 56 -#define B111001 57 -#define B0111001 57 -#define B00111001 57 -#define B111010 58 -#define B0111010 58 -#define B00111010 58 -#define B111011 59 -#define B0111011 59 -#define B00111011 59 -#define B111100 60 -#define B0111100 60 -#define B00111100 60 -#define B111101 61 -#define B0111101 61 -#define B00111101 61 -#define B111110 62 -#define B0111110 62 -#define B00111110 62 -#define B111111 63 -#define B0111111 63 -#define B00111111 63 -#define B1000000 64 -#define B01000000 64 -#define B1000001 65 -#define B01000001 65 -#define B1000010 66 -#define B01000010 66 -#define B1000011 67 -#define B01000011 67 -#define B1000100 68 -#define B01000100 68 -#define B1000101 69 -#define B01000101 69 -#define B1000110 70 -#define B01000110 70 -#define B1000111 71 -#define B01000111 71 -#define B1001000 72 -#define B01001000 72 -#define B1001001 73 -#define B01001001 73 -#define B1001010 74 -#define B01001010 74 -#define B1001011 75 -#define B01001011 75 -#define B1001100 76 -#define B01001100 76 -#define B1001101 77 -#define B01001101 77 -#define B1001110 78 -#define B01001110 78 -#define B1001111 79 -#define B01001111 79 -#define B1010000 80 -#define B01010000 80 -#define B1010001 81 -#define B01010001 81 -#define B1010010 82 -#define B01010010 82 -#define B1010011 83 -#define B01010011 83 -#define B1010100 84 -#define B01010100 84 -#define B1010101 85 -#define B01010101 85 -#define B1010110 86 -#define B01010110 86 -#define B1010111 87 -#define B01010111 87 -#define B1011000 88 -#define B01011000 88 -#define B1011001 89 -#define B01011001 89 -#define B1011010 90 -#define B01011010 90 -#define B1011011 91 -#define B01011011 91 -#define B1011100 92 -#define B01011100 92 -#define B1011101 93 -#define B01011101 93 -#define B1011110 94 -#define B01011110 94 -#define B1011111 95 -#define B01011111 95 -#define B1100000 96 -#define B01100000 96 -#define B1100001 97 -#define B01100001 97 -#define B1100010 98 -#define B01100010 98 -#define B1100011 99 -#define B01100011 99 -#define B1100100 100 -#define B01100100 100 -#define B1100101 101 -#define B01100101 101 -#define B1100110 102 -#define B01100110 102 -#define B1100111 103 -#define B01100111 103 -#define B1101000 104 -#define B01101000 104 -#define B1101001 105 -#define B01101001 105 -#define B1101010 106 -#define B01101010 106 -#define B1101011 107 -#define B01101011 107 -#define B1101100 108 -#define B01101100 108 -#define B1101101 109 -#define B01101101 109 -#define B1101110 110 -#define B01101110 110 -#define B1101111 111 -#define B01101111 111 -#define B1110000 112 -#define B01110000 112 -#define B1110001 113 -#define B01110001 113 -#define B1110010 114 -#define B01110010 114 -#define B1110011 115 -#define B01110011 115 -#define B1110100 116 -#define B01110100 116 -#define B1110101 117 -#define B01110101 117 -#define B1110110 118 -#define B01110110 118 -#define B1110111 119 -#define B01110111 119 -#define B1111000 120 -#define B01111000 120 -#define B1111001 121 -#define B01111001 121 -#define B1111010 122 -#define B01111010 122 -#define B1111011 123 -#define B01111011 123 -#define B1111100 124 -#define B01111100 124 -#define B1111101 125 -#define B01111101 125 -#define B1111110 126 -#define B01111110 126 -#define B1111111 127 -#define B01111111 127 -#define B10000000 128 -#define B10000001 129 -#define B10000010 130 -#define B10000011 131 -#define B10000100 132 -#define B10000101 133 -#define B10000110 134 -#define B10000111 135 -#define B10001000 136 -#define B10001001 137 -#define B10001010 138 -#define B10001011 139 -#define B10001100 140 -#define B10001101 141 -#define B10001110 142 -#define B10001111 143 -#define B10010000 144 -#define B10010001 145 -#define B10010010 146 -#define B10010011 147 -#define B10010100 148 -#define B10010101 149 -#define B10010110 150 -#define B10010111 151 -#define B10011000 152 -#define B10011001 153 -#define B10011010 154 -#define B10011011 155 -#define B10011100 156 -#define B10011101 157 -#define B10011110 158 -#define B10011111 159 -#define B10100000 160 -#define B10100001 161 -#define B10100010 162 -#define B10100011 163 -#define B10100100 164 -#define B10100101 165 -#define B10100110 166 -#define B10100111 167 -#define B10101000 168 -#define B10101001 169 -#define B10101010 170 -#define B10101011 171 -#define B10101100 172 -#define B10101101 173 -#define B10101110 174 -#define B10101111 175 -#define B10110000 176 -#define B10110001 177 -#define B10110010 178 -#define B10110011 179 -#define B10110100 180 -#define B10110101 181 -#define B10110110 182 -#define B10110111 183 -#define B10111000 184 -#define B10111001 185 -#define B10111010 186 -#define B10111011 187 -#define B10111100 188 -#define B10111101 189 -#define B10111110 190 -#define B10111111 191 -#define B11000000 192 -#define B11000001 193 -#define B11000010 194 -#define B11000011 195 -#define B11000100 196 -#define B11000101 197 -#define B11000110 198 -#define B11000111 199 -#define B11001000 200 -#define B11001001 201 -#define B11001010 202 -#define B11001011 203 -#define B11001100 204 -#define B11001101 205 -#define B11001110 206 -#define B11001111 207 -#define B11010000 208 -#define B11010001 209 -#define B11010010 210 -#define B11010011 211 -#define B11010100 212 -#define B11010101 213 -#define B11010110 214 -#define B11010111 215 -#define B11011000 216 -#define B11011001 217 -#define B11011010 218 -#define B11011011 219 -#define B11011100 220 -#define B11011101 221 -#define B11011110 222 -#define B11011111 223 -#define B11100000 224 -#define B11100001 225 -#define B11100010 226 -#define B11100011 227 -#define B11100100 228 -#define B11100101 229 -#define B11100110 230 -#define B11100111 231 -#define B11101000 232 -#define B11101001 233 -#define B11101010 234 -#define B11101011 235 -#define B11101100 236 -#define B11101101 237 -#define B11101110 238 -#define B11101111 239 -#define B11110000 240 -#define B11110001 241 -#define B11110010 242 -#define B11110011 243 -#define B11110100 244 -#define B11110101 245 -#define B11110110 246 -#define B11110111 247 -#define B11111000 248 -#define B11111001 249 -#define B11111010 250 -#define B11111011 251 -#define B11111100 252 -#define B11111101 253 -#define B11111110 254 -#define B11111111 255 - -#endif diff --git a/libraries/Desktop/include/pins_arduino.h b/libraries/Desktop/include/pins_arduino.h deleted file mode 100644 index 7bfa0f09aa..0000000000 --- a/libraries/Desktop/include/pins_arduino.h +++ /dev/null @@ -1,88 +0,0 @@ -/* - pins_arduino.h - Pin definition functions for Arduino - Part of Arduino - http://www.arduino.cc/ - - Copyright (c) 2007 David A. Mellis - - 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 - - $Id: wiring.h 249 2007-02-03 16:52:51Z mellis $ -*/ - -#ifndef Pins_Arduino_h -#define Pins_Arduino_h - -#include - -#define NOT_A_PIN 0 -#define NOT_A_PORT 0 - -#define NOT_ON_TIMER 0 -#define TIMER0A 1 -#define TIMER0B 2 -#define TIMER1A 3 -#define TIMER1B 4 -#define TIMER2 5 -#define TIMER2A 6 -#define TIMER2B 7 - -#define TIMER3A 8 -#define TIMER3B 9 -#define TIMER3C 10 -#define TIMER4A 11 -#define TIMER4B 12 -#define TIMER4C 13 -#define TIMER5A 14 -#define TIMER5B 15 -#define TIMER5C 16 - -#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(DESKTOP_BUILD) -const static uint8_t SS = 53; -const static uint8_t MOSI = 51; -const static uint8_t MISO = 50; -const static uint8_t SCK = 52; -#else -const static uint8_t SS = 10; -const static uint8_t MOSI = 11; -const static uint8_t MISO = 12; -const static uint8_t SCK = 13; -#endif - -// On the ATmega1280, the addresses of some of the port registers are -// greater than 255, so we can't store them in uint8_t's. -extern const uint16_t PROGMEM port_to_mode_PGM[]; -extern const uint16_t PROGMEM port_to_input_PGM[]; -extern const uint16_t PROGMEM port_to_output_PGM[]; - -extern const uint8_t PROGMEM digital_pin_to_port_PGM[]; -// extern const uint8_t PROGMEM digital_pin_to_bit_PGM[]; -extern const uint8_t PROGMEM digital_pin_to_bit_mask_PGM[]; -extern const uint8_t PROGMEM digital_pin_to_timer_PGM[]; - -// Get the bit location within the hardware port of the given virtual pin. -// This comes from the pins_*.c file for the active board configuration. -// -// These perform slightly better as macros compared to inline functions -// -#define digitalPinToPort(P) ( pgm_read_byte( digital_pin_to_port_PGM + (P) ) ) -#define digitalPinToBitMask(P) ( pgm_read_byte( digital_pin_to_bit_mask_PGM + (P) ) ) -#define digitalPinToTimer(P) ( pgm_read_byte( digital_pin_to_timer_PGM + (P) ) ) -#define analogInPinToBit(P) (P) -#define portOutputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_output_PGM + (P))) ) -#define portInputRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_input_PGM + (P))) ) -#define portModeRegister(P) ( (volatile uint8_t *)( pgm_read_word( port_to_mode_PGM + (P))) ) - -#endif diff --git a/libraries/Desktop/include/wiring.h b/libraries/Desktop/include/wiring.h deleted file mode 100644 index 462c2fa5af..0000000000 --- a/libraries/Desktop/include/wiring.h +++ /dev/null @@ -1,136 +0,0 @@ -/* - wiring.h - Partial implementation of the Wiring API for the ATmega8. - Part of Arduino - http://www.arduino.cc/ - - Copyright (c) 2005-2006 David A. Mellis - - 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 - - $Id$ -*/ - -#ifndef Wiring_h -#define Wiring_h - -#include -#include -#include "binary.h" - -#ifdef __cplusplus -extern "C"{ -#endif - -#define HIGH 0x1 -#define LOW 0x0 - -#define INPUT 0x0 -#define OUTPUT 0x1 - -#define true 0x1 -#define false 0x0 - -#define PI 3.1415926535897932384626433832795 -#define HALF_PI 1.5707963267948966192313216916398 -#define TWO_PI 6.283185307179586476925286766559 -#define DEG_TO_RAD 0.017453292519943295769236907684886 -#define RAD_TO_DEG 57.295779513082320876798154814105 - -#define SERIAL 0x0 -#define DISPLAY 0x1 - -#define LSBFIRST 0 -#define MSBFIRST 1 - -#define CHANGE 1 -#define FALLING 2 -#define RISING 3 - -#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(DESKTOP_BUILD) -#define INTERNAL1V1 2 -#define INTERNAL2V56 3 -#else -#define INTERNAL 3 -#endif -#define DEFAULT 1 -#define EXTERNAL 0 - -// undefine stdlib's abs if encountered -#ifdef abs -#undef abs -#endif - -#define min(a,b) ((a)<(b)?(a):(b)) -#define max(a,b) ((a)>(b)?(a):(b)) -#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) -//#define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) -#define radians(deg) ((deg)*DEG_TO_RAD) -#define degrees(rad) ((rad)*RAD_TO_DEG) -#define sq(x) ((x)*(x)) - -#define interrupts() sei() -#define noInterrupts() cli() - -#define clockCyclesPerMicrosecond() ( F_CPU / 1000000L ) -#define clockCyclesToMicroseconds(a) ( ((a) * 1000L) / (F_CPU / 1000L) ) -#define microsecondsToClockCycles(a) ( ((a) * (F_CPU / 1000L)) / 1000L ) - -#define lowByte(w) ((uint8_t) ((w) & 0xff)) -#define highByte(w) ((uint8_t) ((w) >> 8)) - -#define bitRead(value, bit) (((value) >> (bit)) & 0x01) -#define bitSet(value, bit) ((value) |= (1UL << (bit))) -#define bitClear(value, bit) ((value) &= ~(1UL << (bit))) -#define bitWrite(value, bit, bitvalue) (bitvalue ? bitSet(value, bit) : bitClear(value, bit)) -#define bit_is_set(value, bit) (((value) & bit) != 0) - -typedef unsigned int word; - -#define bit(b) (1UL << (b)) - -typedef uint8_t boolean; -typedef uint8_t byte; - -void init(void); - -void pinMode(uint8_t, uint8_t); -void digitalWrite(uint8_t, uint8_t); -int digitalRead(uint8_t); -void analogReference(uint8_t mode); -void analogWrite(uint8_t, int); - -unsigned long millis(void); -unsigned long micros(void); -void delay(unsigned long); -void delayMicroseconds(unsigned int us); -unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout); - -void shiftOut(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder, uint8_t val); -uint8_t shiftIn(uint8_t dataPin, uint8_t clockPin, uint8_t bitOrder); - -void attachInterrupt(uint8_t, void (*)(void), int mode); -void detachInterrupt(uint8_t); - -int abs(int v); - -#ifdef __cplusplus -} // extern "C" -#endif - -void setup(void); -void loop(void); -long map(long , long , long , long , long ); - -#endif diff --git a/libraries/Desktop/support/Arduino.cpp b/libraries/Desktop/support/Arduino.cpp deleted file mode 100644 index 8671e7860d..0000000000 --- a/libraries/Desktop/support/Arduino.cpp +++ /dev/null @@ -1,214 +0,0 @@ -#include "WProgram.h" -#include -#include -#include -#include "avr/pgmspace.h" -#include -#include -#include -#include "desktop.h" - -extern "C" { - -volatile uint8_t __iomem[1024]; -volatile uint8_t SREG = 0x80; - -unsigned __brkval = 0x2000; -unsigned __bss_end = 0x1000; - -// disable interrupts -void cli(void) -{ - SREG &= ~0x80; -} - -// enable interrupts -void sei(void) -{ - SREG |= 0x80; -} - -void pinMode(uint8_t pin, uint8_t mode) -{ - -} - - -long unsigned int millis(void) -{ - struct timeval tp; - gettimeofday(&tp,NULL); - return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - - (desktop_state.sketch_start_time.tv_sec + - (desktop_state.sketch_start_time.tv_usec*1.0e-6))); -} - -long unsigned int micros(void) -{ - struct timeval tp; - gettimeofday(&tp,NULL); - return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - - (desktop_state.sketch_start_time.tv_sec + - (desktop_state.sketch_start_time.tv_usec*1.0e-6))); -} - -void delayMicroseconds(unsigned usec) -{ - uint32_t start = micros(); - while (micros() - start < usec) { - usleep(usec - (micros() - start)); - } -} - -void delay(long unsigned msec) -{ - delayMicroseconds(msec*1000); -} - -size_t strlcat_P(char *d, PGM_P s, size_t bufsize) -{ - size_t len1 = strlen(d); - size_t len2 = strlen(s); - size_t ret = len1 + len2; - - if (len1+len2 >= bufsize) { - if (bufsize < (len1+1)) { - return ret; - } - len2 = bufsize - (len1+1); - } - if (len2 > 0) { - memcpy(d+len1, s, len2); - d[len1+len2] = 0; - } - return ret; -} - -size_t strnlen_P(PGM_P str, size_t size) -{ - return strnlen(str, size); -} - -size_t strlen_P(PGM_P str) -{ - return strlen(str); -} - -int strcasecmp_P(PGM_P str1, PGM_P str2) -{ - return strcasecmp(str1, str2); -} - -int strcmp_P(PGM_P str1, PGM_P str2) -{ - return strcmp(str1, str2); -} - -int strncmp_P(PGM_P str1, PGM_P str2, size_t n) -{ - return strncmp(str1, str2, n); -} - -char *strncpy_P(char *dest, PGM_P src, size_t n) -{ - return strncpy(dest, src, n); -} - -void *memcpy_P(void *dest, PGM_P src, size_t n) -{ - return memcpy(dest, src, n); -} - - -void digitalWrite(uint8_t pin, uint8_t val) -{ -} - -} - - -char *itoa(int __val, char *__s, int __radix) -{ - switch (__radix) { - case 8: - sprintf(__s, "%o", __val); - break; - case 16: - sprintf(__s, "%x", __val); - break; - case 10: - default: - sprintf(__s, "%d", __val); - break; - } - return __s; -} - -char *ultoa(unsigned long __val, char *__s, int __radix) -{ - switch (__radix) { - case 8: - sprintf(__s, "%lo", __val); - break; - case 16: - sprintf(__s, "%lx", __val); - break; - case 10: - default: - sprintf(__s, "%lu", __val); - break; - } - return __s; -} - -char *ltoa(long __val, char *__s, int __radix) -{ - switch (__radix) { - case 8: - sprintf(__s, "%lo", __val); - break; - case 16: - sprintf(__s, "%lx", __val); - break; - case 10: - default: - sprintf(__s, "%ld", __val); - break; - } - return __s; -} - -#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0])) - -static struct { - void (*call)(void); -} interrupt_table[7]; - -void attachInterrupt(uint8_t inum, void (*call)(void), int mode) -{ - if (inum >= ARRAY_LENGTH(interrupt_table)) { - fprintf(stderr, "Bad attachInterrupt to interrupt %u\n", inum); - exit(1); - } - interrupt_table[inum].call = call; -} - -void runInterrupt(uint8_t inum) -{ - if (inum >= ARRAY_LENGTH(interrupt_table)) { - fprintf(stderr, "Bad runInterrupt to interrupt %u\n", inum); - exit(1); - } - if (interrupt_table[inum].call != NULL) { - interrupt_table[inum].call(); - } -} - -// this version of abs() is here to ensure it is 16 bit -// which allows us to find abs() bugs in SITL -int abs(int v) -{ - int16_t v16 = (int16_t)v; - if (v16 >= 0) return v16; - return -v16; -} diff --git a/libraries/Desktop/support/DataFlash_APM1.cpp b/libraries/Desktop/support/DataFlash_APM1.cpp deleted file mode 100644 index 3971a7856a..0000000000 --- a/libraries/Desktop/support/DataFlash_APM1.cpp +++ /dev/null @@ -1,125 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - hacked up DataFlash library for Desktop support -*/ - -#include -#include -#include -#include -#include -#include "DataFlash.h" -#include -#include - -#define DF_PAGE_SIZE 512 -#define DF_NUM_PAGES 4096 - -static int flash_fd; -static uint8_t buffer[2][DF_PAGE_SIZE]; - -// Public Methods ////////////////////////////////////////////////////////////// -void DataFlash_APM1::Init(void) -{ - if (flash_fd == 0) { - flash_fd = open("dataflash.bin", O_RDWR, 0777); - if (flash_fd == -1) { - uint8_t *fill; - fill = (uint8_t *)malloc(DF_PAGE_SIZE*DF_NUM_PAGES); - flash_fd = open("dataflash.bin", O_RDWR | O_CREAT, 0777); - memset(fill, 0xFF, DF_PAGE_SIZE*DF_NUM_PAGES); - write(flash_fd, fill, DF_PAGE_SIZE*DF_NUM_PAGES); - free(fill); - } - } - df_PageSize = DF_PAGE_SIZE; - - // reserve last page for config information - df_NumPages = DF_NUM_PAGES - 1; -} - -// This function is mainly to test the device -void DataFlash_APM1::ReadManufacturerID() -{ - df_manufacturer = 1; - df_device = 0x0203; -} - -bool DataFlash_APM1::CardInserted(void) -{ - return true; -} - -// Read the status register -byte DataFlash_APM1::ReadStatusReg() -{ - return 0; -} - -// Read the status of the DataFlash -inline -byte DataFlash_APM1::ReadStatus() -{ - return 1; -} - - -inline -uint16_t DataFlash_APM1::PageSize() -{ - return df_PageSize; -} - - -// Wait until DataFlash is in ready state... -void DataFlash_APM1::WaitReady() -{ - while(!ReadStatus()); -} - -void DataFlash_APM1::PageToBuffer(unsigned char BufferNum, uint16_t PageAdr) -{ - pread(flash_fd, buffer[BufferNum-1], DF_PAGE_SIZE, PageAdr*DF_PAGE_SIZE); -} - -void DataFlash_APM1::BufferToPage (unsigned char BufferNum, uint16_t PageAdr, unsigned char wait) -{ - pwrite(flash_fd, buffer[BufferNum-1], DF_PAGE_SIZE, PageAdr*DF_PAGE_SIZE); -} - -void DataFlash_APM1::BufferWrite (unsigned char BufferNum, uint16_t IntPageAdr, unsigned char Data) -{ - buffer[BufferNum-1][IntPageAdr] = (uint8_t)Data; -} - -unsigned char DataFlash_APM1::BufferRead (unsigned char BufferNum, uint16_t IntPageAdr) -{ - return (unsigned char)buffer[BufferNum-1][IntPageAdr]; -} - -// *** END OF INTERNAL FUNCTIONS *** - -void DataFlash_APM1::PageErase (uint16_t PageAdr) -{ - uint8_t fill[DF_PAGE_SIZE]; - memset(fill, 0xFF, sizeof(fill)); - pwrite(flash_fd, fill, DF_PAGE_SIZE, PageAdr*DF_PAGE_SIZE); -} - -void DataFlash_APM1::BlockErase (uint16_t BlockAdr) -{ - uint8_t fill[DF_PAGE_SIZE*8]; - memset(fill, 0xFF, sizeof(fill)); - pwrite(flash_fd, fill, DF_PAGE_SIZE*8, BlockAdr*DF_PAGE_SIZE*8); -} - - -void DataFlash_APM1::ChipErase(void (*delay_cb)(unsigned long)) -{ - for (int i=0; i -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "desktop.h" -#include "util.h" - -#define LISTEN_BASE_PORT 5760 -#define BUFFER_SIZE 128 - - -#if defined(UDR3) -# define FS_MAX_PORTS 4 -#elif defined(UDR2) -# define FS_MAX_PORTS 3 -#elif defined(UDR1) -# define FS_MAX_PORTS 2 -#else -# define FS_MAX_PORTS 1 -#endif - -#ifndef MSG_NOSIGNAL -# define MSG_NOSIGNAL 0 -#endif - -static struct tcp_state { - bool connected; // true if a client has connected - int listen_fd; // socket we are listening on - int fd; // data socket - int serial_port; - bool console; -} tcp_state[FS_MAX_PORTS]; - - - -/* - start a TCP connection for a given serial port. If - wait_for_connection is true then block until a client connects - */ -static void tcp_start_connection(unsigned int serial_port, bool wait_for_connection) -{ - struct tcp_state *s = &tcp_state[serial_port]; - int one=1; - struct sockaddr_in sockaddr; - int ret; - - if (desktop_state.console_mode) { - // hack for console access - s->connected = true; - s->listen_fd = -1; - s->fd = 1; - s->serial_port = serial_port; - s->console = true; - set_nonblocking(0); - return; - } - - s->serial_port = serial_port; - - memset(&sockaddr,0,sizeof(sockaddr)); - -#ifdef HAVE_SOCK_SIN_LEN - sockaddr.sin_len = sizeof(sockaddr); -#endif - sockaddr.sin_port = htons(LISTEN_BASE_PORT+serial_port); - sockaddr.sin_family = AF_INET; - - s->listen_fd = socket(AF_INET, SOCK_STREAM, 0); - if (s->listen_fd == -1) { - fprintf(stderr, "socket failed - %s\n", strerror(errno)); - exit(1); - } - - /* we want to be able to re-use ports quickly */ - setsockopt(s->listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); - - ret = bind(s->listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); - if (ret == -1) { - fprintf(stderr, "bind failed on port %u - %s\n", - (unsigned)ntohs(sockaddr.sin_port), - strerror(errno)); - exit(1); - } - - ret = listen(s->listen_fd, 5); - if (ret == -1) { - fprintf(stderr, "listen failed - %s\n", strerror(errno)); - exit(1); - } - - printf("Serial port %u on TCP port %u\n", serial_port, LISTEN_BASE_PORT+serial_port); - fflush(stdout); - - if (wait_for_connection) { - printf("Waiting for connection ....\n"); - s->fd = accept(s->listen_fd, NULL, NULL); - if (s->fd == -1) { - fprintf(stderr, "accept() error - %s", strerror(errno)); - exit(1); - } - setsockopt(s->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); - setsockopt(s->fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); - s->connected = true; - if (!desktop_state.slider) { - set_nonblocking(s->fd); - } - } -} - - -/* - use select() to see if something is pending - */ -static bool select_check(int fd) -{ - fd_set fds; - struct timeval tv; - - FD_ZERO(&fds); - FD_SET(fd, &fds); - - // zero time means immediate return from select() - tv.tv_sec = 0; - tv.tv_usec = 0; - - if (select(fd+1, &fds, NULL, NULL, &tv) == 1) { - return true; - } - return false; -} - - -/* - see if a new connection is coming in - */ -static void check_connection(struct tcp_state *s) -{ - if (s->connected) { - // we only want 1 connection at a time - return; - } - if (select_check(s->listen_fd)) { - s->fd = accept(s->listen_fd, NULL, NULL); - if (s->fd != -1) { - int one = 1; - s->connected = true; - setsockopt(s->fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); - setsockopt(s->fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); - printf("New connection on serial port %u\n", s->serial_port); - if (!desktop_state.slider) { - set_nonblocking(s->fd); - } - } - } -} - - -FastSerial::Buffer __FastSerial__rxBuffer[FS_MAX_PORTS]; -FastSerial::Buffer __FastSerial__txBuffer[FS_MAX_PORTS]; - -// Constructor ///////////////////////////////////////////////////////////////// - -FastSerial::FastSerial(const uint8_t portNumber, volatile uint8_t *ubrrh, volatile uint8_t *ubrrl, - volatile uint8_t *ucsra, volatile uint8_t *ucsrb, const uint8_t u2x, - const uint8_t portEnableBits, const uint8_t portTxBits) : - _ubrrh(ubrrh), - _ubrrl(ubrrl), - _ucsra(ucsra), - _ucsrb(ucsrb), - _u2x(portNumber), - _portEnableBits(portEnableBits), - _portTxBits(portTxBits), - _rxBuffer(&__FastSerial__rxBuffer[portNumber]), - _txBuffer(&__FastSerial__txBuffer[portNumber]) -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// - -void FastSerial::begin(long baud) -{ - switch (_u2x) { - case 0: - tcp_start_connection(_u2x, true); - break; - - case 1: - /* gps */ - tcp_state[1].connected = true; - tcp_state[1].fd = sitl_gps_pipe(); - tcp_state[1].serial_port = 1; - break; - - default: - tcp_start_connection(_u2x, false); - break; - } -} - -void FastSerial::begin(long baud, unsigned int rxSpace, unsigned int txSpace) -{ - begin(baud); -} - -void FastSerial::end() -{ -} - -int FastSerial::available(void) -{ - struct tcp_state *s = &tcp_state[_u2x]; - - check_connection(s); - - if (!s->connected) { - return 0; - } - - if (select_check(s->fd)) { -#ifdef FIONREAD - // use FIONREAD to get exact value if possible - int num_ready; - if (ioctl(s->fd, FIONREAD, &num_ready) == 0) { - if (num_ready > BUFFER_SIZE) { - return BUFFER_SIZE; - } - if (num_ready == 0) { - // EOF is reached - fprintf(stdout, "Closed connection on serial port %u\n", s->serial_port); - close(s->fd); - s->connected = false; - return 0; - } - return num_ready; - } -#endif - return 1; // best we can do is say 1 byte available - } - return 0; -} - -int FastSerial::txspace(void) -{ - // always claim there is space available - return BUFFER_SIZE; -} - -int FastSerial::read(void) -{ - struct tcp_state *s = &tcp_state[_u2x]; - char c; - - if (available() <= 0) { - return -1; - } - - if (s->serial_port == 1) { - if (sitl_gps_read(s->fd, &c, 1) == 1) { - return (uint8_t)c; - } - return -1; - } - - if (s->console) { - return ::read(0, &c, 1); - } - - int n = recv(s->fd, &c, 1, MSG_DONTWAIT | MSG_NOSIGNAL); - if (n <= 0) { - // the socket has reached EOF - close(s->fd); - s->connected = false; - fprintf(stdout, "Closed connection on serial port %u\n", s->serial_port); - fflush(stdout); - return -1; - } - if (n == 1) { - return (uint8_t)c; - } - return -1; -} - -int FastSerial::peek(void) -{ - return -1; -} - -void FastSerial::flush(void) -{ -} - -void FastSerial::write(uint8_t c) -{ - struct tcp_state *s = &tcp_state[_u2x]; - int flags = MSG_NOSIGNAL; - check_connection(s); - if (!s->connected) { - return; - } - if (!desktop_state.slider) { - flags |= MSG_DONTWAIT; - } - if (s->console) { - ::write(s->fd, &c, 1); - } else { - send(s->fd, &c, 1, flags); - } -} - -// Buffer management /////////////////////////////////////////////////////////// - -bool FastSerial::_allocBuffer(Buffer *buffer, unsigned int size) -{ - return false; -} - -void FastSerial::_freeBuffer(Buffer *buffer) -{ -} - -/* - return true if any bytes are pending - */ -void desktop_serial_select_setup(fd_set *fds, int *fd_high) -{ - int i; - - for (i=0; i *fd_high) { - *fd_high = tcp_state[i].fd; - } - } - } -} diff --git a/libraries/Desktop/support/Print.cpp b/libraries/Desktop/support/Print.cpp deleted file mode 100644 index fd689423a4..0000000000 --- a/libraries/Desktop/support/Print.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/* - Print.cpp - Base class that provides print() and println() - Copyright (c) 2008 David A. Mellis. All right reserved. - - 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - - Modified 23 November 2006 by David A. Mellis - */ - -#include -#include -#include -#include -#include "wiring.h" - -#include "Print.h" - -// Public Methods ////////////////////////////////////////////////////////////// - -/* default implementation: may be overridden */ -void Print::write(const char *str) -{ - while (*str) - write(*str++); -} - -/* default implementation: may be overridden */ -void Print::write(const uint8_t *buffer, size_t size) -{ - while (size--) - write(*buffer++); -} - -void Print::print(const String &s) -{ - for (unsigned int i = 0; i < s.length(); i++) { - write(s[i]); - } -} - -void Print::print(const char str[]) -{ - write(str); -} - -void Print::print(char c, int base) -{ - print((long) c, base); -} - -void Print::print(unsigned char b, int base) -{ - print((unsigned long) b, base); -} - -void Print::print(int n, int base) -{ - print((long) n, base); -} - -void Print::print(unsigned int n, int base) -{ - print((unsigned long) n, base); -} - -void Print::print(long n, int base) -{ - if (base == 0) { - write(n); - } else if (base == 10) { - if (n < 0) { - print('-'); - n = -n; - } - printNumber(n, 10); - } else { - printNumber(n, base); - } -} - -void Print::print(unsigned long n, int base) -{ - if (base == 0) write(n); - else printNumber(n, base); -} - -void Print::print(double n, int digits) -{ - printFloat(n, digits); -} - -void Print::println(void) -{ - print('\r'); - print('\n'); -} - -void Print::println(const String &s) -{ - print(s); - println(); -} - -void Print::println(const char c[]) -{ - print(c); - println(); -} - -void Print::println(char c, int base) -{ - print(c, base); - println(); -} - -void Print::println(unsigned char b, int base) -{ - print(b, base); - println(); -} - -void Print::println(int n, int base) -{ - print(n, base); - println(); -} - -void Print::println(unsigned int n, int base) -{ - print(n, base); - println(); -} - -void Print::println(long n, int base) -{ - print(n, base); - println(); -} - -void Print::println(unsigned long n, int base) -{ - print(n, base); - println(); -} - -void Print::println(double n, int digits) -{ - print(n, digits); - println(); -} - -// Private Methods ///////////////////////////////////////////////////////////// - -void Print::printNumber(unsigned long n, uint8_t base) -{ - unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. - unsigned long i = 0; - - if (n == 0) { - print('0'); - return; - } - - while (n > 0) { - buf[i++] = n % base; - n /= base; - } - - for (; i > 0; i--) - print((char) (buf[i - 1] < 10 ? - '0' + buf[i - 1] : - 'A' + buf[i - 1] - 10)); -} - -void Print::printFloat(double number, uint8_t digits) -{ - // Handle negative numbers - if (number < 0.0) - { - print('-'); - number = -number; - } - - // Round correctly so that print(1.999, 2) prints as "2.00" - double rounding = 0.5; - for (uint8_t i=0; i 0) - print("."); - - // Extract digits from the remainder one at a time - while (digits-- > 0) - { - remainder *= 10.0; - int toPrint = int(remainder); - print(toPrint); - remainder -= toPrint; - } -} diff --git a/libraries/Desktop/support/SPI.cpp b/libraries/Desktop/support/SPI.cpp deleted file mode 100644 index 42915df1eb..0000000000 --- a/libraries/Desktop/support/SPI.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright (c) 2010 by Cristian Maglie - * SPI Master library for arduino. - * - * This file is free software; you can redistribute it and/or modify - * it under the terms of either the GNU General Public License version 2 - * or the GNU Lesser General Public License version 2.1, both as - * published by the Free Software Foundation. - */ - -#include "pins_arduino.h" -#include "SPI.h" - -SPIClass SPI; - -void SPIClass::begin() { - // Set direction register for SCK and MOSI pin. - // MISO pin automatically overrides to INPUT. - // When the SS pin is set as OUTPUT, it can be used as - // a general purpose output port (it doesn't influence - // SPI operations). - - pinMode(SCK, OUTPUT); - pinMode(MOSI, OUTPUT); - pinMode(SS, OUTPUT); - - digitalWrite(SCK, LOW); - digitalWrite(MOSI, LOW); - digitalWrite(SS, HIGH); - - // Warning: if the SS pin ever becomes a LOW INPUT then SPI - // automatically switches to Slave, so the data direction of - // the SS pin MUST be kept as OUTPUT. - SPCR |= _BV(MSTR); - SPCR |= _BV(SPE); -} - -void SPIClass::end() { - SPCR &= ~_BV(SPE); -} - -void SPIClass::setBitOrder(uint8_t bitOrder) -{ - if(bitOrder == LSBFIRST) { - SPCR |= _BV(DORD); - } else { - SPCR &= ~(_BV(DORD)); - } -} - -void SPIClass::setDataMode(uint8_t mode) -{ - SPCR = (SPCR & ~SPI_MODE_MASK) | mode; -} - -void SPIClass::setClockDivider(uint8_t rate) -{ - SPCR = (SPCR & ~SPI_CLOCK_MASK) | (rate & SPI_CLOCK_MASK); - SPSR = (SPSR & ~SPI_2XCLOCK_MASK) | ((rate >> 2) & SPI_2XCLOCK_MASK); -} - diff --git a/libraries/Desktop/support/WMath.cpp b/libraries/Desktop/support/WMath.cpp deleted file mode 100644 index 2120c4cc10..0000000000 --- a/libraries/Desktop/support/WMath.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* -*- mode: jde; c-basic-offset: 2; indent-tabs-mode: nil -*- */ - -/* - Part of the Wiring project - http://wiring.org.co - Copyright (c) 2004-06 Hernando Barragan - Modified 13 August 2006, David A. Mellis for Arduino - http://www.arduino.cc/ - - 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 - - $Id$ -*/ - -extern "C" { - #include "stdlib.h" -} - -void randomSeed(unsigned int seed) -{ - if (seed != 0) { - srandom(seed); - } -} - -long random(long howbig) -{ - if (howbig == 0) { - return 0; - } - return random() % howbig; -} - -long random(long howsmall, long howbig) -{ - if (howsmall >= howbig) { - return howsmall; - } - long diff = howbig - howsmall; - return random(diff) + howsmall; -} - -long map(long x, long in_min, long in_max, long out_min, long out_max) -{ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} - -unsigned int makeWord(unsigned int w) { return w; } -unsigned int makeWord(unsigned char h, unsigned char l) { return (h << 8) | l; } \ No newline at end of file diff --git a/libraries/Desktop/support/WString.cpp b/libraries/Desktop/support/WString.cpp deleted file mode 100644 index fa11a7da07..0000000000 --- a/libraries/Desktop/support/WString.cpp +++ /dev/null @@ -1,443 +0,0 @@ -/* - WString.cpp - String library for Wiring & Arduino - Copyright (c) 2009-10 Hernando Barragan. All rights reserved. - - 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -*/ - -#include -#include "WProgram.h" -#include "WString.h" - - -String::String( const char *value ) -{ - if ( value == NULL ) - value = ""; - getBuffer( _length = strlen( value ) ); - if ( _buffer != NULL ) - strcpy( _buffer, value ); -} - -String::String( const String &value ) -{ - getBuffer( _length = value._length ); - if ( _buffer != NULL ) - strcpy( _buffer, value._buffer ); -} - -String::String( const char value ) -{ - _length = 1; - getBuffer(1); - if ( _buffer != NULL ) { - _buffer[0] = value; - _buffer[1] = 0; - } -} - -String::String( const unsigned char value ) -{ - _length = 1; - getBuffer(1); - if ( _buffer != NULL) { - _buffer[0] = value; - _buffer[1] = 0; - } -} - -String::String( const int value, const int base ) -{ - char buf[33]; - itoa((signed long)value, buf, base); - getBuffer( _length = strlen(buf) ); - if ( _buffer != NULL ) - strcpy( _buffer, buf ); -} - -String::String( const unsigned int value, const int base ) -{ - char buf[33]; - ultoa((unsigned long)value, buf, base); - getBuffer( _length = strlen(buf) ); - if ( _buffer != NULL ) - strcpy( _buffer, buf ); -} - -String::String( const long value, const int base ) -{ - char buf[33]; - ltoa(value, buf, base); - getBuffer( _length = strlen(buf) ); - if ( _buffer != NULL ) - strcpy( _buffer, buf ); -} - -String::String( const unsigned long value, const int base ) -{ - char buf[33]; - ultoa(value, buf, 10); - getBuffer( _length = strlen(buf) ); - if ( _buffer != NULL ) - strcpy( _buffer, buf ); -} - -char String::charAt( unsigned int loc ) const -{ - return operator[]( loc ); -} - -void String::setCharAt( unsigned int loc, const char aChar ) -{ - if(_buffer == NULL) return; - if(_length > loc) { - _buffer[loc] = aChar; - } -} - -int String::compareTo( const String &s2 ) const -{ - return strcmp( _buffer, s2._buffer ); -} - -const String & String::concat( const String &s2 ) -{ - return (*this) += s2; -} - -const String & String::operator=( const String &rhs ) -{ - if ( this == &rhs ) - return *this; - - if ( rhs._length > _length ) - { - free(_buffer); - getBuffer( rhs._length ); - } - - if ( _buffer != NULL ) { - _length = rhs._length; - strcpy( _buffer, rhs._buffer ); - } - return *this; -} - -//const String & String::operator+=( const char aChar ) -//{ -// if ( _length == _capacity ) -// doubleBuffer(); -// -// _buffer[ _length++ ] = aChar; -// _buffer[ _length ] = '\0'; -// return *this; -//} - -const String & String::operator+=( const String &other ) -{ - _length += other._length; - if ( _length > _capacity ) - { - char *temp = (char *)realloc(_buffer, _length + 1); - if ( temp != NULL ) { - _buffer = temp; - _capacity = _length; - } else { - _length -= other._length; - return *this; - } - } - strcat( _buffer, other._buffer ); - return *this; -} - - -int String::operator==( const String &rhs ) const -{ - return ( _length == rhs._length && strcmp( _buffer, rhs._buffer ) == 0 ); -} - -int String::operator!=( const String &rhs ) const -{ - return ( _length != rhs.length() || strcmp( _buffer, rhs._buffer ) != 0 ); -} - -int String::operator<( const String &rhs ) const -{ - return strcmp( _buffer, rhs._buffer ) < 0; -} - -int String::operator>( const String &rhs ) const -{ - return strcmp( _buffer, rhs._buffer ) > 0; -} - -int String::operator<=( const String &rhs ) const -{ - return strcmp( _buffer, rhs._buffer ) <= 0; -} - -int String::operator>=( const String & rhs ) const -{ - return strcmp( _buffer, rhs._buffer ) >= 0; -} - -char & String::operator[]( unsigned int index ) -{ - static char dummy_writable_char; - if (index >= _length || !_buffer) { - dummy_writable_char = 0; - return dummy_writable_char; - } - return _buffer[ index ]; -} - -char String::operator[]( unsigned int index ) const -{ - // need to check for valid index, to do later - return _buffer[ index ]; -} - -boolean String::endsWith( const String &s2 ) const -{ - if ( _length < s2._length ) - return 0; - - return strcmp( &_buffer[ _length - s2._length], s2._buffer ) == 0; -} - -boolean String::equals( const String &s2 ) const -{ - return ( _length == s2._length && strcmp( _buffer,s2._buffer ) == 0 ); -} - -boolean String::equalsIgnoreCase( const String &s2 ) const -{ - if ( this == &s2 ) - return true; //1; - else if ( _length != s2._length ) - return false; //0; - - return strcmp(toLowerCase()._buffer, s2.toLowerCase()._buffer) == 0; -} - -String String::replace( char findChar, char replaceChar ) -{ - if ( _buffer == NULL ) return *this; - String theReturn = _buffer; - char* temp = theReturn._buffer; - while( (temp = strchr( temp, findChar )) != 0 ) - *temp = replaceChar; - - return theReturn; -} - -String String::replace( const String& match, const String& rep ) -{ - if ( _buffer == NULL ) return *this; - String temp = _buffer, newString; - - int loc; - while ( (loc = temp.indexOf( match )) != -1 ) - { - newString += temp.substring( 0, loc ); - newString += rep; - temp = temp.substring( loc + match._length ); - } - newString += temp; - return newString; -} - -int String::indexOf( char temp ) const -{ - return indexOf( temp, 0 ); -} - -int String::indexOf( char ch, unsigned int fromIndex ) const -{ - if ( fromIndex >= _length ) - return -1; - - const char* temp = strchr( &_buffer[fromIndex], ch ); - if ( temp == NULL ) - return -1; - - return temp - _buffer; -} - -int String::indexOf( const String &s2 ) const -{ - return indexOf( s2, 0 ); -} - -int String::indexOf( const String &s2, unsigned int fromIndex ) const -{ - if ( fromIndex >= _length ) - return -1; - - const char *theFind = strstr( &_buffer[ fromIndex ], s2._buffer ); - - if ( theFind == NULL ) - return -1; - - return theFind - _buffer; // pointer subtraction -} - -int String::lastIndexOf( char theChar ) const -{ - return lastIndexOf( theChar, _length - 1 ); -} - -int String::lastIndexOf( char ch, unsigned int fromIndex ) const -{ - if ( fromIndex >= _length ) - return -1; - - char tempchar = _buffer[fromIndex + 1]; - _buffer[fromIndex + 1] = '\0'; - char* temp = strrchr( _buffer, ch ); - _buffer[fromIndex + 1] = tempchar; - - if ( temp == NULL ) - return -1; - - return temp - _buffer; -} - -int String::lastIndexOf( const String &s2 ) const -{ - return lastIndexOf( s2, _length - s2._length ); -} - -int String::lastIndexOf( const String &s2, unsigned int fromIndex ) const -{ - // check for empty strings - if ( s2._length == 0 || s2._length - 1 > fromIndex || fromIndex >= _length ) - return -1; - - // matching first character - char temp = s2[ 0 ]; - - for ( int i = fromIndex; i >= 0; i-- ) - { - if ( _buffer[ i ] == temp && (*this).substring( i, i + s2._length ).equals( s2 ) ) - return i; - } - return -1; -} - -boolean String::startsWith( const String &s2 ) const -{ - if ( _length < s2._length ) - return 0; - - return startsWith( s2, 0 ); -} - -boolean String::startsWith( const String &s2, unsigned int offset ) const -{ - if ( offset > _length - s2._length ) - return 0; - - return strncmp( &_buffer[offset], s2._buffer, s2._length ) == 0; -} - -String String::substring( unsigned int left ) const -{ - return substring( left, _length ); -} - -String String::substring( unsigned int left, unsigned int right ) const -{ - if ( left > right ) - { - int temp = right; - right = left; - left = temp; - } - - if ( right > _length ) - { - right = _length; - } - - char temp = _buffer[ right ]; // save the replaced character - _buffer[ right ] = '\0'; - String outPut = ( _buffer + left ); // pointer arithmetic - _buffer[ right ] = temp; //restore character - return outPut; -} - -String String::toLowerCase() const -{ - String temp = _buffer; - - for ( unsigned int i = 0; i < _length; i++ ) - temp._buffer[ i ] = (char)tolower( temp._buffer[ i ] ); - return temp; -} - -String String::toUpperCase() const -{ - String temp = _buffer; - - for ( unsigned int i = 0; i < _length; i++ ) - temp._buffer[ i ] = (char)toupper( temp._buffer[ i ] ); - return temp; -} - -String String::trim() const -{ - if ( _buffer == NULL ) return *this; - String temp = _buffer; - unsigned int i,j; - - for ( i = 0; i < _length; i++ ) - { - if ( !isspace(_buffer[i]) ) - break; - } - - for ( j = temp._length - 1; j > i; j-- ) - { - if ( !isspace(_buffer[j]) ) - break; - } - - return temp.substring( i, j + 1); -} - -void String::getBytes(unsigned char *buf, unsigned int bufsize) -{ - if (!bufsize || !buf) return; - unsigned int len = bufsize - 1; - if (len > _length) len = _length; - strncpy((char *)buf, _buffer, len); - buf[len] = 0; -} - -void String::toCharArray(char *buf, unsigned int bufsize) -{ - if (!bufsize || !buf) return; - unsigned int len = bufsize - 1; - if (len > _length) len = _length; - strncpy(buf, _buffer, len); - buf[len] = 0; -} - - -long String::toInt() { - return atol(_buffer); -} diff --git a/libraries/Desktop/support/desktop.h b/libraries/Desktop/support/desktop.h deleted file mode 100644 index b6ff35e4f7..0000000000 --- a/libraries/Desktop/support/desktop.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef _DESKTOP_H -#define _DESKTOP_H - -#include -#include - -enum vehicle_type { - ArduCopter, - APMrover2, - ArduPlane -}; - -struct desktop_info { - bool slider; // slider switch state, True means CLI mode - struct timeval sketch_start_time; - enum vehicle_type vehicle; - unsigned framerate; - float initial_height; - bool console_mode; -}; - -extern struct desktop_info desktop_state; - -void desktop_serial_select_setup(fd_set *fds, int *fd_high); -void sitl_input(void); -void sitl_setup(void); -int sitl_gps_pipe(void); -ssize_t sitl_gps_read(int fd, void *buf, size_t count); -void sitl_update_compass(float roll, float pitch, float yaw); -void sitl_update_gps(double latitude, double longitude, float altitude, - double speedN, double speedE, bool have_lock); -void sitl_update_adc(float roll, float pitch, float yaw, - double rollRate, double pitchRate, double yawRate, - double xAccel, double yAccel, double zAccel, - float airspeed); -void sitl_setup_adc(void); -void sitl_update_barometer(float altitude); - -void sitl_simstate_send(uint8_t chan); - -#endif diff --git a/libraries/Desktop/support/digital.c b/libraries/Desktop/support/digital.c deleted file mode 100644 index 691c887999..0000000000 --- a/libraries/Desktop/support/digital.c +++ /dev/null @@ -1,22 +0,0 @@ -// support for digitalRead() and digitalWrite() -#include -#include -#include -#include -#include -#include "desktop.h" - - -int digitalRead(uint8_t address) -{ - switch (address) { - case 40: - // CLI slider switch - return desktop_state.slider?0:1; - - default: - break; - } - printf("digitalRead(0x%x) unsupported\n", address); - return 0; -} diff --git a/libraries/Desktop/support/eeprom.c b/libraries/Desktop/support/eeprom.c deleted file mode 100644 index 35f4e658aa..0000000000 --- a/libraries/Desktop/support/eeprom.c +++ /dev/null @@ -1,87 +0,0 @@ -#include -#include -#include -#include -#include -#include - -static int eeprom_fd; - -static void eeprom_open(void) -{ - if (eeprom_fd == 0) { - eeprom_fd = open("eeprom.bin", O_RDWR|O_CREAT, 0777); - ftruncate(eeprom_fd, 4096); - } -} - -void eeprom_write_byte(uint8_t *p, uint8_t value) -{ - intptr_t ofs = (intptr_t)p; - assert(ofs < 4096); - eeprom_open(); - pwrite(eeprom_fd, &value, 1, ofs); -} - - -void eeprom_write_word(uint16_t *p, uint16_t value) -{ - intptr_t ofs = (intptr_t)p; - assert(ofs < 4096); - eeprom_open(); - pwrite(eeprom_fd, &value, 2, ofs); -} - -void eeprom_write_dword(uint32_t *p, uint32_t value) -{ - intptr_t ofs = (intptr_t)p; - assert(ofs < 4096); - eeprom_open(); - pwrite(eeprom_fd, &value, 4, ofs); -} - -uint8_t eeprom_read_byte(const uint8_t *p) -{ - intptr_t ofs = (intptr_t)p; - uint8_t value; - assert(ofs < 4096); - eeprom_open(); - pread(eeprom_fd, &value, 1, ofs); - return value; -} - -uint16_t eeprom_read_word(const uint16_t *p) -{ - intptr_t ofs = (intptr_t)p; - uint16_t value; - assert(ofs < 4096); - eeprom_open(); - pread(eeprom_fd, &value, 2, ofs); - return value; -} - -uint32_t eeprom_read_dword(const uint32_t *p) -{ - intptr_t ofs = (intptr_t)p; - uint32_t value; - assert(ofs < 4096); - eeprom_open(); - pread(eeprom_fd, &value, 4, ofs); - return value; -} - -void eeprom_read_block(void *buf, void *ptr, uint8_t size) -{ - intptr_t ofs = (intptr_t)ptr; - assert(ofs < 4096); - eeprom_open(); - pread(eeprom_fd, buf, size, ofs); -} - -void eeprom_write_block(const void *buf, void *ptr, uint8_t size) -{ - intptr_t ofs = (intptr_t)ptr; - assert(ofs < 4096); - eeprom_open(); - pwrite(eeprom_fd, buf, size, ofs); -} diff --git a/libraries/Desktop/support/main.cpp b/libraries/Desktop/support/main.cpp deleted file mode 100644 index 7869fe7aff..0000000000 --- a/libraries/Desktop/support/main.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "desktop.h" - -void setup(void); -void loop(void); - -// the state of the desktop simulation -struct desktop_info desktop_state; - -// catch floating point exceptions -static void sig_fpe(int signum) -{ - printf("ERROR: Floating point exception\n"); - exit(1); -} - -static void usage(void) -{ - printf("Options:\n"); - printf("\t-s enable CLI slider switch\n"); - printf("\t-w wipe eeprom and dataflash\n"); - printf("\t-r RATE set SITL framerate\n"); - printf("\t-H HEIGHT initial barometric height\n"); - printf("\t-C use console instead of TCP ports\n"); -} - -int main(int argc, char * const argv[]) -{ - int opt; - // default state - desktop_state.slider = false; - gettimeofday(&desktop_state.sketch_start_time, NULL); - - signal(SIGFPE, sig_fpe); - - while ((opt = getopt(argc, argv, "swhr:H:C")) != -1) { - switch (opt) { - case 's': - desktop_state.slider = true; - break; - case 'w': - AP_Param::erase_all(); - unlink("dataflash.bin"); - break; - case 'r': - desktop_state.framerate = (unsigned)atoi(optarg); - break; - case 'H': - desktop_state.initial_height = atof(optarg); - break; - case 'C': - desktop_state.console_mode = true; - break; - default: - usage(); - exit(1); - } - } - - printf("Starting sketch '%s'\n", SKETCH); - - if (strcmp(SKETCH, "ArduCopter") == 0) { - desktop_state.vehicle = ArduCopter; - if (desktop_state.framerate == 0) { - desktop_state.framerate = 200; - } - } else if (strcmp(SKETCH, "APMrover2") == 0) { - desktop_state.vehicle = APMrover2; - if (desktop_state.framerate == 0) { - desktop_state.framerate = 50; - } - // set right default throttle for rover (allowing for reverse) - ICR4.set(2, 1500); - } else { - desktop_state.vehicle = ArduPlane; - if (desktop_state.framerate == 0) { - desktop_state.framerate = 50; - } - } - - sitl_setup(); - setup(); - - while (true) { - struct timeval tv; - fd_set fds; - int fd_high = 0; - -#ifdef __CYGWIN__ - // under windows if this loop is using alot of cpu, - // the alarm gets called at a slower rate. - sleep(5); -#endif - - FD_ZERO(&fds); - loop(); - - desktop_serial_select_setup(&fds, &fd_high); - tv.tv_sec = 0; - tv.tv_usec = 100; - - select(fd_high+1, &fds, NULL, NULL, &tv); - } - return 0; -} diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp deleted file mode 100644 index dd228dcf49..0000000000 --- a/libraries/Desktop/support/sitl.cpp +++ /dev/null @@ -1,367 +0,0 @@ -/* - SITL handling - - This simulates the APM1 hardware sufficiently for the APM code to - think it is running on real hardware - - Andrew Tridgell November 2011 - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "sitl_adc.h" -#include "sitl_rc.h" -#include "desktop.h" -#include "util.h" - -#define SIMIN_PORT 5501 -#define RCOUT_PORT 5502 - -static int sitl_fd; -struct sockaddr_in rcout_addr; -#ifndef __CYGWIN__ -static pid_t parent_pid; -#endif -struct ADC_UDR2 UDR2; -struct RC_ICR4 ICR4; -extern AP_TimerProcess timer_scheduler; -extern Arduino_Mega_ISR_Registry isr_registry; -extern SITL sitl; - -static uint32_t update_count; - - -/* - setup a SITL FDM listening UDP port - */ -static void setup_fdm(void) -{ - int one=1, ret; - struct sockaddr_in sockaddr; - - memset(&sockaddr,0,sizeof(sockaddr)); - -#ifdef HAVE_SOCK_SIN_LEN - sockaddr.sin_len = sizeof(sockaddr); -#endif - sockaddr.sin_port = htons(SIMIN_PORT); - sockaddr.sin_family = AF_INET; - - sitl_fd = socket(AF_INET, SOCK_DGRAM, 0); - if (sitl_fd == -1) { - fprintf(stderr, "SITL: socket failed - %s\n", strerror(errno)); - exit(1); - } - - /* we want to be able to re-use ports quickly */ - setsockopt(sitl_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); - - ret = bind(sitl_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); - if (ret == -1) { - fprintf(stderr, "SITL: bind failed on port %u - %s\n", - (unsigned)ntohs(sockaddr.sin_port), strerror(errno)); - exit(1); - } - - set_nonblocking(sitl_fd); -} - -/* - check for a SITL FDM packet - */ -static void sitl_fdm_input(void) -{ - ssize_t size; - struct pwm_packet { - uint16_t pwm[8]; - }; - union { - struct sitl_fdm fg_pkt; - struct pwm_packet pwm_pkt; - } d; - - size = recv(sitl_fd, &d, sizeof(d), MSG_DONTWAIT); - switch (size) { - case 132: - static uint32_t last_report; - static uint32_t count; - - if (d.fg_pkt.magic != 0x4c56414e) { - printf("Bad FDM packet - magic=0x%08x\n", d.fg_pkt.magic); - return; - } - - if (d.fg_pkt.latitude == 0 || - d.fg_pkt.longitude == 0 || - d.fg_pkt.altitude <= 0) { - // garbage input - return; - } - - sitl.state = d.fg_pkt; - update_count++; - - count++; - if (millis() - last_report > 1000) { - //printf("SIM %u FPS\n", count); - count = 0; - last_report = millis(); - } - break; - - case 16: { - // a packet giving the receiver PWM inputs - uint8_t i; - for (i=0; i<8; i++) { - // setup the ICR4 register for the RC channel - // inputs - if (d.pwm_pkt.pwm[i] != 0) { - ICR4.set(i, d.pwm_pkt.pwm[i]); - } - } - break; - } - } - -} - -// used for noise generation in the ADC code -bool sitl_motors_on; - -/* - send RC outputs to simulator - */ -static void sitl_simulator_output(void) -{ - static uint32_t last_update; - struct { - uint16_t pwm[11]; - uint16_t speed, direction, turbulance; - } control; - /* this maps the registers used for PWM outputs. The RC - * driver updates these whenever it wants the channel output - * to change */ - uint16_t *reg[11] = { &OCR5B, &OCR5C, &OCR1B, &OCR1C, - &OCR4C, &OCR4B, &OCR3C, &OCR3B, - &OCR5A, &OCR1A, &OCR3A }; - uint8_t i; - - if (last_update == 0) { - for (i=0; i<11; i++) { - (*reg[i]) = 1000*2; - } - if (desktop_state.vehicle == ArduPlane) { - (*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2; - (*reg[7]) = 1800*2; - } - if (desktop_state.vehicle == APMrover2) { - (*reg[0]) = (*reg[1]) = (*reg[2]) = (*reg[3]) = 1500*2; - (*reg[7]) = 1800*2; - } - } - - // output at chosen framerate - if (last_update != 0 && millis() - last_update < 1000/desktop_state.framerate) { - return; - } - last_update = millis(); - - for (i=0; i<11; i++) { - if (*reg[i] == 0xFFFF) { - control.pwm[i] = 0; - } else { - control.pwm[i] = (*reg[i])/2; - } - } - - if (desktop_state.vehicle == ArduPlane) { - // add in engine multiplier - if (control.pwm[2] > 1000) { - control.pwm[2] = ((control.pwm[2]-1000) * sitl.engine_mul) + 1000; - if (control.pwm[2] > 2000) control.pwm[2] = 2000; - } - sitl_motors_on = ((control.pwm[2]-1000)/1000.0) > 0; - } else if (desktop_state.vehicle == APMrover2) { - // add in engine multiplier - if (control.pwm[2] != 1500) { - control.pwm[2] = ((control.pwm[2]-1500) * sitl.engine_mul) + 1500; - if (control.pwm[2] > 2000) control.pwm[2] = 2000; - if (control.pwm[2] < 1000) control.pwm[2] = 1000; - } - sitl_motors_on = ((control.pwm[2]-1500)/500.0) != 0; - } else { - sitl_motors_on = false; - for (i=0; i<4; i++) { - if ((control.pwm[i]-1000)/1000.0 > 0) { - sitl_motors_on = true; - } - } - } - - // setup wind control - control.speed = sitl.wind_speed * 100; - float direction = sitl.wind_direction; - if (direction < 0) { - direction += 360; - } - control.direction = direction * 100; - control.turbulance = sitl.wind_turbulance * 100; - - // zero the wind for the first 15s to allow pitot calibration - if (millis() < 15000) { - control.speed = 0; - } - - sendto(sitl_fd, (void*)&control, sizeof(control), MSG_DONTWAIT, (const sockaddr *)&rcout_addr, sizeof(rcout_addr)); -} - -/* - timer called at 1kHz - */ -static void timer_handler(int signum) -{ - static uint32_t last_update_count; - static bool in_timer; - - if (in_timer || _interrupts_are_blocked()) { - return; - } - uint8_t oldSREG = SREG; - cli(); - - in_timer = true; - -#ifndef __CYGWIN__ - /* make sure we die if our parent dies */ - if (kill(parent_pid, 0) != 0) { - exit(1); - } -#else - - static uint16_t count = 0; - static uint32_t last_report; - - count++; - if (millis() - last_report > 1000) { - printf("TH %u cps\n", count); - count = 0; - last_report = millis(); - } -#endif - - /* check for packet from flight sim */ - sitl_fdm_input(); - - // trigger RC input - if (isr_registry._registry[ISR_REGISTRY_TIMER4_CAPT]) { - isr_registry._registry[ISR_REGISTRY_TIMER4_CAPT](); - } - - // send RC output to flight sim - sitl_simulator_output(); - - if (update_count == 0) { - sitl_update_gps(0, 0, 0, 0, 0, false); - timer_scheduler.run(); - SREG = oldSREG; - in_timer = false; - return; - } - - if (update_count == last_update_count) { - timer_scheduler.run(); - SREG = oldSREG; - in_timer = false; - return; - } - last_update_count = update_count; - - sitl_update_gps(sitl.state.latitude, sitl.state.longitude, - sitl.state.altitude, - sitl.state.speedN, sitl.state.speedE, !sitl.gps_disable); - sitl_update_adc(sitl.state.rollDeg, sitl.state.pitchDeg, sitl.state.yawDeg, - sitl.state.rollRate, sitl.state.pitchRate, sitl.state.yawRate, - sitl.state.xAccel, sitl.state.yAccel, sitl.state.zAccel, - sitl.state.airspeed); - sitl_update_barometer(sitl.state.altitude); - sitl_update_compass(sitl.state.rollDeg, sitl.state.pitchDeg, sitl.state.heading); - - // clear the ADC conversion flag, - // so the ADC code doesn't get stuck - ADCSRA &= ~_BV(ADSC); - - // trigger all APM timers. We do this last as it can re-enable - // interrupts, which can lead to recursion - timer_scheduler.run(); - - SREG = oldSREG; - in_timer = false; -} - - -/* - setup a timer used to prod the ISRs - */ -static void setup_timer(void) -{ - struct itimerval it; - struct sigaction act; - - act.sa_handler = timer_handler; - act.sa_flags = SA_RESTART|SA_NODEFER; - sigemptyset(&act.sa_mask); - sigaddset(&act.sa_mask, SIGALRM); - sigaction(SIGALRM, &act, NULL); - - it.it_interval.tv_sec = 0; - it.it_interval.tv_usec = 1000; // 1KHz - it.it_value = it.it_interval; - - setitimer(ITIMER_REAL, &it, NULL); -} - - -/* - setup for SITL handling - */ -void sitl_setup(void) -{ -#ifndef __CYGWIN__ - parent_pid = getppid(); -#endif - - rcout_addr.sin_family = AF_INET; - rcout_addr.sin_port = htons(RCOUT_PORT); - inet_pton(AF_INET, "127.0.0.1", &rcout_addr.sin_addr); - - setup_timer(); - setup_fdm(); - sitl_setup_adc(); - printf("Starting SITL input\n"); - - // setup some initial values - sitl_update_barometer(desktop_state.initial_height); - sitl_update_adc(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0); - sitl_update_compass(0, 0, 0); - sitl_update_gps(0, 0, 0, 0, 0, false); -} - - diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp deleted file mode 100644 index fcd7e82f3a..0000000000 --- a/libraries/Desktop/support/sitl_adc.cpp +++ /dev/null @@ -1,151 +0,0 @@ -/* - SITL handling - - This emulates the ADS7844 ADC - - Andrew Tridgell November 2011 - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include "wiring.h" -#include "sitl_adc.h" -#include "desktop.h" -#include "util.h" - -extern SITL sitl; - -/* - convert airspeed in m/s to an airspeed sensor value - */ -static uint16_t airspeed_sensor(float airspeed) -{ - const float airspeed_ratio = 1.9936; - const float airspeed_offset = 2820; - float airspeed_pressure, airspeed_raw; - - airspeed_pressure = sqr(airspeed) / airspeed_ratio; - airspeed_raw = airspeed_pressure + airspeed_offset; - return airspeed_raw; -} - - -static float gyro_drift(void) -{ - if (sitl.drift_speed == 0.0) { - return 0; - } - double period = sitl.drift_time * 2; - double minutes = fmod(micros() / 60.0e6, period); - if (minutes < period/2) { - return minutes * ToRad(sitl.drift_speed); - } - return (period - minutes) * ToRad(sitl.drift_speed); - -} - -/* - setup the ADC channels with new input - - Note that this uses roll, pitch and yaw only as inputs. The - simulator rollrates are instantaneous, whereas we need to use - average rates to cope with slow update rates. - - inputs are in degrees - - phi - roll - theta - pitch - psi - true heading - alpha - angle of attack - beta - side slip - phidot - roll rate - thetadot - pitch rate - psidot - yaw rate - v_north - north velocity in local/body frame - v_east - east velocity in local/body frame - v_down - down velocity in local/body frame - A_X_pilot - X accel in body frame - A_Y_pilot - Y accel in body frame - A_Z_pilot - Z accel in body frame - - Note: doubles on high prec. stuff are preserved until the last moment - - */ -void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth - double rollRate, double pitchRate,double yawRate, // Local to plane - double xAccel, double yAccel, double zAccel, // Local to plane - float airspeed) -{ - static const uint8_t sensor_map[6] = { 1, 2, 0, 4, 5, 6 }; - static const float _sensor_signs[6] = { 1, -1, -1, 1, -1, -1 }; - const float accel_offset = 2041; - const float gyro_offset = 1658; - const float _gyro_gain_x = ToRad(0.4); - const float _gyro_gain_y = ToRad(0.41); - const float _gyro_gain_z = ToRad(0.41); - const float _accel_scale = 9.80665 / 423.8; - double adc[7]; - double p, q, r; - extern bool sitl_motors_on; - - SITL::convert_body_frame(roll, pitch, - rollRate, pitchRate, yawRate, - &p, &q, &r); - - // minimum noise levels are 2 bits - float accel_noise = _accel_scale*2; - float gyro_noise = _gyro_gain_y*2; - if (sitl_motors_on) { - // add extra noise when the motors are on - accel_noise += sitl.accel_noise; - gyro_noise += ToRad(sitl.gyro_noise); - } - xAccel += accel_noise * rand_float(); - yAccel += accel_noise * rand_float(); - zAccel += accel_noise * rand_float(); - - p += gyro_noise * rand_float(); - q += gyro_noise * rand_float(); - r += gyro_noise * rand_float(); - - p += gyro_drift(); - q += gyro_drift(); - r += gyro_drift(); - - /* work out the ADC channel values */ - adc[0] = (p / (_gyro_gain_x * _sensor_signs[0])) + gyro_offset; - adc[1] = (q / (_gyro_gain_y * _sensor_signs[1])) + gyro_offset; - adc[2] = (r / (_gyro_gain_z * _sensor_signs[2])) + gyro_offset; - - adc[3] = (xAccel / (_accel_scale * _sensor_signs[3])) + accel_offset; - adc[4] = (yAccel / (_accel_scale * _sensor_signs[4])) + accel_offset; - adc[5] = (zAccel / (_accel_scale * _sensor_signs[5])) + accel_offset; - - /* tell the UDR2 register emulation what values to give to the driver */ - for (uint8_t i=0; i<6; i++) { - UDR2.set(sensor_map[i], adc[i]); - } - // set the airspeed sensor input - UDR2.set(7, airspeed_sensor(airspeed)); - - /* FIX: rubbish value for temperature for now */ - UDR2.set(3, 2000); - - runInterrupt(6); -} - - -/* - setup ADC emulation - */ -void sitl_setup_adc(void) -{ - // mark it always ready. This is the register - // the ADC driver uses to tell if there is new data pending - UCSR2A = (1 << RXC2); -} diff --git a/libraries/Desktop/support/sitl_adc.h b/libraries/Desktop/support/sitl_adc.h deleted file mode 100644 index 4087c34968..0000000000 --- a/libraries/Desktop/support/sitl_adc.h +++ /dev/null @@ -1,78 +0,0 @@ -/* - ADS7844 register emulation - Code by Andrew Tridgell November 2011 - */ - -#ifndef _SITL_ADC_H -#define _SITL_ADC_H - -#include -#include -#include "util.h" - -// this implements the UDR2 register -struct ADC_UDR2 { - uint16_t value, next_value; - uint8_t idx; - float channels[8]; - - ADC_UDR2() { - // constructor - for (uint8_t i=0; i<8; i++) { - channels[i] = 0xFFFF; - } - value = next_value = 0; - idx = 0; - } - - /* - assignment of UDR2 selects which ADC channel - to output next - */ - ADC_UDR2& operator=(uint8_t cmd) { - float next_analog; - uint8_t chan; - switch (cmd) { - case 0x87: chan = 0; break; - case 0xC7: chan = 1; break; - case 0x97: chan = 2; break; - case 0xD7: chan = 3; break; - case 0xA7: chan = 4; break; - case 0xE7: chan = 5; break; - case 0xB7: chan = 6; break; - case 0xF7: chan = 7; break; - case 0: - default: return *this; - } - next_analog = channels[chan]; - idx = 1; - if (next_analog > 0xFFF) next_analog = 0xFFF; - if (next_analog < 0) next_analog = 0; - next_value = ((unsigned)next_analog) << 3; - return *this; - } - - /* - read from UDR2 fetches a byte from the channel - */ - operator int() { - uint8_t ret; - if (idx & 1) { - ret = (value&0xFF); - value = next_value; - } else { - ret = (value>>8); - } - idx ^= 1; - return ret; - } - - /* - interface to set a channel value from SITL - */ - void set(uint8_t i, float v) { - channels[i] = v; - } -}; - -#endif // _SITL_ADC_H diff --git a/libraries/Desktop/support/sitl_barometer.cpp b/libraries/Desktop/support/sitl_barometer.cpp deleted file mode 100644 index 465041f00c..0000000000 --- a/libraries/Desktop/support/sitl_barometer.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/* - SITL handling - - This simulates a barometer - - Andrew Tridgell November 2011 - */ -#include -#include -#include -#include -#include -#include // ArduPilot Mega BMP085 Library -#include -#include "desktop.h" -#include "util.h" - -extern SITL sitl; - -/* - setup the barometer with new input - altitude is in meters - */ -void sitl_update_barometer(float altitude) -{ - extern AP_Baro_BMP085_HIL barometer; - double Temp, Press, y; - static uint32_t last_update; - - // 80Hz, to match the real APM2 barometer - if (millis() - last_update < 12) { - return; - } - last_update = millis(); - - Temp = 312; - - y = ((altitude-584.0) * 1000.0) / 29271.267; - y /= (Temp / 10.0) + 273.15; - y = 1.0/exp(y); - y *= 95446.0; - - Press = y + (rand_float() * sitl.baro_noise); - - barometer.setHIL(Temp, Press); -} diff --git a/libraries/Desktop/support/sitl_compass.cpp b/libraries/Desktop/support/sitl_compass.cpp deleted file mode 100644 index 6c04b77b14..0000000000 --- a/libraries/Desktop/support/sitl_compass.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/* - SITL handling - - This simulates a compass - - Andrew Tridgell November 2011 - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "desktop.h" -#include "util.h" - -#define MAG_OFS_X 5.0 -#define MAG_OFS_Y 13.0 -#define MAG_OFS_Z -18.0 - -// inclination in Canberra (degrees) -#define MAG_INCLINATION -66 - -// magnetic field strength in Canberra as observed -// using an APM1 with 5883L compass -#define MAG_FIELD_STRENGTH 818 - -extern SITL sitl; - -/* - given a magnetic heading, and roll, pitch, yaw values, - calculate consistent magnetometer components - - All angles are in radians - */ -static Vector3f heading_to_mag(float roll, float pitch, float yaw) -{ - Vector3f Bearth, m; - Matrix3f R; - float declination = AP_Declination::get_declination(sitl.state.latitude, sitl.state.longitude); - - // Bearth is the magnetic field in Canberra. We need to adjust - // it for inclination and declination - Bearth(MAG_FIELD_STRENGTH, 0, 0); - R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(declination)); - Bearth = R * Bearth; - - // create a rotation matrix for the given attitude - R.from_euler(roll, pitch, yaw); - - // convert the earth frame magnetic vector to body frame, and - // apply the offsets - m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z); - return m + (rand_vec3f() * sitl.mag_noise); -} - - - -/* - setup the compass with new input - all inputs are in degrees - */ -void sitl_update_compass(float roll, float pitch, float yaw) -{ - extern AP_Compass_HIL compass; - Vector3f m = heading_to_mag(ToRad(roll), - ToRad(pitch), - ToRad(yaw)); - compass.setHIL(m.x, m.y, m.z); -} diff --git a/libraries/Desktop/support/sitl_gps.cpp b/libraries/Desktop/support/sitl_gps.cpp deleted file mode 100644 index b20008b742..0000000000 --- a/libraries/Desktop/support/sitl_gps.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/* - SITL handling - - This simulates a GPS on a serial port - - Andrew Tridgell November 2011 - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "desktop.h" -#include "util.h" - -extern SITL sitl; - -#define MAX_GPS_DELAY 100 - -struct gps_data { - double latitude; - double longitude; - float altitude; - double speedN; - double speedE; - bool have_lock; -} gps_data[MAX_GPS_DELAY]; - -static uint8_t next_gps_index; -static uint8_t gps_delay; - -// state of GPS emulation -static struct { - /* pipe emulating UBLOX GPS serial stream */ - int gps_fd, client_fd; - uint32_t last_update; // milliseconds -} gps_state; - -/* - hook for reading from the GPS pipe - */ -ssize_t sitl_gps_read(int fd, void *buf, size_t count) -{ -#ifdef FIONREAD - // use FIONREAD to get exact value if possible - int num_ready; - while (ioctl(fd, FIONREAD, &num_ready) == 0 && num_ready > 256) { - // the pipe is filling up - drain it - uint8_t tmp[128]; - if (read(fd, tmp, sizeof(tmp)) != sizeof(tmp)) { - break; - } - } -#endif - return read(fd, buf, count); -} - -/* - setup GPS input pipe - */ -int sitl_gps_pipe(void) -{ - int fd[2]; - if (gps_state.client_fd != 0) { - return gps_state.client_fd; - } - pipe(fd); - gps_state.gps_fd = fd[1]; - gps_state.client_fd = fd[0]; - gps_state.last_update = millis(); - set_nonblocking(gps_state.gps_fd); - set_nonblocking(fd[0]); - return gps_state.client_fd; -} - - -/* - send a UBLOX GPS message - */ -static void gps_send(uint8_t msgid, uint8_t *buf, uint16_t size) -{ - const uint8_t PREAMBLE1 = 0xb5; - const uint8_t PREAMBLE2 = 0x62; - const uint8_t CLASS_NAV = 0x1; - uint8_t hdr[6], chk[2]; - hdr[0] = PREAMBLE1; - hdr[1] = PREAMBLE2; - hdr[2] = CLASS_NAV; - hdr[3] = msgid; - hdr[4] = size & 0xFF; - hdr[5] = size >> 8; - chk[0] = chk[1] = hdr[2]; - chk[1] += (chk[0] += hdr[3]); - chk[1] += (chk[0] += hdr[4]); - chk[1] += (chk[0] += hdr[5]); - for (uint8_t i=0; i= gps_delay) { - next_gps_index = 0; - } - - d = gps_data[next_gps_index]; - - if (sitl.gps_delay != gps_delay) { - // cope with updates to the delay control - gps_delay = sitl.gps_delay; - for (uint8_t i=0; i -#include -#include -#include -#include -#include -#include -#include -#include "desktop.h" -#include "util.h" - - -void set_nonblocking(int fd) -{ - unsigned v = fcntl(fd, F_GETFL, 0); - fcntl(fd, F_SETFL, v | O_NONBLOCK); -} - -double normalise(double v, double min, double max) -{ - while (v < min) { - v += (max - min); - } - while (v > max) { - v -= (max - min); - } - return v; -} - -double normalise180(double v) -{ - return normalise(v, -180, 180); -} - -// generate a random Vector3f of size 1 -Vector3f rand_vec3f(void) -{ - Vector3f v = Vector3f(rand_float(), - rand_float(), - rand_float()); - if (v.length() != 0.0) { - v.normalize(); - } - return v; -} diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h deleted file mode 100644 index a4e54cce0e..0000000000 --- a/libraries/Desktop/support/util.h +++ /dev/null @@ -1,17 +0,0 @@ - -#define ft2m(x) ((x) * 0.3048) -#define kt2mps(x) ((x) * 0.514444444) -#define sqr(x) ((x)*(x)) -#define ToRad(x) (x*0.01745329252) // *pi/180 - -void set_nonblocking(int fd); -double normalise(double v, double min, double max); -double normalise180(double v); -void runInterrupt(uint8_t inum); - -// generate a random float between -1 and 1 -#define rand_float() (((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6) - -#ifdef VECTOR3_H -Vector3f rand_vec3f(void); -#endif