mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 00:04:02 -04:00
SITL: removed old SITL code
This commit is contained in:
Andrew Tridgell
parent
0c9d37e2ee
commit
69bebbcaf8
@ -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=<path>)
|
||||
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 <Foo.h>
|
||||
#
|
||||
# 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
|
||||
#
|
||||
# <newline><type>[<qualifier>...]<name>([<arguments>]){
|
||||
#
|
||||
# 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
|
||||
@ -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"
|
||||
@ -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.
|
||||
@ -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 <inttypes.h>
|
||||
|
||||
#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
|
||||
@ -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 <inttypes.h>
|
||||
#include <stdio.h> // 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
|
||||
@ -1,70 +0,0 @@
|
||||
/*
|
||||
* Copyright (c) 2010 by Cristian Maglie <c.maglie@bug.st>
|
||||
* 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 <stdio.h>
|
||||
#include <WProgram.h>
|
||||
#include <avr/pgmspace.h>
|
||||
|
||||
#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
|
||||
@ -1,16 +0,0 @@
|
||||
#ifndef Stream_h
|
||||
#define Stream_h
|
||||
|
||||
#include <inttypes.h>
|
||||
#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
|
||||
@ -1 +0,0 @@
|
||||
#include "wiring.h"
|
||||
@ -1,29 +0,0 @@
|
||||
#ifndef WProgram_h
|
||||
#define WProgram_h
|
||||
|
||||
#include "wiring.h"
|
||||
#include <stdarg.h>
|
||||
#include "HardwareSerial.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
|
||||
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
|
||||
@ -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 <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
|
||||
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
|
||||
@ -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 <inttypes.h>
|
||||
|
||||
#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
|
||||
|
||||
@ -1,24 +0,0 @@
|
||||
#ifndef _AVR_EEPROM_H_
|
||||
#define _AVR_EEPROM_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
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
|
||||
@ -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
|
||||
@ -1,25 +0,0 @@
|
||||
#ifndef _AVR_IO_H_
|
||||
#define _AVR_IO_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
@ -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 <avr/iomxx0_1.h>
|
||||
|
||||
/* 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_ */
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,57 +0,0 @@
|
||||
#ifndef _PGMSPACE_H
|
||||
#define _PGMSPACE_H
|
||||
|
||||
#include <stdarg.h>
|
||||
#include <unistd.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
#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
|
||||
@ -1,4 +0,0 @@
|
||||
|
||||
#define WDTO_15MS 15
|
||||
|
||||
#define wdt_enable(x)
|
||||
@ -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
|
||||
@ -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 <avr/pgmspace.h>
|
||||
|
||||
#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
|
||||
@ -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 <avr/io.h>
|
||||
#include <stdlib.h>
|
||||
#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
|
||||
@ -1,214 +0,0 @@
|
||||
#include "WProgram.h"
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include "avr/pgmspace.h"
|
||||
#include <BetterStream.h>
|
||||
#include <sys/time.h>
|
||||
#include <signal.h>
|
||||
#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;
|
||||
}
|
||||
@ -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 <unistd.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdint.h>
|
||||
#include "DataFlash.h"
|
||||
#include <SPI.h>
|
||||
#include <AP_Semaphore.h>
|
||||
|
||||
#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<DF_NUM_PAGES; i++) {
|
||||
PageErase(i);
|
||||
delay_cb(1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1,378 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
//
|
||||
// Interrupt-driven serial transmit/receive library.
|
||||
//
|
||||
// Copyright (c) 2010 Michael Smith. All rights reserved.
|
||||
//
|
||||
// Receive and baudrate calculations derived from the Arduino
|
||||
// HardwareSerial driver:
|
||||
//
|
||||
// Copyright (c) 2006 Nicholas Zambetti. All right reserved.
|
||||
//
|
||||
// Transmit algorithm inspired by work:
|
||||
//
|
||||
// Code Jose Julio and Jordi Munoz. DIYDrones.com
|
||||
//
|
||||
// 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 "../AP_Common/AP_Common.h"
|
||||
#include "FastSerial.h"
|
||||
#include "WProgram.h"
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#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<FS_MAX_PORTS; i++) {
|
||||
if (tcp_state[i].connected) {
|
||||
FD_SET(tcp_state[i].fd, fds);
|
||||
if (tcp_state[i].fd > *fd_high) {
|
||||
*fd_high = tcp_state[i].fd;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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 <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#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<digits; ++i)
|
||||
rounding /= 10.0;
|
||||
|
||||
number += rounding;
|
||||
|
||||
// Extract the integer part of the number and print it
|
||||
unsigned long int_part = (unsigned long)number;
|
||||
double remainder = number - (double)int_part;
|
||||
print(int_part);
|
||||
|
||||
// Print the decimal point, but only if there are digits beyond
|
||||
if (digits > 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;
|
||||
}
|
||||
}
|
||||
@ -1,61 +0,0 @@
|
||||
/*
|
||||
* Copyright (c) 2010 by Cristian Maglie <c.maglie@bug.st>
|
||||
* 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);
|
||||
}
|
||||
|
||||
@ -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; }
|
||||
@ -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 <stdlib.h>
|
||||
#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);
|
||||
}
|
||||
@ -1,41 +0,0 @@
|
||||
#ifndef _DESKTOP_H
|
||||
#define _DESKTOP_H
|
||||
|
||||
#include <unistd.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
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
|
||||
@ -1,22 +0,0 @@
|
||||
// support for digitalRead() and digitalWrite()
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
#include <stdint.h>
|
||||
#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;
|
||||
}
|
||||
@ -1,87 +0,0 @@
|
||||
#include <unistd.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <assert.h>
|
||||
#include <stdint.h>
|
||||
|
||||
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);
|
||||
}
|
||||
@ -1,115 +0,0 @@
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
#include <sched.h>
|
||||
#include <wiring.h>
|
||||
#include <getopt.h>
|
||||
#include <signal.h>
|
||||
#include <string.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Param.h>
|
||||
#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;
|
||||
}
|
||||
@ -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 <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/udp.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <time.h>
|
||||
#include <sys/time.h>
|
||||
#include <signal.h>
|
||||
#include <math.h>
|
||||
#include <APM_RC.h>
|
||||
#include <wiring.h>
|
||||
#include <AP_PeriodicProcess.h>
|
||||
#include <AP_TimerProcess.h>
|
||||
#include <SITL.h>
|
||||
#include <avr/interrupt.h>
|
||||
#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);
|
||||
}
|
||||
|
||||
|
||||
@ -1,151 +0,0 @@
|
||||
/*
|
||||
SITL handling
|
||||
|
||||
This emulates the ADS7844 ADC
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <sys/types.h>
|
||||
#include <math.h>
|
||||
#include <AP_ADC.h>
|
||||
#include <SITL.h>
|
||||
#include <avr/interrupt.h>
|
||||
#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);
|
||||
}
|
||||
@ -1,78 +0,0 @@
|
||||
/*
|
||||
ADS7844 register emulation
|
||||
Code by Andrew Tridgell November 2011
|
||||
*/
|
||||
|
||||
#ifndef _SITL_ADC_H
|
||||
#define _SITL_ADC_H
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#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
|
||||
@ -1,46 +0,0 @@
|
||||
/*
|
||||
SITL handling
|
||||
|
||||
This simulates a barometer
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
#include <AP_Baro.h> // ArduPilot Mega BMP085 Library
|
||||
#include <SITL.h>
|
||||
#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);
|
||||
}
|
||||
@ -1,74 +0,0 @@
|
||||
/*
|
||||
SITL handling
|
||||
|
||||
This simulates a compass
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Compass.h>
|
||||
#include <AP_Declination.h>
|
||||
#include <SITL.h>
|
||||
#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);
|
||||
}
|
||||
@ -1,243 +0,0 @@
|
||||
/*
|
||||
SITL handling
|
||||
|
||||
This simulates a GPS on a serial port
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <math.h>
|
||||
#include <SITL.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <AP_GPS_UBLOX.h>
|
||||
#include <sys/ioctl.h>
|
||||
#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<size; i++) {
|
||||
chk[1] += (chk[0] += buf[i]);
|
||||
}
|
||||
write(gps_state.gps_fd, hdr, sizeof(hdr));
|
||||
write(gps_state.gps_fd, buf, size);
|
||||
write(gps_state.gps_fd, chk, sizeof(chk));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
possibly send a new GPS UBLOX packet
|
||||
*/
|
||||
void sitl_update_gps(double latitude, double longitude, float altitude,
|
||||
double speedN, double speedE, bool have_lock)
|
||||
{
|
||||
struct ubx_nav_posllh {
|
||||
uint32_t time; // GPS msToW
|
||||
int32_t longitude;
|
||||
int32_t latitude;
|
||||
int32_t altitude_ellipsoid;
|
||||
int32_t altitude_msl;
|
||||
uint32_t horizontal_accuracy;
|
||||
uint32_t vertical_accuracy;
|
||||
} pos;
|
||||
struct ubx_nav_status {
|
||||
uint32_t time; // GPS msToW
|
||||
uint8_t fix_type;
|
||||
uint8_t fix_status;
|
||||
uint8_t differential_status;
|
||||
uint8_t res;
|
||||
uint32_t time_to_first_fix;
|
||||
uint32_t uptime; // milliseconds
|
||||
} status;
|
||||
struct ubx_nav_velned {
|
||||
uint32_t time; // GPS msToW
|
||||
int32_t ned_north;
|
||||
int32_t ned_east;
|
||||
int32_t ned_down;
|
||||
uint32_t speed_3d;
|
||||
uint32_t speed_2d;
|
||||
int32_t heading_2d;
|
||||
uint32_t speed_accuracy;
|
||||
uint32_t heading_accuracy;
|
||||
} velned;
|
||||
struct ubx_nav_solution {
|
||||
uint32_t time;
|
||||
int32_t time_nsec;
|
||||
int16_t week;
|
||||
uint8_t fix_type;
|
||||
uint8_t fix_status;
|
||||
int32_t ecef_x;
|
||||
int32_t ecef_y;
|
||||
int32_t ecef_z;
|
||||
uint32_t position_accuracy_3d;
|
||||
int32_t ecef_x_velocity;
|
||||
int32_t ecef_y_velocity;
|
||||
int32_t ecef_z_velocity;
|
||||
uint32_t speed_accuracy;
|
||||
uint16_t position_DOP;
|
||||
uint8_t res;
|
||||
uint8_t satellites;
|
||||
uint32_t res2;
|
||||
} sol;
|
||||
const uint8_t MSG_POSLLH = 0x2;
|
||||
const uint8_t MSG_STATUS = 0x3;
|
||||
const uint8_t MSG_VELNED = 0x12;
|
||||
const uint8_t MSG_SOL = 0x6;
|
||||
struct gps_data d;
|
||||
|
||||
// 5Hz, to match the real UBlox config in APM
|
||||
if (millis() - gps_state.last_update < 200) {
|
||||
return;
|
||||
}
|
||||
gps_state.last_update = millis();
|
||||
|
||||
d.latitude = latitude;
|
||||
d.longitude = longitude;
|
||||
d.altitude = altitude;
|
||||
d.speedN = speedN;
|
||||
d.speedE = speedE;
|
||||
d.have_lock = have_lock;
|
||||
|
||||
// add in some GPS lag
|
||||
gps_data[next_gps_index++] = d;
|
||||
if (next_gps_index >= 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<gps_delay; i++) {
|
||||
gps_data[i] = d;
|
||||
}
|
||||
}
|
||||
|
||||
pos.time = millis(); // FIX
|
||||
pos.longitude = d.longitude * 1.0e7;
|
||||
pos.latitude = d.latitude * 1.0e7;
|
||||
pos.altitude_ellipsoid = d.altitude*1000.0;
|
||||
pos.altitude_msl = d.altitude*1000.0;
|
||||
pos.horizontal_accuracy = 5;
|
||||
pos.vertical_accuracy = 10;
|
||||
|
||||
status.time = pos.time;
|
||||
status.fix_type = d.have_lock?3:0;
|
||||
status.fix_status = d.have_lock?1:0;
|
||||
status.differential_status = 0;
|
||||
status.res = 0;
|
||||
status.time_to_first_fix = 0;
|
||||
status.uptime = millis();
|
||||
|
||||
velned.time = pos.time;
|
||||
velned.ned_north = 100.0 * d.speedN;
|
||||
velned.ned_east = 100.0 * d.speedE;
|
||||
velned.ned_down = 0;
|
||||
#define sqr(x) ((x)*(x))
|
||||
velned.speed_2d = sqrt(sqr(d.speedN)+sqr(d.speedE)) * 100;
|
||||
velned.speed_3d = velned.speed_2d;
|
||||
velned.heading_2d = ToDeg(atan2(d.speedE, d.speedN)) * 100000.0;
|
||||
if (velned.heading_2d < 0.0) {
|
||||
velned.heading_2d += 360.0 * 100000.0;
|
||||
}
|
||||
velned.speed_accuracy = 2;
|
||||
velned.heading_accuracy = 4;
|
||||
|
||||
memset(&sol, 0, sizeof(sol));
|
||||
sol.fix_type = d.have_lock?3:0;
|
||||
sol.fix_status = 221;
|
||||
sol.satellites = d.have_lock?10:3;
|
||||
|
||||
if (gps_state.gps_fd == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
gps_send(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
|
||||
gps_send(MSG_STATUS, (uint8_t*)&status, sizeof(status));
|
||||
gps_send(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
|
||||
gps_send(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
|
||||
}
|
||||
@ -1,52 +0,0 @@
|
||||
/*
|
||||
RC input emulation
|
||||
Code by Andrew Tridgell November 2011
|
||||
*/
|
||||
|
||||
#ifndef _SITL_RC_H
|
||||
#define _SITL_RC_H
|
||||
|
||||
struct RC_ICR4 {
|
||||
uint16_t channels[9]; // 9th channel is sync
|
||||
uint32_t value;
|
||||
uint8_t idx;
|
||||
|
||||
RC_ICR4() {
|
||||
// constructor
|
||||
channels[0] = channels[1] = channels[3] = 1500;
|
||||
channels[4] = channels[7] = 1800;
|
||||
channels[2] = channels[5] = channels[6] = 1000;
|
||||
channels[8] = 4500; // sync
|
||||
idx = 0;
|
||||
}
|
||||
|
||||
/*
|
||||
read from ICR4 fetches next channel
|
||||
*/
|
||||
operator int() {
|
||||
value += channels[idx++]*2;
|
||||
if (idx == 9) {
|
||||
idx = 0;
|
||||
}
|
||||
value = value % 40000;
|
||||
return (uint16_t)value;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
ignore rate assignment for now (needed for apm2
|
||||
emulation)
|
||||
*/
|
||||
RC_ICR4& operator=(uint16_t rate) {
|
||||
return *this;
|
||||
}
|
||||
|
||||
/*
|
||||
interface to set a channel value from SITL
|
||||
*/
|
||||
void set(uint8_t i, uint16_t v) {
|
||||
channels[i] = v;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -1,53 +0,0 @@
|
||||
/*
|
||||
SITL handling - utility functions
|
||||
|
||||
This simulates the APM1 hardware sufficiently for the APM code to
|
||||
think it is running on real hardware
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <fcntl.h>
|
||||
#include <AP_Math.h>
|
||||
#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;
|
||||
}
|
||||
@ -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
|
||||
Loading…
Reference in New Issue
Block a user