diff --git a/.gitignore b/.gitignore
index 6a0bf36496..78379d4bf0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,6 @@
+# ignore html dir for github pages
+/doc/html
+
 *.so
 *.[oa]
 *.out
@@ -17,6 +20,13 @@
 
 *.aux
 
+# Eclipse IDE project files
+*.cproject
+*.project
+*.externalToolBuilders
+/build.properties
+/META-INF
+
 # Debian related files
 *.deb
 *.dsc
diff --git a/Doxyfile b/Doxyfile
index 934897d58e..bd783c89f6 100644
--- a/Doxyfile
+++ b/Doxyfile
@@ -1,4 +1,4 @@
-# Doxyfile 1.6.3
+# Doxyfile 1.7.4
 
 # This file describes the settings to be used by the documentation system
 # doxygen (www.doxygen.org) for a project
@@ -25,20 +25,33 @@ DOXYFILE_ENCODING      = UTF-8
 # The PROJECT_NAME tag is a single word (or a sequence of words surrounded
 # by quotes) that should identify the project.
 
-PROJECT_NAME           = Paparazzi
+PROJECT_NAME           = "Paparazzi UAS"
 
 # The PROJECT_NUMBER tag can be used to enter a project or revision number.
 # This could be handy for archiving the generated documentation or
 # if some version control system is used.
 
-PROJECT_NUMBER         = 4
+PROJECT_NUMBER         =
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer
+# a quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF          = "Paparazzi is a free software Unmanned Aircraft System."
+
+# With the PROJECT_LOGO tag one can specify an logo or icon that is
+# included in the documentation. The maximum height of the logo should not
+# exceed 55 pixels and the maximum width should not exceed 200 pixels.
+# Doxygen will copy the logo to the output directory.
+
+PROJECT_LOGO           = data/pictures/penguin_icon.png
 
 # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute)
 # base path where the generated documentation will be put.
 # If a relative path is entered, it will be relative to the location
 # where doxygen was started. If left blank the current directory will be used.
 
-OUTPUT_DIRECTORY       = dox
+OUTPUT_DIRECTORY       = doc/html
 
 # If the CREATE_SUBDIRS tag is set to YES, then doxygen will create
 # 4096 sub-directories (in 2 levels) under the output directory of each output
@@ -57,7 +70,7 @@ CREATE_SUBDIRS         = NO
 # Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German,
 # Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English
 # messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian,
-# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrilic, Slovak,
+# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak,
 # Slovene, Spanish, Swedish, Ukrainian, and Vietnamese.
 
 OUTPUT_LANGUAGE        = English
@@ -126,7 +139,7 @@ STRIP_FROM_PATH        =
 STRIP_FROM_INC_PATH    =
 
 # If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter
-# (but less readable) file names. This can be useful is your file systems
+# (but less readable) file names. This can be useful if your file system
 # doesn't support long names like on DOS, Mac, or CD-ROM.
 
 SHORT_NAMES            = NO
@@ -137,7 +150,7 @@ SHORT_NAMES            = NO
 # comments will behave just like regular Qt-style comments
 # (thus requiring an explicit @brief command for a brief description.)
 
-JAVADOC_AUTOBRIEF      = NO
+JAVADOC_AUTOBRIEF      = YES
 
 # If the QT_AUTOBRIEF tag is set to YES then Doxygen will
 # interpret the first line (until the first dot) of a Qt-style
@@ -207,14 +220,15 @@ OPTIMIZE_FOR_FORTRAN   = NO
 
 OPTIMIZE_OUTPUT_VHDL   = NO
 
-# Doxygen selects the parser to use depending on the extension of the files it parses.
-# With this tag you can assign which parser to use for a given extension.
-# Doxygen has a built-in mapping, but you can override or extend it using this tag.
-# The format is ext=language, where ext is a file extension, and language is one of
-# the parsers supported by doxygen: IDL, Java, Javascript, C#, C, C++, D, PHP,
-# Objective-C, Python, Fortran, VHDL, C, C++. For instance to make doxygen treat
-# .inc files as Fortran files (default is PHP), and .f files as C (default is Fortran),
-# use: inc=Fortran f=C. Note that for custom extensions you also need to set FILE_PATTERNS otherwise the files are not read by doxygen.
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given extension.
+# Doxygen has a built-in mapping, but you can override or extend it using this
+# tag. The format is ext=language, where ext is a file extension, and language
+# is one of the parsers supported by doxygen: IDL, Java, Javascript, CSharp, C,
+# C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, C++. For instance to make
+# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C
+# (default is Fortran), use: inc=Fortran f=C. Note that for custom extensions
+# you also need to set FILE_PATTERNS otherwise the files are not read by doxygen.
 
 EXTENSION_MAPPING      =
 
@@ -222,7 +236,7 @@ EXTENSION_MAPPING      =
 # to include (a tag file for) the STL sources as input, then you should
 # set this tag to YES in order to let doxygen match functions declarations and
 # definitions whose arguments contain STL classes (e.g. func(std::string); v.s.
-# func(std::string) {}). This also make the inheritance and collaboration
+# func(std::string) {}). This also makes the inheritance and collaboration
 # diagrams that involve STL classes more complete and accurate.
 
 BUILTIN_STL_SUPPORT    = NO
@@ -240,7 +254,7 @@ SIP_SUPPORT            = NO
 
 # For Microsoft's IDL there are propget and propput attributes to indicate getter
 # and setter methods for a property. Setting this option to YES (the default)
-# will make doxygen to replace the get and set methods by a property in the
+# will make doxygen replace the get and set methods by a property in the
 # documentation. This will only work if the methods are indeed getting or
 # setting a simple type. If this is not the case, or you want to show the
 # methods anyway, you should set this option to NO.
@@ -262,6 +276,13 @@ DISTRIBUTE_GROUP_DOC   = NO
 
 SUBGROUPING            = YES
 
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and
+# unions are shown inside the group in which they are included (e.g. using
+# @ingroup) instead of on a separate page (for HTML and Man pages) or
+# section (for LaTeX and RTF).
+
+INLINE_GROUPED_CLASSES = NO
+
 # When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum
 # is documented as struct, union, or enum with the name of the typedef. So
 # typedef struct TypeS {} TypeT, will appear in the documentation as a struct
@@ -278,10 +299,10 @@ TYPEDEF_HIDES_STRUCT   = NO
 # For small to medium size projects (<1000 input files) the default value is
 # probably good enough. For larger projects a too small cache size can cause
 # doxygen to be busy swapping symbols to and from disk most of the time
-# causing a significant performance penality.
+# causing a significant performance penalty.
 # If the system has enough physical memory increasing the cache will improve the
 # performance by keeping more symbols in memory. Note that the value works on
-# a logarithmic scale so increasing the size by one will rougly double the
+# a logarithmic scale so increasing the size by one will roughly double the
 # memory usage. The cache size is given by this formula:
 # 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0,
 # corresponding to a cache size of 2^16 = 65536 symbols
@@ -326,7 +347,7 @@ EXTRACT_LOCAL_METHODS  = NO
 # extracted and appear in the documentation as a namespace called
 # 'anonymous_namespace{file}', where file will be replaced with the base
 # name of the file that contains the anonymous namespace. By default
-# anonymous namespace are hidden.
+# anonymous namespaces are hidden.
 
 EXTRACT_ANON_NSPACES   = NO
 
@@ -411,7 +432,13 @@ SORT_MEMBER_DOCS       = YES
 
 SORT_BRIEF_DOCS        = NO
 
-# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the (brief and detailed) documentation of class members so that constructors and destructors are listed first. If set to NO (the default) the constructors will appear in the respective orders defined by SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO.
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen
+# will sort the (brief and detailed) documentation of class members so that
+# constructors and destructors are listed first. If set to NO (the default)
+# the constructors will appear in the respective orders defined by
+# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS.
+# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO
+# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO.
 
 SORT_MEMBERS_CTORS_1ST = NO
 
@@ -431,6 +458,15 @@ SORT_GROUP_NAMES       = NO
 
 SORT_BY_SCOPE_NAME     = NO
 
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to
+# do proper type resolution of all parameters of a function it will reject a
+# match between the prototype and the implementation of a member function even
+# if there is only one candidate or it is obvious which candidate to choose
+# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen
+# will still accept a match between prototype and implementation in such cases.
+
+STRICT_PROTO_MATCHING  = NO
+
 # The GENERATE_TODOLIST tag can be used to enable (YES) or
 # disable (NO) the todo list. This list is created by putting \todo
 # commands in the documentation.
@@ -461,10 +497,10 @@ GENERATE_DEPRECATEDLIST= YES
 ENABLED_SECTIONS       =
 
 # The MAX_INITIALIZER_LINES tag determines the maximum number of lines
-# the initial value of a variable or define consists of for it to appear in
+# the initial value of a variable or macro consists of for it to appear in
 # the documentation. If the initializer consists of more lines than specified
 # here it will be hidden. Use a value of 0 to hide initializers completely.
-# The appearance of the initializer of individual variables and defines in the
+# The appearance of the initializer of individual variables and macros in the
 # documentation can be controlled using \showinitializer or \hideinitializer
 # command in the documentation regardless of this setting.
 
@@ -489,8 +525,7 @@ SHOW_DIRECTORIES       = YES
 SHOW_FILES             = YES
 
 # Set the SHOW_NAMESPACES tag to NO to disable the generation of the
-# Namespaces page.
-# This will remove the Namespaces entry from the Quick Index
+# Namespaces page.  This will remove the Namespaces entry from the Quick Index
 # and from the Folder Tree View (if specified). The default is YES.
 
 SHOW_NAMESPACES        = YES
@@ -505,12 +540,12 @@ SHOW_NAMESPACES        = YES
 
 FILE_VERSION_FILTER    =
 
-# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed by
-# doxygen. The layout file controls the global structure of the generated output files
-# in an output format independent way. The create the layout file that represents
-# doxygen's defaults, run doxygen with the -l option. You can optionally specify a
-# file name after the option, if omitted DoxygenLayout.xml will be used as the name
-# of the layout file.
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. The create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option.
+# You can optionally specify a file name after the option, if omitted
+# DoxygenLayout.xml will be used as the name of the layout file.
 
 LAYOUT_FILE            =
 
@@ -542,7 +577,7 @@ WARN_IF_UNDOCUMENTED   = YES
 
 WARN_IF_DOC_ERROR      = YES
 
-# This WARN_NO_PARAMDOC option can be abled to get warnings for
+# The WARN_NO_PARAMDOC option can be enabled to get warnings for
 # functions that are documented, but have no documentation for their parameters
 # or return value. If set to NO (the default) doxygen will only warn about
 # wrong or incomplete parameter documentation, but not about the absence of
@@ -574,7 +609,7 @@ WARN_LOGFILE           =
 # directories like "/usr/src/myproject". Separate the files or directories
 # with spaces.
 
-INPUT                  = sw/airborne sw/airborne/math
+INPUT                  = doc/manual sw/airborne
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is
@@ -588,8 +623,9 @@ INPUT_ENCODING         = UTF-8
 # FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp
 # and *.h) to filter out the source-files in the directories. If left
 # blank the following patterns are tested:
-# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx
-# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90
+# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh
+# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py
+# *.f90 *.f *.for *.vhd *.vhdl
 
 FILE_PATTERNS          =
 
@@ -597,16 +633,30 @@ FILE_PATTERNS          =
 # should be searched for input files as well. Possible values are YES and NO.
 # If left blank NO is used.
 
-RECURSIVE              = NO
+RECURSIVE              = YES
 
 # The EXCLUDE tag can be used to specify files and/or directories that should
 # excluded from the INPUT source files. This way you can easily exclude a
 # subdirectory from a directory tree whose root is specified with the INPUT tag.
 
-EXCLUDE                = var
+EXCLUDE                = sw/airborne/csc/ \
+                         sw/airborne/obsolete/ \
+                         sw/airborne/booz/ \
+                         sw/airborne/test/ \
+                         sw/airborne/arch/avr/ \
+                         sw/airborne/firmwares/non_ap/ \
+                         sw/airborne/firmwares/logger/ \
+                         sw/airborne/firmwares/vor/ \
+                         sw/airborne/firmwares/wind_tunnel/ \
+                         sw/airborne/firmwares/beth/ \
+                         sw/airborne/firmwares/motor_bench/ \
+                         sw/airborne/fms/ \
+                         sw/airborne/arch/lpc21/test/ \
+                         sw/airborne/arch/lpc21/efsl/ \
+                         sw/airborne/arch/lpc21/lpcusb/
 
 # The EXCLUDE_SYMLINKS tag can be used select whether or not files or
-# directories that are symbolic links (a Unix filesystem feature) are excluded
+# directories that are symbolic links (a Unix file system feature) are excluded
 # from the input.
 
 EXCLUDE_SYMLINKS       = NO
@@ -658,20 +708,17 @@ IMAGE_PATH             =
 # by executing (via popen()) the command <filter> <input-file>, where <filter>
 # is the value of the INPUT_FILTER tag, and <input-file> is the name of an
 # input file. Doxygen will then use the output that the filter program writes
-# to standard output.
-# If FILTER_PATTERNS is specified, this tag will be
+# to standard output.  If FILTER_PATTERNS is specified, this tag will be
 # ignored.
 
 INPUT_FILTER           =
 
 # The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
-# basis.
-# Doxygen will compare the file name with each pattern and apply the
-# filter if there is a match.
-# The filters are a list of the form:
+# basis.  Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match.  The filters are a list of the form:
 # pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further
-# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER
-# is applied to all files.
+# info on how filters are used. If FILTER_PATTERNS is empty or if
+# non of the patterns match the file name, INPUT_FILTER is applied.
 
 FILTER_PATTERNS        =
 
@@ -681,6 +728,14 @@ FILTER_PATTERNS        =
 
 FILTER_SOURCE_FILES    = NO
 
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any)
+# and it is also possible to disable source filtering for a specific pattern
+# using *.ext= (so without naming a filter). This option only has effect when
+# FILTER_SOURCE_FILES is enabled.
+
+FILTER_SOURCE_PATTERNS =
+
 #---------------------------------------------------------------------------
 # configuration options related to source browsing
 #---------------------------------------------------------------------------
@@ -718,8 +773,7 @@ REFERENCES_RELATION    = YES
 # If the REFERENCES_LINK_SOURCE tag is set to YES (the default)
 # and SOURCE_BROWSER tag is set to YES, then the hyperlinks from
 # functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will
-# link to the source code.
-# Otherwise they will link to the documentation.
+# link to the source code.  Otherwise they will link to the documentation.
 
 REFERENCES_LINK_SOURCE = YES
 
@@ -745,7 +799,7 @@ VERBATIM_HEADERS       = YES
 # of all compounds will be generated. Enable this if the project
 # contains a lot of classes, structs, unions or interfaces.
 
-ALPHABETICAL_INDEX     = NO
+ALPHABETICAL_INDEX     = YES
 
 # If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then
 # the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns
@@ -773,7 +827,7 @@ GENERATE_HTML          = YES
 # If a relative path is entered the value of OUTPUT_DIRECTORY will be
 # put in front of it. If left blank `html' will be used as the default path.
 
-HTML_OUTPUT            = html
+HTML_OUTPUT            = docs
 
 # The HTML_FILE_EXTENSION tag can be used to specify the file extension for
 # each generated HTML page (for example: .htm,.php,.asp). If it is left blank
@@ -783,7 +837,14 @@ HTML_FILE_EXTENSION    = .html
 
 # The HTML_HEADER tag can be used to specify a personal HTML header for
 # each generated HTML page. If it is left blank doxygen will generate a
-# standard header.
+# standard header. Note that when using a custom header you are responsible
+# for the proper inclusion of any scripts and style sheets that doxygen
+# needs, which is dependent on the configuration options used.
+# It is adviced to generate a default header using "doxygen -w html
+# header.html footer.html stylesheet.css YourConfigFile" and then modify
+# that header. Note that the header is subject to change so you typically
+# have to redo this when upgrading to a newer version of doxygen or when
+# changing the value of configuration settings such as GENERATE_TREEVIEW!
 
 HTML_HEADER            =
 
@@ -802,6 +863,40 @@ HTML_FOOTER            =
 
 HTML_STYLESHEET        =
 
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that
+# the files will be copied as-is; there are no commands or markers available.
+
+HTML_EXTRA_FILES       =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output.
+# Doxygen will adjust the colors in the stylesheet and background images
+# according to this color. Hue is specified as an angle on a colorwheel,
+# see http://en.wikipedia.org/wiki/Hue for more information.
+# For instance the value 0 represents red, 60 is yellow, 120 is green,
+# 180 is cyan, 240 is blue, 300 purple, and 360 is red again.
+# The allowed range is 0 to 359.
+
+HTML_COLORSTYLE_HUE    = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of
+# the colors in the HTML output. For a value of 0 the output will use
+# grayscales only. A value of 255 will produce the most vivid colors.
+
+HTML_COLORSTYLE_SAT    = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to
+# the luminance component of the colors in the HTML output. Values below
+# 100 gradually make the output lighter, whereas values above 100 make
+# the output darker. The value divided by 100 is the actual gamma applied,
+# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2,
+# and 100 does not change the gamma.
+
+HTML_COLORSTYLE_GAMMA  = 80
+
 # If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
 # page will contain the date and time when the page was generated. Setting
 # this to NO can help when comparing the output of multiple runs.
@@ -830,7 +925,8 @@ HTML_DYNAMIC_SECTIONS  = YES
 # directory and running "make install" will install the docset in
 # ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find
 # it at startup.
-# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html for more information.
+# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
 
 GENERATE_DOCSET        = NO
 
@@ -848,6 +944,16 @@ DOCSET_FEEDNAME        = "Doxygen generated docs"
 
 DOCSET_BUNDLE_ID       = org.doxygen.Project
 
+# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+
+DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
+
+# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher.
+
+DOCSET_PUBLISHER_NAME  = Publisher
+
 # If the GENERATE_HTMLHELP tag is set to YES, additional index files
 # will be generated that can be used as input for tools like the
 # Microsoft HTML help workshop to generate a compiled HTML help file (.chm)
@@ -892,10 +998,10 @@ BINARY_TOC             = NO
 
 TOC_EXPAND             = NO
 
-# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and QHP_VIRTUAL_FOLDER
-# are set, an additional index file will be generated that can be used as input for
-# Qt's qhelpgenerator to generate a Qt Compressed Help (.qch) of the generated
-# HTML documentation.
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated
+# that can be used as input for Qt's qhelpgenerator to generate a
+# Qt Compressed Help (.qch) of the generated HTML documentation.
 
 GENERATE_QHP           = NO
 
@@ -917,20 +1023,24 @@ QHP_NAMESPACE          = org.doxygen.Project
 
 QHP_VIRTUAL_FOLDER     = doc
 
-# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to add.
-# For more information please see
+# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to
+# add. For more information please see
 # http://doc.trolltech.com/qthelpproject.html#custom-filters
 
 QHP_CUST_FILTER_NAME   =
 
-# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the custom filter to add.For more information please see
-# <a href="http://doc.trolltech.com/qthelpproject.html#custom-filters">Qt Help Project / Custom Filters</a>.
+# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see
+# <a href="http://doc.trolltech.com/qthelpproject.html#custom-filters">
+# Qt Help Project / Custom Filters</a>.
 
 QHP_CUST_FILTER_ATTRS  =
 
-# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this project's
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's
 # filter section matches.
-# <a href="http://doc.trolltech.com/qthelpproject.html#filter-attributes">Qt Help Project / Filter Attributes</a>.
+# <a href="http://doc.trolltech.com/qthelpproject.html#filter-attributes">
+# Qt Help Project / Filter Attributes</a>.
 
 QHP_SECT_FILTER_ATTRS  =
 
@@ -942,12 +1052,13 @@ QHP_SECT_FILTER_ATTRS  =
 QHG_LOCATION           =
 
 # If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files
-#  will be generated, which together with the HTML files, form an Eclipse help
-#  plugin. To install this plugin and make it available under the help contents
+# will be generated, which together with the HTML files, form an Eclipse help
+# plugin. To install this plugin and make it available under the help contents
 # menu in Eclipse, the contents of the directory containing the HTML and XML
 # files needs to be copied into the plugins directory of eclipse. The name of
 # the directory within the plugins directory should be the same as
-# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before the help appears.
+# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before
+# the help appears.
 
 GENERATE_ECLIPSEHELP   = NO
 
@@ -963,8 +1074,10 @@ ECLIPSE_DOC_ID         = org.doxygen.Project
 
 DISABLE_INDEX          = NO
 
-# This tag can be used to set the number of enum values (range [1..20])
-# that doxygen will group on one line in the generated HTML documentation.
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values
+# (range [0,1..20]) that doxygen will group on one line in the generated HTML
+# documentation. Note that a value of 0 will completely suppress the enum
+# values from appearing in the overview section.
 
 ENUM_VALUES_PER_LINE   = 4
 
@@ -976,7 +1089,7 @@ ENUM_VALUES_PER_LINE   = 4
 # JavaScript, DHTML, CSS and frames is required (i.e. any modern browser).
 # Windows users are probably better off using the HTML help feature.
 
-GENERATE_TREEVIEW      = NO
+GENERATE_TREEVIEW      = YES
 
 # By enabling USE_INLINE_TREES, doxygen will generate the Groups, Directories,
 # and Class Hierarchy pages using a tree view instead of an ordered list.
@@ -989,6 +1102,11 @@ USE_INLINE_TREES       = NO
 
 TREEVIEW_WIDTH         = 250
 
+# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open
+# links to external symbols imported via tag files in a separate window.
+
+EXT_LINKS_IN_WINDOW    = NO
+
 # Use this tag to change the font size of Latex formulas included
 # as images in the HTML documentation. The default is 10. Note that
 # when you change the font size after a successful doxygen run you need
@@ -997,15 +1115,50 @@ TREEVIEW_WIDTH         = 250
 
 FORMULA_FONTSIZE       = 10
 
-# When the SEARCHENGINE tag is enabled doxygen will generate a search box for the HTML output. The underlying search engine uses javascript
-# and DHTML and should work on any modern browser. Note that when using HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) there is already a search function so this one should
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are
+# not supported properly for IE 6.0, but are supported on all modern browsers.
+# Note that when changing this option you need to delete any form_*.png files
+# in the HTML output before the changes have effect.
+
+FORMULA_TRANSPARENT    = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax
+# (see http://www.mathjax.org) which uses client side Javascript for the
+# rendering instead of using prerendered bitmaps. Use this if you do not
+# have LaTeX installed or if you want to formulas look prettier in the HTML
+# output. When enabled you also need to install MathJax separately and
+# configure the path to it using the MATHJAX_RELPATH option.
+
+USE_MATHJAX            = NO
+
+# When MathJax is enabled you need to specify the location relative to the
+# HTML output directory using the MATHJAX_RELPATH option. The destination
+# directory should contain the MathJax.js script. For instance, if the mathjax
+# directory is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the
+# mathjax.org site, so you can quickly see the result without installing
+# MathJax, but it is strongly recommended to install a local copy of MathJax
+# before deployment.
+
+MATHJAX_RELPATH        = http://www.mathjax.org/mathjax
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box
+# for the HTML output. The underlying search engine uses javascript
+# and DHTML and should work on any modern browser. Note that when using
+# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets
+# (GENERATE_DOCSET) there is already a search function so this one should
 # typically be disabled. For large projects the javascript based search engine
 # can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution.
 
 SEARCHENGINE           = YES
 
-# When the SERVER_BASED_SEARCH tag is enabled the search engine will be implemented using a PHP enabled web server instead of at the web client using Javascript. Doxygen will generate the search PHP script and index
-# file to put on the web server. The advantage of the server based approach is that it scales better to large projects and allows full text search. The disadvances is that it is more difficult to setup
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a PHP enabled web server instead of at the web client
+# using Javascript. Doxygen will generate the search PHP script and index
+# file to put on the web server. The advantage of the server
+# based approach is that it scales better to large projects and allows
+# full text search. The disadvantages are that it is more difficult to setup
 # and does not have live searching capabilities.
 
 SERVER_BASED_SEARCH    = NO
@@ -1046,7 +1199,7 @@ MAKEINDEX_CMD_NAME     = makeindex
 COMPACT_LATEX          = NO
 
 # The PAPER_TYPE tag can be used to set the paper type that is used
-# by the printer. Possible values are: a4, a4wide, letter, legal and
+# by the printer. Possible values are: a4, letter, legal and
 # executive. If left blank a4wide will be used.
 
 PAPER_TYPE             = a4wide
@@ -1063,6 +1216,13 @@ EXTRA_PACKAGES         =
 
 LATEX_HEADER           =
 
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for
+# the generated latex document. The footer should contain everything after
+# the last chapter. If it is left blank doxygen will generate a
+# standard footer. Notice: only use this tag if you know what you are doing!
+
+LATEX_FOOTER           =
+
 # If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated
 # is prepared for conversion to pdf (using ps2pdf). The pdf file will
 # contain links (just like the HTML output) instead of page references
@@ -1089,7 +1249,10 @@ LATEX_BATCHMODE        = NO
 
 LATEX_HIDE_INDICES     = NO
 
-# If LATEX_SOURCE_CODE is set to YES then doxygen will include source code with syntax highlighting in the LaTeX output. Note that which sources are shown also depends on other settings such as SOURCE_BROWSER.
+# If LATEX_SOURCE_CODE is set to YES then doxygen will include
+# source code with syntax highlighting in the LaTeX output.
+# Note that which sources are shown also depends on other settings
+# such as SOURCE_BROWSER.
 
 LATEX_SOURCE_CODE      = NO
 
@@ -1229,10 +1392,8 @@ GENERATE_PERLMOD       = NO
 PERLMOD_LATEX          = NO
 
 # If the PERLMOD_PRETTY tag is set to YES the Perl module output will be
-# nicely formatted so it can be parsed by a human reader.
-# This is useful
-# if you want to understand what is going on.
-# On the other hand, if this
+# nicely formatted so it can be parsed by a human reader.  This is useful
+# if you want to understand what is going on.  On the other hand, if this
 # tag is set to NO the size of the Perl module output will be much smaller
 # and Perl will parse it just the same.
 
@@ -1269,7 +1430,7 @@ MACRO_EXPANSION        = NO
 EXPAND_ONLY_PREDEF     = NO
 
 # If the SEARCH_INCLUDES tag is set to YES (the default) the includes files
-# in the INCLUDE_PATH (see below) will be search if a #include is found.
+# pointed to by INCLUDE_PATH will be searched when a #include is found.
 
 SEARCH_INCLUDES        = YES
 
@@ -1277,8 +1438,7 @@ SEARCH_INCLUDES        = YES
 # contain include files that are not input files but should be processed by
 # the preprocessor.
 
-INCLUDE_PATH           = sw/include \
-                         sw/airborne
+INCLUDE_PATH           = sw/include
 
 # You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
 # patterns (like *.h and *.hpp) to filter out the header-files in the
@@ -1295,22 +1455,20 @@ INCLUDE_FILE_PATTERNS  =
 # undefined via #undef or recursively expanded use the := operator
 # instead of the = operator.
 
-PREDEFINED             = KILL_SWITCH \
-                         USE_BUSS_TWI_BLMC \
-                         USE_AMI601
+PREDEFINED             =
 
 # If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then
 # this tag can be used to specify a list of macro names that should be expanded.
 # The macro definition that is found in the sources will be used.
-# Use the PREDEFINED tag if you want to use a different macro definition.
+# Use the PREDEFINED tag if you want to use a different macro definition that
+# overrules the definition found in the source code.
 
 EXPAND_AS_DEFINED      =
 
 # If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then
-# doxygen's preprocessor will remove all function-like macros that are alone
-# on a line, have an all uppercase name, and do not end with a semicolon. Such
-# function macros are typically used for boiler-plate code, and will confuse
-# the parser if not removed.
+# doxygen's preprocessor will remove all references to function-like macros
+# that are alone on a line, have an all uppercase name, and do not end with a
+# semicolon, because these will confuse the parser if not removed.
 
 SKIP_FUNCTION_MACROS   = YES
 
@@ -1322,11 +1480,9 @@ SKIP_FUNCTION_MACROS   = YES
 # Optionally an initial location of the external documentation
 # can be added for each tagfile. The format of a tag file without
 # this location is as follows:
-#
-# TAGFILES = file1 file2 ...
+#   TAGFILES = file1 file2 ...
 # Adding location for the tag files is done as follows:
-#
-# TAGFILES = file1=loc1 "file2 = loc2" ...
+#   TAGFILES = file1=loc1 "file2 = loc2" ...
 # where "loc1" and "loc2" can be relative or absolute paths or
 # URLs. If a location is present for each tag, the installdox tool
 # does not have to be run to correct the links.
@@ -1366,9 +1522,8 @@ PERL_PATH              = /usr/bin/perl
 # If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will
 # generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base
 # or super classes. Setting the tag to NO turns the diagrams off. Note that
-# this option is superseded by the HAVE_DOT option below. This is only a
-# fallback. It is recommended to install and use dot, since it yields more
-# powerful graphs.
+# this option also works with HAVE_DOT disabled, but it is recommended to
+# install and use dot, since it yields more powerful graphs.
 
 CLASS_DIAGRAMS         = YES
 
@@ -1394,11 +1549,18 @@ HIDE_UNDOC_RELATIONS   = YES
 
 HAVE_DOT               = YES
 
-# By default doxygen will write a font called FreeSans.ttf to the output
-# directory and reference it in all dot files that doxygen generates. This
-# font does not include all possible unicode characters however, so when you need
-# these (or just want a differently looking font) you can specify the font name
-# using DOT_FONTNAME. You need need to make sure dot is able to find the font,
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is
+# allowed to run in parallel. When set to 0 (the default) doxygen will
+# base this on the number of processors available in the system. You can set it
+# explicitly to a value larger than 0 to get control over the balance
+# between CPU load and processing speed.
+
+DOT_NUM_THREADS        = 0
+
+# By default doxygen will write a font called Helvetica to the output
+# directory and reference it in all dot files that doxygen generates.
+# When you want a differently looking font you can specify the font name
+# using DOT_FONTNAME. You need to make sure dot is able to find the font,
 # which can be done by putting it in a standard location or by setting the
 # DOTFONTPATH environment variable or by setting DOT_FONTPATH to the directory
 # containing the font.
@@ -1478,7 +1640,7 @@ CALL_GRAPH             = YES
 CALLER_GRAPH           = YES
 
 # If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen
-# will graphical hierarchy of all classes instead of a textual one.
+# will generate a graphical hierarchy of all classes instead of a textual one.
 
 GRAPHICAL_HIERARCHY    = YES
 
@@ -1490,10 +1652,10 @@ GRAPHICAL_HIERARCHY    = YES
 DIRECTORY_GRAPH        = YES
 
 # The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
-# generated by dot. Possible values are png, jpg, or gif
+# generated by dot. Possible values are svg, png, jpg, or gif.
 # If left blank png will be used.
 
-DOT_IMAGE_FORMAT       = gif
+DOT_IMAGE_FORMAT       = png
 
 # The tag DOT_PATH can be used to specify the path where the dot tool can be
 # found. If left blank, it is assumed the dot tool can be found in the path.
@@ -1506,6 +1668,12 @@ DOT_PATH               =
 
 DOTFILE_DIRS           =
 
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the
+# \mscfile command).
+
+MSCFILE_DIRS           =
+
 # The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of
 # nodes that will be shown in the graph. If the number of nodes in a graph
 # becomes larger than this value, doxygen will truncate the graph, which is
diff --git a/README b/README
index 1fbae924f2..f93c8bf9a8 100644
--- a/README
+++ b/README
@@ -1,5 +1,4 @@
-#   Paparazzi $Id$
-#   Copyright (C) 2003-2010 The Paparazzi Team
+#   Copyright (C) 2003-2011 The Paparazzi Team
 #
 # This file is part of Paparazzi.
 #
@@ -25,7 +24,7 @@ Intro
 Paparazzi is an attempt to develop a free software Unmanned (Air) Vehicle System.
  As of today the system is being used successfuly by a number of hobyists, universities and companies all over the world, on vehicle of various size ( 100g to 25Kg ) and of various nature ( fixed wing, rotorcrafts, boats and surface vehicles).
 
-Up to date informations are available from the wiki website
+Up to date information is available from the wiki website
 
     http://paparazzi.enac.fr
 
@@ -39,8 +38,6 @@ conf: the configuration directory (airframe, radio, ... descriptions).
 
 data: where to put read-only data (e.g. maps, terrain elevation files, icons)
 
-hw: hardware (electronic schemas, PCBs, ...)
-
 sw: software (onboard, ground station, simulation, ...)
 
 var: products of compilation, cache for the map tiles, ...
@@ -64,8 +61,7 @@ For Debian or Ubuntu users, required packages are available at
 
 
   - "paparazzi-dev" will provide everything needed to compile and run the ground segment and the simulator. If something is missing, please report it.
-  - "paparazzi-arm7" is required to compile the code for LPC21 based boards ( tiny, twog, booz, etc).
-  - "paparazzi-stm32" is needed for building code for STM32 based boards (lisa/L, lisa/M)
+  - "paparazzi-arm-multilib" is required to compile the code for LPC21 and STM32 based boards
   - "paparazzi-omap" is needed for building code for the optional Gumstix Overo module available on lisa/L
 
   - "paparazzi-jsbsim" is needed for using jsbsim as flight dynamic model for the simulator.
diff --git a/conf/airframes/CDW/ChimuJTinyFwSpi.xml b/conf/airframes/CDW/ChimuJTinyFwSpi.xml
new file mode 100644
index 0000000000..98868c8e66
--- /dev/null
+++ b/conf/airframes/CDW/ChimuJTinyFwSpi.xml
@@ -0,0 +1,195 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<!--
+	YAPA + XSens + XBee
+-->
+
+<airframe name="Yapa v1">
+
+  <servos>
+    <servo name="THROTTLE" no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="1" min="1000" neutral="1500" max="2000"/>
+    <servo name="AILERON_RIGHT" no="2" min="2000" neutral="1500" max="1000"/>
+    <servo name="ELEVATOR" no="3" min="2000" neutral="1500" max="1000"/>
+    <servo name="RUDDER" no="4" min="1100" neutral="1500" max="1900"/>
+  </servos>
+
+  <commands>
+    <axis name="THROTTLE" failsafe_value="0"/>
+    <axis name="ROLL" failsafe_value="0"/>
+    <axis name="PITCH" failsafe_value="0"/>
+    <axis name="BRAKE" failsafe_value="0"/>	<!-- both elerons up as butterfly brake ? -->
+  </commands>
+
+  <rc_commands>
+    <set command="THROTTLE" value="@THROTTLE"/>
+    <set command="ROLL" value="@ROLL"/>
+    <set command="PITCH" value="@PITCH"/>
+    <set command="BRAKE" value="@YAW"/>
+  </rc_commands>
+
+  <section name="SERVO_MIXER_GAINS">
+    <define name="AILERON_RATE_UP" value="0.50f"/>
+    <define name="AILERON_RATE_DOWN" value="0.25f"/>
+
+    <define name="AILERON_RATE_UP_BRAKE" value="0.5f"/>
+    <define name="AILERON_RATE_DOWN_BRAKE" value="0.9f"/>
+
+    <define name="PITCH_GAIN" value="0.9f"/>
+
+    <define name="YAW_THRUST" value="0.0f"/>
+    <define name="BRAKE_AILEVON" value="-0.68f"/>
+    <define name="BRAKE_PITCH" value="0.0f"/>
+    <define name="MAX_BRAKE_RATE" value="150"/>
+
+    <define name="LIMIT(X,XL,XH)" value="(    ((X)>(XH))   ?    (XH)    :    ( ((X)>(XL)) ? (X) : (XL) )    )"/>
+  </section>
+
+  <command_laws>
+    <!-- Differential Aileron Depending on Brake Value -->
+    <set servo="AILERON_LEFT" value="@ROLL"/>
+    <set servo="AILERON_RIGHT" value="@ROLL"/>
+
+    <!-- Differential Thurst -->
+    <set servo="THROTTLE" value="@THROTTLE"/>
+
+    <!-- Pitch with Brake-Trim Function -->
+    <set servo="ELEVATOR" value="@PITCH"/>
+  </command_laws>
+
+  <section name="INS" prefix="INS_">
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+  </section>
+
+  <section name="AUTO1" prefix="AUTO1_">
+    <define name="MAX_ROLL" value="0.7"/>
+    <define name="MAX_PITCH" value="0.7"/>
+  </section>
+
+  <section name="BAT">
+    <define name="MilliAmpereOfAdc(adc)" value="((adc) - 505) * 124.0f"/>
+
+    <define name="LOW_BAT_LEVEL" value="10.5" unit="V"/>
+    <define name="CRITIC_BAT_LEVEL" value="10" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.1" unit="V"/>
+  </section>
+
+  <section name="MISC">
+    <define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
+    <define name="CARROT" value="5." unit="s"/>
+    <define name="CONTROL_RATE" value="60" unit="Hz"/>
+    <define name="XBEE_INIT" value="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
+<!--    <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
+    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+
+    <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
+
+    <define name="GLIDE_AIRSPEED" value="10"/>
+    <define name="GLIDE_VSPEED" value="3."/>
+    <define name="GLIDE_PITCH" value="45" unit="deg"/>
+  </section>
+
+  <section name="VERTICAL CONTROL" prefix="V_CTL_">
+    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
+    <!-- outer loop proportional gain -->
+    <define name="ALTITUDE_PGAIN" value="-0.03"/>
+    <!-- outer loop saturation -->
+    <define name="ALTITUDE_MAX_CLIMB" value="2."/>
+
+    <!-- auto throttle inner loop -->
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
+    <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
+    <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
+    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
+    <define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
+    <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
+    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
+
+    <define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
+  </section>
+
+  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
+    <define name="COURSE_PGAIN" value="-1.20000004768"/>
+    <define name="COURSE_DGAIN" value="0.3"/>
+    <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
+
+    <define name="ROLL_MAX_SETPOINT" value="0.9" unit="radians"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+
+    <define name="PITCH_PGAIN" value="-12000."/>
+    <define name="PITCH_DGAIN" value="0"/>
+
+    <define name="ELEVATOR_OF_ROLL" value="1000."/>
+
+    <define name="ROLL_ATTITUDE_GAIN" value="-11500"/>
+    <define name="ROLL_RATE_GAIN" value="-600."/>
+  </section>
+
+  <section name="AGGRESSIVE" prefix="AGR_">
+    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="CLIMB_THROTTLE" value="1.00"/><!-- Gaz for Aggressive Climb -->
+    <define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
+    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
+    <define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
+    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
+    <define name="DESCENT_NAV_RATIO" value="1.0"/>
+  </section>
+
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DEFAULT_THROTTLE" value="0.35" unit="%"/>
+    <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
+    <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
+
+    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
+    <define name="KILL_MODE_DISTANCE" value="(MAX_DIST_FROM_HOME*1.5)"/>
+    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
+  </section>
+
+  <section name="DIGITAL_CAMERA" prefix="DC_">
+    <define name="AUTOSHOOT_QUARTERSEC_PERIOD" value="6" unit="quarter_second"/>
+    <define name="AUTOSHOOT_METER_GRID" value="50" unit="meter"/>
+  </section>
+
+
+  <modules>
+    <load name="ins_chimu_spi.xml" >
+<!--			<define name="CHIMU_BIG_ENDIAN" />  -->
+		</load>
+
+  </modules>
+
+  <firmware name="fixedwing">
+    <target name="ap" board="tiny_2.11">
+      <define name="STRONG_WIND"/>
+      <define name="WIND_INFO"/>
+      <define name="WIND_INFO_RET"/>
+      <define name="LOITER_TRIM"/>
+      <define name="ALT_KALMAN"/>
+			<!--      <define name="NB_CHANNELS" value="5" />  -->
+    </target>
+    <target name="sim" board="pc"/>
+
+    <subsystem name="radio_control" type="ppm"/>
+    <subsystem name="gps" type="ublox" />
+
+    <!-- Communication -->
+    <subsystem name="telemetry" type="xbee_api">
+      <configure name="MODEM_BAUD" value="B9600"/>
+    </subsystem>
+
+    <!-- Actuators -->
+    <subsystem name="control"/>
+    <!-- Sensors -->
+    <subsystem name="navigation"/>
+<!--    <subsystem name="gps"    type="ublox"/> -->
+
+    <subsystem name="spi_slave_hs"/>
+
+  </firmware>
+
+</airframe>
diff --git a/conf/airframes/CDW/ChimuLisaFw.xml b/conf/airframes/CDW/ChimuLisaFw.xml
index e05a0eb607..c41fbb30a9 100644
--- a/conf/airframes/CDW/ChimuLisaFw.xml
+++ b/conf/airframes/CDW/ChimuLisaFw.xml
@@ -61,9 +61,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -90,9 +87,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.4"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="12000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -125,8 +122,8 @@
 
   <section name="SIMU">
     <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
   <section name="INS" prefix="INS_">
diff --git a/conf/airframes/CDW/ChimuTinyFw.xml b/conf/airframes/CDW/ChimuTinyFw.xml
index 84c2ac6cfa..75af2eac19 100644
--- a/conf/airframes/CDW/ChimuTinyFw.xml
+++ b/conf/airframes/CDW/ChimuTinyFw.xml
@@ -85,9 +85,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -116,9 +113,9 @@
     <define name="COURSE_DGAIN" value="0.3"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="12000."/>
     <define name="PITCH_DGAIN" value="0"/>
diff --git a/conf/airframes/CDW/ChimuTinyFwSpi.xml b/conf/airframes/CDW/ChimuTinyFwSpi.xml
index 380628161c..fd896e3c2a 100644
--- a/conf/airframes/CDW/ChimuTinyFwSpi.xml
+++ b/conf/airframes/CDW/ChimuTinyFwSpi.xml
@@ -85,9 +85,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -116,9 +113,9 @@
     <define name="COURSE_DGAIN" value="0.3"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.9" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.9" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="12000."/>
     <define name="PITCH_DGAIN" value="0"/>
diff --git a/conf/airframes/CDW/debug_i2c.xml b/conf/airframes/CDW/debug_i2c.xml
index c6a280c7e1..3a279378ab 100644
--- a/conf/airframes/CDW/debug_i2c.xml
+++ b/conf/airframes/CDW/debug_i2c.xml
@@ -61,9 +61,9 @@
     <define name="MAG_Y_SENS" value="4.884119848" integer="16"/>
     <define name="MAG_Z_SENS" value="2.59926404993" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 135.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="135." unit="deg"/>
 
   </section>
 
@@ -93,25 +93,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!--  gaui props
@@ -162,7 +162,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="10." integer="16"/>
-    <define name="UNTILT_ACCEL" value="1"/>
   </section>
 
  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/conf/airframes/ENAC/fixed-wing/firestorm.xml b/conf/airframes/ENAC/fixed-wing/firestorm.xml
index 965ea8fe08..8f1f2e60a9 100644
--- a/conf/airframes/ENAC/fixed-wing/firestorm.xml
+++ b/conf/airframes/ENAC/fixed-wing/firestorm.xml
@@ -177,9 +177,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.805000007153" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.805000007153" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="14255.3193359"/>
     <define name="PITCH_DGAIN" value="1.5"/>
diff --git a/conf/airframes/ENAC/fixed-wing/funjet2.xml b/conf/airframes/ENAC/fixed-wing/funjet2.xml
index ab480f59c0..fc1eea3acf 100644
--- a/conf/airframes/ENAC/fixed-wing/funjet2.xml
+++ b/conf/airframes/ENAC/fixed-wing/funjet2.xml
@@ -171,9 +171,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="9000."/>
     <define name="ROLL_RATE_GAIN" value="3000."/>
diff --git a/conf/airframes/ENAC/fixed-wing/funjet2_nc.xml b/conf/airframes/ENAC/fixed-wing/funjet2_nc.xml
index d509548687..b81515dc1d 100644
--- a/conf/airframes/ENAC/fixed-wing/funjet2_nc.xml
+++ b/conf/airframes/ENAC/fixed-wing/funjet2_nc.xml
@@ -191,9 +191,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="9000."/>
     <define name="ROLL_RATE_GAIN" value="3000."/>
diff --git a/conf/airframes/ENAC/fixed-wing/funjet2_new.xml b/conf/airframes/ENAC/fixed-wing/funjet2_new.xml
index 6d38cf42b7..34c951909d 100644
--- a/conf/airframes/ENAC/fixed-wing/funjet2_new.xml
+++ b/conf/airframes/ENAC/fixed-wing/funjet2_new.xml
@@ -191,9 +191,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.7"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="9000."/>
     <define name="ROLL_RATE_GAIN" value="3000."/>
diff --git a/conf/airframes/ENAC/fixed-wing/funjet3.xml b/conf/airframes/ENAC/fixed-wing/funjet3.xml
index 91b7059898..4474096979 100644
--- a/conf/airframes/ENAC/fixed-wing/funjet3.xml
+++ b/conf/airframes/ENAC/fixed-wing/funjet3.xml
@@ -168,9 +168,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.699999988079"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="6000."/>
     <define name="ROLL_RATE_GAIN" value="1000."/>
@@ -208,8 +208,8 @@
     <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
     <!--define name="JSBSIM_INIT"	value="&quot;Malolo1-IC&quot;"/-->
     <define name="JSBSIM_LAUNCHSPEED"	value="15.0"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
 </airframe>
diff --git a/conf/airframes/ENAC/fixed-wing/merlin.xml b/conf/airframes/ENAC/fixed-wing/merlin.xml
index 3d24b90a02..42b93e31db 100644
--- a/conf/airframes/ENAC/fixed-wing/merlin.xml
+++ b/conf/airframes/ENAC/fixed-wing/merlin.xml
@@ -152,9 +152,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="5900"/>
     <define name="ROLL_RATE_GAIN" value="2900"/>
diff --git a/conf/airframes/ENAC/fixed-wing/minimag1.xml b/conf/airframes/ENAC/fixed-wing/minimag1.xml
index 3cc8ff06d3..a416db1dc2 100644
--- a/conf/airframes/ENAC/fixed-wing/minimag1.xml
+++ b/conf/airframes/ENAC/fixed-wing/minimag1.xml
@@ -157,9 +157,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.2"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="12000."/>
diff --git a/conf/airframes/ENAC/fixed-wing/obsolete/drops.xml b/conf/airframes/ENAC/fixed-wing/obsolete/drops.xml
index f25bebc0c8..558226830c 100644
--- a/conf/airframes/ENAC/fixed-wing/obsolete/drops.xml
+++ b/conf/airframes/ENAC/fixed-wing/obsolete/drops.xml
@@ -159,9 +159,9 @@ on<!DOCTYPE airframe SYSTEM "airframe.dtd">
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0340000391"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -229,11 +229,11 @@ ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_AUTO1
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
@@ -264,7 +264,7 @@ ap.srcs += subsystems/navigation/snav.c
 # Config for SITL simulation
 # include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.ARCHDIR = $(ARCHI)
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
diff --git a/conf/airframes/ENAC/fixed-wing/obsolete/funjet1.xml b/conf/airframes/ENAC/fixed-wing/obsolete/funjet1.xml
index cb29af14b9..8d1e2ca662 100644
--- a/conf/airframes/ENAC/fixed-wing/obsolete/funjet1.xml
+++ b/conf/airframes/ENAC/fixed-wing/obsolete/funjet1.xml
@@ -144,9 +144,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.20599997044"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.805000007153" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.805000007153" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="14814.8154297"/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -233,11 +233,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
@@ -288,7 +288,7 @@ ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150
 #include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.ARCHDIR = $(ARCHI)
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 
@@ -307,7 +307,7 @@ sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_re
 setup_actuators.ARCHDIR = $(ARCHI)
 
 setup_actuators.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny.h\" -DUSE_LED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART1 -DUART1_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart1
-setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4015_hw.c main.c
+setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c subsystems/datalink/pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4015_hw.c main.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/ENAC/fixed-wing/obsolete/malolo_sim.xml b/conf/airframes/ENAC/fixed-wing/obsolete/malolo_sim.xml
index 4484dc92dc..daddc4fc3a 100644
--- a/conf/airframes/ENAC/fixed-wing/obsolete/malolo_sim.xml
+++ b/conf/airframes/ENAC/fixed-wing/obsolete/malolo_sim.xml
@@ -141,9 +141,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="50000"/>
     <define name="PITCH_DGAIN" value="0"/>
@@ -197,8 +197,8 @@
   <section name="SIMU">
     <!--define name="JSBSIM_MODEL" value="&quot;fulmar&quot;"/-->
     <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
   <makefile>
diff --git a/conf/airframes/ENAC/fixed-wing/obsolete/slayer2.xml b/conf/airframes/ENAC/fixed-wing/obsolete/slayer2.xml
index 5b545d2463..454e24bb65 100644
--- a/conf/airframes/ENAC/fixed-wing/obsolete/slayer2.xml
+++ b/conf/airframes/ENAC/fixed-wing/obsolete/slayer2.xml
@@ -107,9 +107,6 @@
     <define name="UNLOCKED_HOME_MODE" value="TRUE"/>
     <define name="RC_LOST_MODE" value="PPRZ_MODE_AUTO2"/>
 
-    <define name="GLIDE_AIRSPEED" value="12.5"/>
-    <define name="GLIDE_VSPEED" value="2."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
 
     <define name="TELEMETRY_MODE_AP" value="1"/>
   </section>
@@ -146,9 +143,9 @@
     <define name="COURSE_PGAIN" value="2.6"/>
     <define name="COURSE_DGAIN" value="1"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="9000."/>
     <define name="PITCH_DGAIN" value="0."/>
@@ -206,7 +203,7 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 # AEROCOMM
 # ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
@@ -214,7 +211,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 # RADIOTRONIX
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
 
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
@@ -265,7 +262,7 @@ ap.CFLAGS += -D BARO_MS5534A_W1=0xAC20 -D BARO_MS5534A_W2=0x87D9 -D BARO_MS5534A
 #include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.ARCHDIR = $(ARCHI)
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN  -DUSE_BARO_MS5534A
 
diff --git a/conf/airframes/ENAC/fixed-wing/obsolete/solarstorm.xml b/conf/airframes/ENAC/fixed-wing/obsolete/solarstorm.xml
index 84f58261d1..cf30dd8658 100644
--- a/conf/airframes/ENAC/fixed-wing/obsolete/solarstorm.xml
+++ b/conf/airframes/ENAC/fixed-wing/obsolete/solarstorm.xml
@@ -146,9 +146,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.885999977589"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="14255.3193359"/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -218,7 +218,7 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
@@ -254,7 +254,7 @@ ap.CFLAGS += -DUSE_MODULES
 #include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.ARCHDIR = $(ARCHI)
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 
diff --git a/conf/airframes/ENAC/fixed-wing/obsolete/spocII.xml b/conf/airframes/ENAC/fixed-wing/obsolete/spocII.xml
index 1c96f6c340..a36cb2fa2f 100644
--- a/conf/airframes/ENAC/fixed-wing/obsolete/spocII.xml
+++ b/conf/airframes/ENAC/fixed-wing/obsolete/spocII.xml
@@ -148,9 +148,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.20599997044"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.805000007153" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.805000007153" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="7627.11914062"/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -201,8 +201,8 @@
 
   <section name="SIMU">
     <define name="JSBSIM_MODEL" value="&quot;Corsica&quot;"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
  <makefile>
@@ -225,7 +225,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #TRANSPARENT
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
@@ -254,7 +254,7 @@ ap.CFLAGS += -DUSE_ADC_3 -DADC_CHANNEL_CURRENT=ADC_3
 #include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.ARCHDIR = $(ARCHI)
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 
@@ -275,7 +275,7 @@ jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
 setup_actuators.ARCHDIR = $(ARCHI)
 
 setup_actuators.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny.h\" -DUSE_LED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART1 -DUART1_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart1
-setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4015_hw.c main.c
+setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c subsystems/datalink/pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4015_hw.c main.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/ENAC/fixed-wing/overview.xml b/conf/airframes/ENAC/fixed-wing/overview.xml
index cd9d05c471..d98916856e 100644
--- a/conf/airframes/ENAC/fixed-wing/overview.xml
+++ b/conf/airframes/ENAC/fixed-wing/overview.xml
@@ -96,10 +96,10 @@
   </section>
 
   <section name="CAM" prefix="CAM_">
-    <define name="TILT_MAX" value="45" unit="deg"/>
-    <define name="TILT_NEUTRAL" value="0" unit="deg"/>
-    <define name="TILT_MIN" value="-45" unit="deg"/>
-    <define name="TILT0" value="0" unit="deg"/>
+    <define name="TILT_MAX" value="45" unit="deg" code_unit="deg"/>
+    <define name="TILT_NEUTRAL" value="0" unit="deg" code_unit="deg"/>
+    <define name="TILT_MIN" value="-45" unit="deg" code_unit="deg"/>
+    <define name="TILT0" value="0" unit="deg" code_unit="deg"/>
   </section>
 
   <section name="MISC">
@@ -146,9 +146,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="9000."/>
     <define name="ROLL_RATE_GAIN" value="3000."/>
diff --git a/conf/airframes/ENAC/fixed-wing/soarzi.xml b/conf/airframes/ENAC/fixed-wing/soarzi.xml
index 2472ada786..a521dce1d5 100644
--- a/conf/airframes/ENAC/fixed-wing/soarzi.xml
+++ b/conf/airframes/ENAC/fixed-wing/soarzi.xml
@@ -137,9 +137,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.2"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="12000."/>
@@ -191,10 +191,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #XBEE ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#XBEE ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#XBEE ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-1.xml b/conf/airframes/ENAC/fixed-wing/spocIII-1.xml
index fb3242b796..5feea6c8a7 100644
--- a/conf/airframes/ENAC/fixed-wing/spocIII-1.xml
+++ b/conf/airframes/ENAC/fixed-wing/spocIII-1.xml
@@ -149,9 +149,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.20599997044"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.805000007153" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.805000007153" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="8200"/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -181,8 +181,8 @@
 
   <section name="SIMU">
     <define name="JSBSIM_MODEL" value="&quot;Corsica&quot;"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
 </airframe>
diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml
index d6708e947e..872c679760 100644
--- a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml
+++ b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml
@@ -176,9 +176,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.20599997044"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="8500."/>
     <define name="ROLL_RATE_GAIN" value="2400."/>
@@ -212,8 +212,8 @@
 
   <section name="SIMU">
     <define name="JSBSIM_MODEL" value="&quot;Corsica&quot;"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
 </airframe>
diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-3.xml b/conf/airframes/ENAC/fixed-wing/spocIII-3.xml
index 92497748cf..61c1e445c5 100644
--- a/conf/airframes/ENAC/fixed-wing/spocIII-3.xml
+++ b/conf/airframes/ENAC/fixed-wing/spocIII-3.xml
@@ -179,9 +179,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.20599997044"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="8500."/>
     <define name="ROLL_RATE_GAIN" value="2400."/>
@@ -215,8 +215,8 @@
 
   <section name="SIMU">
     <define name="JSBSIM_MODEL" value="&quot;Corsica&quot;"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
 </airframe>
diff --git a/conf/airframes/ENAC/fixed-wing/twinjet2.xml b/conf/airframes/ENAC/fixed-wing/twinjet2.xml
index a800fe0ee0..c3b2f37d7a 100644
--- a/conf/airframes/ENAC/fixed-wing/twinjet2.xml
+++ b/conf/airframes/ENAC/fixed-wing/twinjet2.xml
@@ -166,9 +166,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.995000004768"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="3553.89208984"/>
     <define name="ROLL_RATE_GAIN" value="0."/>
@@ -242,11 +242,11 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ########## Modems
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ########## ADC
 ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3
@@ -299,7 +299,7 @@ ap.CFLAGS += -DUSE_MODULES
 
 sim.ARCHDIR = $(ARCHI)
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl_n.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl_n.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DPITCH_TRIM -DALT_KALMAN
 
diff --git a/conf/airframes/ENAC/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml
index cc4d9650e7..6fc2dbab9d 100644
--- a/conf/airframes/ENAC/fixed-wing/weasel.xml
+++ b/conf/airframes/ENAC/fixed-wing/weasel.xml
@@ -192,9 +192,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.7"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.8" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="3800."/>
     <define name="ROLL_RATE_GAIN" value="500."/>
@@ -232,8 +232,8 @@
     <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
     <!--define name="JSBSIM_INIT"	value="&quot;Malolo1-IC&quot;"/-->
     <define name="JSBSIM_LAUNCHSPEED"	value="15.0"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
 </airframe>
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index 1d94fee657..44e4371d67 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -12,13 +12,12 @@
   <firmware name="rotorcraft">
     <define name="USE_INS_NAV_INIT"/>
     <define name="USE_ADAPT_HOVER"/>
-    <define name="NO_FUCKING_STARTUP_DELAY"/>
     <!--define name="GUIDANCE_H_USE_REF"/-->
 
     <target name="ap" board="booz_1.0">
       <define name="FAILSAFE_GROUND_DETECT"/>
       <define name="USE_GPS_ACC4R"/>
-      <define name="BOOZ_START_DELAY" value="3"/>
+      <define name="ACTUATORS_START_DELAY" value="3"/>
     </target>
     <target name="sim" board="pc">
       <subsystem name="fdm"         type="nps"/>
@@ -130,10 +129,10 @@
     <define name="MAG_Y_SENS" value="5.43740121157" integer="16"/>
     <define name="MAG_Z_SENS" value="2.60492836649" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-    <!--define name="BODY_TO_IMU_PSI"   value="RadOfDeg(45.)"/-->
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <!--define name="BODY_TO_IMU_PSI"   value="45." unit="deg"/-->
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -152,25 +151,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(90.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
 
     <!-- feedback -->
@@ -246,8 +245,8 @@
     <define name="TILT_NEUTRAL" value="1500"/>
     <define name="TILT_MIN" value="1060"/>
     <define name="TILT_MAX" value="2250"/>
-    <define name="TILT_ANGLE_MIN" value="RadOfDeg(-90.)" unit="rad"/>
-    <define name="TILT_ANGLE_MAX" value="RadOfDeg(  0.)" unit="rad"/>
+    <define name="TILT_ANGLE_MIN" value="-90." unit="deg"/>
+    <define name="TILT_ANGLE_MAX" value="  0." unit="deg"/>
     <define name="PAN_NEUTRAL" value="0"/>
     <define name="PAN_MIN" value="0"/>
     <define name="PAN_MAX" value="25736"/> <!-- 360 deg (2^12) -->
@@ -261,7 +260,7 @@
    <define name="FACE_REINJ_1"  value="1024"/>
    <define name="DEFAULT_CIRCLE_RADIUS" value="10."/>
    <define name="BoozDropPwm(_v)" value="BoozSetPwm0Value(_v)"/>
-   <define name="IMU_MAG_OFFSET" value="-9."/>
+   <define name="IMU_MAG_OFFSET" value="RadOfDeg(-9.)"/>
  </section>
 
   <section name="GCS">
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 8d97ace3df..2c55323d36 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -16,13 +16,12 @@
   <firmware name="rotorcraft">
     <define name="USE_INS_NAV_INIT"/>
     <define name="USE_ADAPT_HOVER"/>
-    <define name="NO_FUCKING_STARTUP_DELAY"/>
     <!--define name="GUIDANCE_H_USE_REF"/-->
 
     <target name="ap" board="booz_1.0">
       <define name="FAILSAFE_GROUND_DETECT"/>
       <define name="USE_GPS_ACC4R"/>
-      <define name="BOOZ_START_DELAY" value="3"/>
+      <define name="ACTUATORS_START_DELAY" value="3"/>
     </target>
     <target name="sim" board="pc">
       <subsystem name="fdm"         type="nps"/>
@@ -94,9 +93,9 @@
     <define name="MAG_Y_SENS" value="21.79885" integer="16"/>
     <define name="MAG_Z_SENS" value="14.675745" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(1.3)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(-2.6)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="1.3" unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="-2.6" unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -115,25 +114,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(90.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
 
    <!-- feedback -->
@@ -158,7 +157,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="15." integer="16"/>
-    <define name="UNTILT_ACCEL" value="1"/>
     <define name="SONAR_SENS" value="2.146" integer="16"/>
   </section>
 
@@ -212,8 +210,8 @@
     <define name="TILT_NEUTRAL" value="1500"/>
     <define name="TILT_MAX" value="1000"/>
     <define name="TILT_MIN" value="2300"/>
-    <define name="TILT_ANGLE_MIN" value="RadOfDeg(-90.)" unit="rad"/>
-    <define name="TILT_ANGLE_MAX" value="RadOfDeg( 10.)" unit="rad"/>
+    <define name="TILT_ANGLE_MIN" value="-90." unit="deg"/>
+    <define name="TILT_ANGLE_MAX" value=" 10." unit="deg"/>
     <define name="PAN_NEUTRAL" value="0"/>
     <define name="PAN_MIN" value="0"/>
     <define name="PAN_MAX" value="25736"/> <!-- 360 deg (2^12) -->
@@ -230,7 +228,7 @@
     <define name="BOOZ_ANALOG_BARO_THRESHOLD" value="800"/>
     <define name="FACE_REINJ_1"  value="1024"/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="10."/>
-    <!--define name="IMU_MAG_OFFSET" value="-5.8"/-->
+    <!--define name="IMU_MAG_OFFSET" value="RadOfDeg(-5.8)"/-->
   </section>
 
   <section name="GCS">
diff --git a/conf/airframes/ENAC/quadrotor/g1_vision.xml b/conf/airframes/ENAC/quadrotor/g1_vision.xml
index 0892e2eaf0..1a9fd10b02 100644
--- a/conf/airframes/ENAC/quadrotor/g1_vision.xml
+++ b/conf/airframes/ENAC/quadrotor/g1_vision.xml
@@ -67,9 +67,9 @@
     <define name="MAG_Y_SENS" value="21.79885" integer="16"/>
     <define name="MAG_Z_SENS" value="14.675745" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(1.3)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(-2.6)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="1.3" unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="-2.6" unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -88,25 +88,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(90.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
 
    <!-- feedback -->
@@ -131,7 +131,6 @@
 
   <section name="INS" prefix="BOOZ_INS_">
     <define name="BARO_SENS" value="15." integer="16"/>
-    <define name="UNTILT_ACCEL" value="1"/>
     <define name="SONAR_SENS" value="2.146" integer="16"/>
   </section>
 
@@ -231,8 +230,8 @@ include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
 #include $(CFG_BOOZ)/subsystems/booz2_ins_hff.makefile
 #ap.CFLAGS += -DR_POS=0.1
 
-ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
-sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
+ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
+sim.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
 
 ap.CFLAGS += -DUSE_MODULES
 
diff --git a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml b/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
index a9095dd873..653ae0fafe 100644
--- a/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
+++ b/conf/airframes/ENAC/quadrotor/mkk1-vision.xml
@@ -87,9 +87,9 @@
     <define name="MAG_Y_SENS" value="-21.79885" integer="16"/>
     <define name="MAG_Z_SENS" value="-14.675745" integer="16"/-->
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -121,25 +121,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="BOOZ_STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.9"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.9"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.9"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(90.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
 
     <!-- feedback -->
@@ -164,7 +164,6 @@
 
   <section name="INS" prefix="BOOZ_INS_">
     <define name="BARO_SENS" value="15." integer="16"/>
-    <define name="UNTILT_ACCEL" value="1"/>
     <!--define name="SONAR_SENS" value="2.146" integer="16"/-->
     <define name="SONAR_SENS" value="3." integer="16"/>
   </section>
diff --git a/conf/airframes/ENAC/quadrotor/mkk1.xml b/conf/airframes/ENAC/quadrotor/mkk1.xml
index 4536233f45..d9c7be6a38 100644
--- a/conf/airframes/ENAC/quadrotor/mkk1.xml
+++ b/conf/airframes/ENAC/quadrotor/mkk1.xml
@@ -117,9 +117,9 @@
     <define name="MAG_Y_SENS" value="-21.79885" integer="16"/>
     <define name="MAG_Z_SENS" value="-14.675745" integer="16"/-->
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -151,25 +151,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.9"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.9"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.9"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(90.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
 
     <!-- feedback -->
@@ -194,7 +194,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="15." integer="16"/>
-    <define name="UNTILT_ACCEL" value="1"/>
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/conf/airframes/ENAC/quadrotor/navgo.xml b/conf/airframes/ENAC/quadrotor/navgo.xml
index 95552ba9b7..5b898ba78f 100644
--- a/conf/airframes/ENAC/quadrotor/navgo.xml
+++ b/conf/airframes/ENAC/quadrotor/navgo.xml
@@ -11,7 +11,6 @@
   <firmware name="rotorcraft">
     <define name="USE_INS_NAV_INIT"/>
     <define name="USE_ADAPT_HOVER"/>
-    <define name="NO_FUCKING_STARTUP_DELAY"/>
     <define name="USE_ATTITUDE_REF" value="0"/>
     <!--define name="GUIDANCE_H_USE_REF"/-->
 
@@ -99,9 +98,9 @@
     <define name="MAG_Y_SENS" value="4.8961742578" integer="16"/>
     <define name="MAG_Z_SENS" value="5.31527656902" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/> <!-- -10 -->
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/> <!-- -10 -->
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/> <!-- -10 -->
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/> <!-- -10 -->
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -133,9 +132,9 @@
     <define name="MAG_Y_SENS" value="4.49687342915" integer="16"/>
     <define name="MAG_Z_SENS" value="4.93138019221" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(-10.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(-10.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="-10." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="-10." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -168,25 +167,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(90.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
 
     <!-- feedback -->
@@ -257,8 +256,8 @@
     <define name="TILT_NEUTRAL" value="1500"/>
     <define name="TILT_MIN" value="1060"/>
     <define name="TILT_MAX" value="2250"/>
-    <define name="TILT_ANGLE_MIN" value="RadOfDeg(-90.)" unit="rad"/>
-    <define name="TILT_ANGLE_MAX" value="RadOfDeg(  0.)" unit="rad"/>
+    <define name="TILT_ANGLE_MIN" value="-90." unit="deg"/>
+    <define name="TILT_ANGLE_MAX" value="  0." unit="deg"/>
     <define name="PAN_NEUTRAL" value="0"/>
     <define name="PAN_MIN" value="0"/>
     <define name="PAN_MAX" value="25736"/> <!-- 360 deg (2^12) -->
@@ -272,7 +271,7 @@
    <define name="FACE_REINJ_1"  value="1024"/>
    <define name="DEFAULT_CIRCLE_RADIUS" value="10."/>
    <define name="BoozDropPwm(_v)" value="BoozSetPwm1Value(_v)"/>
-   <define name="IMU_MAG_OFFSET" value="-9."/>
+   <define name="IMU_MAG_OFFSET" value="RadOfDeg(-9.)"/>
  </section>
 
   <section name="GCS">
diff --git a/conf/airframes/ENAC/quadrotor/nova1.xml b/conf/airframes/ENAC/quadrotor/nova1.xml
index dafdff39a8..2e1e0f2447 100644
--- a/conf/airframes/ENAC/quadrotor/nova1.xml
+++ b/conf/airframes/ENAC/quadrotor/nova1.xml
@@ -88,9 +88,9 @@
     <define name="MAG_Y_SENS" value="6.95371185551" integer="16"/>
     <define name="MAG_Z_SENS" value="3.12322557454" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(-135.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-135." unit="deg"/>
 
   </section>
 
@@ -122,25 +122,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.9"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.9"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.9"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(90.)"/>
+    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
 
     <!-- feedback -->
@@ -165,7 +165,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="15." integer="16"/>
-    <define name="UNTILT_ACCEL" value="1"/>
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/conf/airframes/LAAS/mmlaas_N1.xml b/conf/airframes/LAAS/mmlaas_N1.xml
index c4571f9e9e..d56a35d931 100644
--- a/conf/airframes/LAAS/mmlaas_N1.xml
+++ b/conf/airframes/LAAS/mmlaas_N1.xml
@@ -167,9 +167,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_ATTITUDE_GAIN" value="10000"/>
diff --git a/conf/airframes/LAAS/mmlaas_N2.xml b/conf/airframes/LAAS/mmlaas_N2.xml
index 2e74bf7049..d5f35f384d 100644
--- a/conf/airframes/LAAS/mmlaas_N2.xml
+++ b/conf/airframes/LAAS/mmlaas_N2.xml
@@ -163,9 +163,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_ATTITUDE_GAIN" value="10000"/>
diff --git a/conf/airframes/LAAS/mmlaas_N3.xml b/conf/airframes/LAAS/mmlaas_N3.xml
index 8260d269b7..8eb4d5dde1 100644
--- a/conf/airframes/LAAS/mmlaas_N3.xml
+++ b/conf/airframes/LAAS/mmlaas_N3.xml
@@ -163,9 +163,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_ATTITUDE_GAIN" value="10000"/>
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml
index dd5169359a..e54555d794 100644
--- a/conf/airframes/NoVa_L.xml
+++ b/conf/airframes/NoVa_L.xml
@@ -81,9 +81,9 @@
    <define name="MAG_Y_CHAN" value="0"/>
     <define name="MAG_Z_CHAN" value="2"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(  4.5)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  6.8)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  -45.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="4.5" unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="6.8" unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-45." unit="deg"/>
 
     <define name="GYRO_P_CHAN" value="0"/>
     <define name="GYRO_Q_CHAN" value="4"/>
@@ -121,25 +121,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(180.)"/><!-- Maximum speed in deg/s-->
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="180." unit="deg/s"/><!-- Maximum speed in deg/s-->
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!--  gaui props
@@ -190,7 +190,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="10." integer="16"/>
-    <define name="UNTILT_ACCEL" value="1"/>
   </section>
 
  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
index 4742474e8b..602b5dd20d 100644
--- a/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+++ b/conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
@@ -122,9 +122,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -153,9 +150,9 @@
     <define name="COURSE_DGAIN" value="0.3"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="12000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
index ad12551489..039210f933 100644
--- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
+++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_asctec_example.xml
@@ -85,25 +85,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback gains for better endurance -->
diff --git a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
index 9b805c686d..9dcfcbfb62 100644
--- a/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
+++ b/conf/airframes/PPZUAV/quadrotor/ppzuav_booz2_mkk_example.xml
@@ -86,25 +86,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback gains for better endurance -->
diff --git a/conf/airframes/Poine/antenna.xml b/conf/airframes/Poine/antenna.xml
index 40ee8725ba..8ac39889a6 100644
--- a/conf/airframes/Poine/antenna.xml
+++ b/conf/airframes/Poine/antenna.xml
@@ -29,7 +29,7 @@ ant.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 ant.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 ant.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-ant.srcs += downlink.c pprz_transport.c
+ant.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 ant.CFLAGS += -DDOWNLINK_AP_DEVICE=Uart0 -DAP
 ant.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
diff --git a/conf/airframes/Poine/beth.xml b/conf/airframes/Poine/beth.xml
index 38269d5b50..0ab3152413 100644
--- a/conf/airframes/Poine/beth.xml
+++ b/conf/airframes/Poine/beth.xml
@@ -67,9 +67,9 @@
     <define name="MAG_Z_SENS" value="-3.11986612709" integer="16"/>
 
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45." unit="deg"/>
 
 <!--  3  -->
     <define name="GYRO_P_CHAN" value="0"/>
@@ -116,7 +116,7 @@ main_stm32.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 main_stm32.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
 main_stm32.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 main_stm32.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-main_stm32.srcs += downlink.c pprz_transport.c
+main_stm32.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 
 main_stm32.CFLAGS += -DUSE_OVERO_LINK
@@ -136,7 +136,7 @@ main_stm32.srcs += lisa/lisa_overo_link.c lisa/arch/stm32/lisa_overo_link_arch.c
 
 #crista IMU
 main_stm32.srcs += math/pprz_trig_int.c
-main_stm32.CFLAGS += -DIMU_TYPE_H=\"imu/imu_crista.h\" -DIMU_OVERRIDE_CHANNELS
+main_stm32.CFLAGS += -DIMU_TYPE_H=\"imu/imu_crista.h\"
 main_stm32.srcs += subsystems/imu.c             \
                         subsystems/imu/imu_crista.c \
                         arch/$(ARCH)/subsystems/imu/imu_crista_arch.c
@@ -147,7 +147,7 @@ main_stm32.srcs += firmwares/rotorcraft/actuators/actuators_asctec.c
 #\
 #                             $(SRC_BOOZ_ARCH)/actuators/actuators_asctec_arch.c
 main_stm32.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
-# -DBOOZ_START_DELAY=3  -DUSE_TIM2_IRQ
+# -DACTUATORS_START_DELAY=3  -DUSE_TIM2_IRQ
 main_stm32.CFLAGS += -DUSE_I2C1
 main_stm32.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
 
@@ -188,7 +188,7 @@ main_coders.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 main_coders.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
 main_coders.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 main_coders.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-main_coders.srcs += downlink.c pprz_transport.c
+main_coders.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 #main_coders.CFLAGS += -DUSE_I2C2
 #main_coders.srcs += $(SRC_ARCH)/i2c_hw.c
@@ -229,7 +229,7 @@ main_overo.srcs += $(SRC_FMS)/fms_spi_link.c
 main_overo.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
 
 main_overo.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-main_overo.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
+main_overo.srcs    += $(SRC_FMS)/udp_transport2.c subsystems/datalink/downlink.c
 main_overo.srcs    += $(SRC_FMS)/fms_network.c
 main_overo.LDFLAGS += -levent -lm
 
@@ -252,7 +252,7 @@ overo_test_uart.srcs    += $(SRC_FMS)/fms_periodic.c
 overo_test_uart.srcs    += $(SRC_FMS)/fms_serial_port.c
 overo_test_uart.LDFLAGS += -lrt
 overo_test_uart.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_test_uart.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
+overo_test_uart.srcs    += $(SRC_FMS)/udp_transport2.c subsystems/datalink/downlink.c
 overo_test_uart.srcs    += $(SRC_FMS)/fms_network.c
 overo_test_uart.LDFLAGS += -levent -lm
 overo_test_uart.srcs    += $(SRC_BETH)/overo_gcs_com.c
@@ -261,7 +261,7 @@ overo_test_uart.srcs    += $(SRC_BETH)/overo_gcs_com.c
 overo_test_uart.CFLAGS  += -DUBX -DUSE_GPS -DUSE_UART0 -DUART0_BAUD=B38400 -DGPS_LINK=Uart0 -DGPS_USE_LATLONG
 overo_test_uart.srcs    += gps_ubx.c gps.c latlong.c $(SRC_BETH)/uart_hw.c
 overo_test_uart.CFLAGS  +=-DUSE_UART1 -DUART1_BAUD=B57600 -DDOWNLINK_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ
-overo_test_uart.srcs    += pprz_transport.c $(SRC_BETH)/rcv_telemetry.c
+overo_test_uart.srcs    += subsystems/datalink/pprz_transport.c $(SRC_BETH)/rcv_telemetry.c
 
 #
 # Overo twisting
@@ -278,7 +278,7 @@ overo_twist.srcs += $(SRC_FMS)/fms_spi_link.c
 overo_twist.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
 
 overo_twist.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_twist.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
+overo_twist.srcs    += $(SRC_FMS)/udp_transport2.c subsystems/datalink/downlink.c
 overo_twist.srcs    += $(SRC_FMS)/fms_network.c
 overo_twist.LDFLAGS += -levent -lm
 
@@ -304,7 +304,7 @@ overo_sfb.srcs += $(SRC_FMS)/fms_spi_link.c
 overo_sfb.CFLAGS += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
 
 overo_sfb.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_sfb.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
+overo_sfb.srcs    += $(SRC_FMS)/udp_transport2.c subsystems/datalink/downlink.c
 overo_sfb.srcs    += $(SRC_FMS)/fms_network.c
 overo_sfb.LDFLAGS += -levent
 overo_sfb.LDFLAGS += -lm
diff --git a/conf/airframes/Poine/bl_mc.xml b/conf/airframes/Poine/bl_mc.xml
index 7e93514d9d..53b280f7ba 100644
--- a/conf/airframes/Poine/bl_mc.xml
+++ b/conf/airframes/Poine/bl_mc.xml
@@ -33,7 +33,7 @@ main.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
 main.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-main.srcs += downlink.c pprz_transport.c
+main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 main.srcs += $(SRC_ARCH)/bl_mc_power.c
 
diff --git a/conf/airframes/Poine/booz2_a1.xml b/conf/airframes/Poine/booz2_a1.xml
index 9389fc0821..994704b005 100644
--- a/conf/airframes/Poine/booz2_a1.xml
+++ b/conf/airframes/Poine/booz2_a1.xml
@@ -73,13 +73,13 @@
     <define name="MAG_X_SENS" value=" 3.4936416" integer="16"/>
     <define name="MAG_Y_SENS" value=" 3.607713"  integer="16"/>
 
-   <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(4.)"/>
-   <define name="BODY_TO_IMU_THETA" value="RadOfDeg(3.)"/>
-   <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+   <define name="BODY_TO_IMU_PHI"   value="4." unit="deg"/>
+   <define name="BODY_TO_IMU_THETA" value="3." unit="deg"/>
+   <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 <!--
-   <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-   <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-   <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+   <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+   <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+   <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 -->
 
   </section>
@@ -100,25 +100,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.9"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.9"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.9"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
diff --git a/conf/airframes/Poine/booz2_a1p.xml b/conf/airframes/Poine/booz2_a1p.xml
index 6dc5afda02..b400829f6e 100644
--- a/conf/airframes/Poine/booz2_a1p.xml
+++ b/conf/airframes/Poine/booz2_a1p.xml
@@ -76,9 +76,9 @@
 <!--    <define name="MAG_X_SENS" value="-3.4936416" integer="16"/> -->
 <!--    <define name="MAG_Y_SENS" value=" 3.607713"  integer="16"/> -->
 
-   <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(4.)"/>
-   <define name="BODY_TO_IMU_THETA" value="RadOfDeg(3.)"/>
-   <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+   <define name="BODY_TO_IMU_PHI"   value="4." unit="deg"/>
+   <define name="BODY_TO_IMU_THETA" value="3." unit="deg"/>
+   <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -98,25 +98,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.9"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.9"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.9"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
diff --git a/conf/airframes/Poine/booz2_a7.xml b/conf/airframes/Poine/booz2_a7.xml
index e5729293e7..5aa9a6707c 100644
--- a/conf/airframes/Poine/booz2_a7.xml
+++ b/conf/airframes/Poine/booz2_a7.xml
@@ -61,9 +61,9 @@
     <define name="MAG_Y_SENS" value="4.884119848" integer="16"/>
     <define name="MAG_Z_SENS" value="2.59926404993" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 135.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="135." unit="deg"/>
 
   </section>
 
@@ -93,25 +93,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!--  gaui props
@@ -162,7 +162,6 @@
 
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="10." integer="16"/>
-    <define name="UNTILT_ACCEL" value="1"/>
   </section>
 
  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/conf/airframes/Poine/booz2_a8.xml b/conf/airframes/Poine/booz2_a8.xml
index c98fbb24cb..088b1421f1 100644
--- a/conf/airframes/Poine/booz2_a8.xml
+++ b/conf/airframes/Poine/booz2_a8.xml
@@ -74,9 +74,9 @@
     <define name="MAG_Z_SENS" value="-3.11986612709" integer="16"/>
 
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45." unit="deg"/>
 
     <define name="GYRO_P_CHAN" value="0"/>
     <define name="GYRO_Q_CHAN" value="4"/>
@@ -118,9 +118,9 @@
     <define name="MAG_Y_NEUTRAL" value=" 49"/>
     <define name="MAG_Z_NEUTRAL" value="-66"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45." unit="deg"/>
 
   </section>
 -->
@@ -167,9 +167,9 @@
     <define name="MAG_Z_SENS" value="-3.11986612709" integer="16"/>
 
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 45.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45." unit="deg"/>
 
     <define name="GYRO_P_CHAN" value="0"/>
     <define name="GYRO_Q_CHAN" value="4"/>
diff --git a/conf/airframes/Poine/easy_glider1.xml b/conf/airframes/Poine/easy_glider1.xml
index 224518ed69..17eb552927 100644
--- a/conf/airframes/Poine/easy_glider1.xml
+++ b/conf/airframes/Poine/easy_glider1.xml
@@ -88,9 +88,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -118,9 +115,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.10800004005"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="15184.5644531"/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -176,7 +173,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # transparent
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/Poine/funjet42.xml b/conf/airframes/Poine/funjet42.xml
index 7647c1e3f8..fc85b71511 100644
--- a/conf/airframes/Poine/funjet42.xml
+++ b/conf/airframes/Poine/funjet42.xml
@@ -80,8 +80,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="35" unit="deg"/>
   </section>
 
   <section name="INFRARED" prefix="IR_">
@@ -142,9 +142,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/Poine/h_hex.xml b/conf/airframes/Poine/h_hex.xml
index f747e97c59..53b3b285b6 100644
--- a/conf/airframes/Poine/h_hex.xml
+++ b/conf/airframes/Poine/h_hex.xml
@@ -63,9 +63,9 @@
     <define name="MAG_Y_NEUTRAL" value=" 49"/>
     <define name="MAG_Z_NEUTRAL" value="-66"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 90.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="90." unit="deg"/>
 
   </section>
 
@@ -95,25 +95,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
@@ -170,7 +170,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_l_1.0">
-<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+<!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
       <subsystem name="actuators"     type="mkk"/>
     </target>
@@ -181,7 +181,7 @@
     </target>
     <subsystem name="telemetry"     type="transparent"/>
 <!--    <subsystem name="imu"           type="b2_v1.1"/> -->
-    <subsystem name="imu"           type="aspirin"/>
+    <subsystem name="imu"           type="aspirin_v1.0"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="int_cmpl_euler"/>
diff --git a/conf/airframes/Poine/motor_bench.xml b/conf/airframes/Poine/motor_bench.xml
index 741c80dd4a..6bb5865631 100644
--- a/conf/airframes/Poine/motor_bench.xml
+++ b/conf/airframes/Poine/motor_bench.xml
@@ -25,7 +25,7 @@ main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B115200
 main.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-main.srcs += downlink.c pprz_transport.c
+main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 main.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 
diff --git a/conf/airframes/Poine/pt_ant.xml b/conf/airframes/Poine/pt_ant.xml
index c73a262c56..c626c69d52 100644
--- a/conf/airframes/Poine/pt_ant.xml
+++ b/conf/airframes/Poine/pt_ant.xml
@@ -34,7 +34,7 @@ main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 main.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-main.srcs += downlink.c pprz_transport.c $(PT_ANT)/pt_ant_telemetry.c
+main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c $(PT_ANT)/pt_ant_telemetry.c
 
 main.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 main.srcs += $(PT_ANT)/pt_ant_datalink.c
diff --git a/conf/airframes/Poine/swift_1.xml b/conf/airframes/Poine/swift_1.xml
index f38ba28349..61058b22b7 100644
--- a/conf/airframes/Poine/swift_1.xml
+++ b/conf/airframes/Poine/swift_1.xml
@@ -74,8 +74,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="35" unit="deg"/>
   </section>
 
   <section name="INFRARED" prefix="IR_">
@@ -136,9 +136,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/Poine/test_libeknav.xml b/conf/airframes/Poine/test_libeknav.xml
index 0c54159b75..59ec1b8ac7 100644
--- a/conf/airframes/Poine/test_libeknav.xml
+++ b/conf/airframes/Poine/test_libeknav.xml
@@ -74,7 +74,7 @@
     overo_test_telemetry2.CFLAGS  += $(PAPARAZZI_INC)
     overo_test_telemetry2.srcs     = fms/overo_test_telemetry2.c
     overo_test_telemetry2.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-    overo_test_telemetry2.srcs    += fms/udp_transport2.c downlink.c
+    overo_test_telemetry2.srcs    += fms/udp_transport2.c subsystems/datalink/downlink.c
     overo_test_telemetry2.srcs    += fms/fms_network.c
     overo_test_telemetry2.LDFLAGS += -levent
 
diff --git a/conf/airframes/Poine/turntable.xml b/conf/airframes/Poine/turntable.xml
index a0d389e319..83ab40b552 100644
--- a/conf/airframes/Poine/turntable.xml
+++ b/conf/airframes/Poine/turntable.xml
@@ -31,7 +31,7 @@ main.srcs += mcu_periph/uart.c
 main.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
 
-main.srcs += downlink.c pprz_transport.c
+main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 main.CFLAGS += -DMB_TACHO
 main.srcs += $(MB)/mb_tacho.c
diff --git a/conf/airframes/TU_Delft/EasyStartPanTiltCHIMU_SPI.xml b/conf/airframes/TU_Delft/EasyStartPanTiltCHIMU_SPI.xml
index 145badd740..ba06b1beed 100644
--- a/conf/airframes/TU_Delft/EasyStartPanTiltCHIMU_SPI.xml
+++ b/conf/airframes/TU_Delft/EasyStartPanTiltCHIMU_SPI.xml
@@ -59,14 +59,14 @@
   </section>
 
   <section name="PANTILT" prefix="CAM_">
-    <define name="PAN_MIN" value="-45"/>
-    <define name="PAN_NEUTRAL" value="33"/>
-    <define name="PAN_MAX" value="156"/>
-    <define name="PAN0" value="0"/>
-    <define name="TILT_MIN" value="-62"/>
-    <define name="TILT_NEUTRAL" value="47"/>
-    <define name="TILT_MAX" value="122"/>
-    <define name="TILT0" value="90"/>
+    <define name="PAN_MIN" value="-45" unit="deg" code_unit="deg"/>
+    <define name="PAN_NEUTRAL" value="33" unit="deg" code_unit="deg"/>
+    <define name="PAN_MAX" value="156" unit="deg" code_unit="deg"/>
+    <define name="PAN0" value="0" unit="deg" code_unit="deg"/>
+    <define name="TILT_MIN" value="-62" unit="deg" code_unit="deg"/>
+    <define name="TILT_NEUTRAL" value="47" unit="deg" code_unit="deg"/>
+    <define name="TILT_MAX" value="122" unit="deg" code_unit="deg"/>
+    <define name="TILT0" value="90" unit="deg" code_unit="deg"/>
     <define name="MODE0" value="1"/>
   </section>
 
@@ -86,9 +86,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -117,9 +114,9 @@
     <define name="COURSE_DGAIN" value="0.3"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="0.2"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.9" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.9" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="12000."/>
     <define name="PITCH_DGAIN" value="0"/>
diff --git a/conf/airframes/TU_Delft/MicrojetBR.xml b/conf/airframes/TU_Delft/MicrojetBR.xml
index 96546c858b..1f8f270954 100644
--- a/conf/airframes/TU_Delft/MicrojetBR.xml
+++ b/conf/airframes/TU_Delft/MicrojetBR.xml
@@ -93,9 +93,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
 
 
     <define name="LIGHT_LED_1" value="3"/>
@@ -139,9 +136,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.54100000858"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.916999995708" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.916999995708" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="5000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -194,15 +191,15 @@
   </section>
 
   <section name="CAM" prefix="CAM_">
-    <define name="TILT_MAX" value="30" unit="deg"/>
-    <define name="TILT_NEUTRAL" value="0" unit="deg"/>
-    <define name="TILT_MIN" value="-30" unit="deg"/>
-    <define name="TILT0" value="0" unit="deg"/>
+    <define name="TILT_MAX" value="30" unit="deg" code_unit="deg"/>
+    <define name="TILT_NEUTRAL" value="0" unit="deg" code_unit="deg"/>
+    <define name="TILT_MIN" value="-30" unit="deg" code_unit="deg"/>
+    <define name="TILT0" value="0" unit="deg" code_unit="deg"/>
 
-    <define name="PAN_MAX" value="45" unit="deg"/>
-    <define name="PAN_NEUTRAL" value="0" unit="deg"/>
-    <define name="PAN_MIN" value="-45" unit="deg"/>
-    <define name="PAN0" value="0" unit="deg"/>
+    <define name="PAN_MAX" value="45" unit="deg" code_unit="deg"/>
+    <define name="PAN_NEUTRAL" value="0" unit="deg" code_unit="deg"/>
+    <define name="PAN_MIN" value="-45" unit="deg" code_unit="deg"/>
+    <define name="PAN0" value="0" unit="deg" code_unit="deg"/>
   </section>
 
 <!--moet nog in tuning-->
diff --git a/conf/airframes/TU_Delft/MicrojetBRimu.xml b/conf/airframes/TU_Delft/MicrojetBRimu.xml
index 8fc4a92176..45f61c2641 100644
--- a/conf/airframes/TU_Delft/MicrojetBRimu.xml
+++ b/conf/airframes/TU_Delft/MicrojetBRimu.xml
@@ -103,8 +103,8 @@
   </section>
 
   <section name="INS" prefix="INS_">
-    <define name="ROLL_NEUTRAL_DEFAULT" value="RadOfDeg(6.18794441223)" unit="rad"/>
-    <define name="PITCH_NEUTRAL_DEFAULT" value="RadOfDeg(-6.87549352646)" unit="deg"/>
+    <define name="ROLL_NEUTRAL_DEFAULT" value="6.18794441223" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="-6.87549352646" unit="deg"/>
   </section>
 
   <section name="AUTO1" prefix="AUTO1_">
@@ -128,9 +128,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
 
 
     <define name="LIGHT_LED_1" value="3"/>
@@ -175,9 +172,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.12900002003"/>
     <define name="COURSE_DGAIN" value="0.556999993324"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.916999995708" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.916999995708" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="0."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -230,15 +227,15 @@
   </section>
 
   <section name="CAM" prefix="CAM_">
-    <define name="TILT_MAX" value="30" unit="deg"/>
-    <define name="TILT_NEUTRAL" value="0" unit="deg"/>
-    <define name="TILT_MIN" value="-30" unit="deg"/>
-    <define name="TILT0" value="0" unit="deg"/>
+    <define name="TILT_MAX" value="30" unit="deg" code_unit="deg"/>
+    <define name="TILT_NEUTRAL" value="0" unit="deg" code_unit="deg"/>
+    <define name="TILT_MIN" value="-30" unit="deg" code_unit="deg"/>
+    <define name="TILT0" value="0" unit="deg" code_unit="deg"/>
 
-    <define name="PAN_MAX" value="45" unit="deg"/>
-    <define name="PAN_NEUTRAL" value="0" unit="deg"/>
-    <define name="PAN_MIN" value="-45" unit="deg"/>
-    <define name="PAN0" value="0" unit="deg"/>
+    <define name="PAN_MAX" value="45" unit="deg" code_unit="deg"/>
+    <define name="PAN_NEUTRAL" value="0" unit="deg" code_unit="deg"/>
+    <define name="PAN_MIN" value="-45" unit="deg" code_unit="deg"/>
+    <define name="PAN0" value="0" unit="deg" code_unit="deg"/>
   </section>
 
 <!--moet nog in tuning-->
diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml
index 31e1587520..3ae16b42a4 100644
--- a/conf/airframes/TU_Delft/MicrojetCDW.xml
+++ b/conf/airframes/TU_Delft/MicrojetCDW.xml
@@ -123,9 +123,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="75."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="0." unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -151,9 +148,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.39999997616"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.703999996185" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.703999996185" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="6000."/>
     <define name="PITCH_DGAIN" value="0."/>
diff --git a/conf/airframes/TU_Delft/Trip50A.xml b/conf/airframes/TU_Delft/Trip50A.xml
index 86e9467fca..5dc6235492 100644
--- a/conf/airframes/TU_Delft/Trip50A.xml
+++ b/conf/airframes/TU_Delft/Trip50A.xml
@@ -105,9 +105,6 @@
     <define name="NO_XBEE_API_INIT" value="TRUE"/>  <!-- uncommant after programed-->
     <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="70."/>
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="5" unit="deg"/>
 
   </section>
 
@@ -163,9 +160,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.5"/>	<!-- Heading outerloop: only P-gain -->
 
-    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="radians"/> <!-- Max Angles -->
-    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.75" unit="rad"/> <!-- Max Angles -->
+    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="7110.09179688"/>	<!-- Pitch Angle PD control -->
     <define name="PITCH_DGAIN" value="2."/>
@@ -238,15 +235,15 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -#DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 # Maxstream API protocol
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/TU_Delft/skywalker.xml b/conf/airframes/TU_Delft/skywalker.xml
index cbb73a4633..c685616329 100644
--- a/conf/airframes/TU_Delft/skywalker.xml
+++ b/conf/airframes/TU_Delft/skywalker.xml
@@ -90,8 +90,8 @@
   </section>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(75)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(45)"/>
+    <define name="MAX_ROLL" value="75" unit="deg"/>
+    <define name="MAX_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="BAT">
@@ -117,9 +117,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
 
     <define name="COMMAND_ROLL_TRIM" value="-2300" />
     <define name="COMMAND_PITCH_TRIM" value="1800" />
@@ -151,9 +148,9 @@
     <define name="COURSE_DGAIN" value="0.280999988317"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="1.0039999485"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.851999998093" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.851999998093" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="15429.6865234"/>
     <define name="PITCH_DGAIN" value="7.73400020599"/>
diff --git a/conf/airframes/TU_Delft/skywalkerfiber.xml b/conf/airframes/TU_Delft/skywalkerfiber.xml
index dea9812b18..8c3adb94f7 100644
--- a/conf/airframes/TU_Delft/skywalkerfiber.xml
+++ b/conf/airframes/TU_Delft/skywalkerfiber.xml
@@ -159,8 +159,8 @@
   </section>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(75)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(45)"/>
+    <define name="MAX_ROLL" value="75" unit="deg"/>
+    <define name="MAX_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="BAT">
@@ -245,9 +245,6 @@
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
 <!-- The Glide definitions are used for calculating the touch down point during auto landing -->
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
 <!-- ******************* VERTICAL CONTROL ********************************** -->
@@ -279,9 +276,9 @@
     <define name="COURSE_DGAIN" value="0.280999988317"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="1.0039999485"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.851999998093" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.851999998093" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="15429.6865234"/>
     <define name="PITCH_DGAIN" value="7.73400020599"/>
diff --git a/conf/airframes/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/UofAdelaide/A1000_BOOZ.xml
index ee83c8370a..aab64eeeab 100644
--- a/conf/airframes/UofAdelaide/A1000_BOOZ.xml
+++ b/conf/airframes/UofAdelaide/A1000_BOOZ.xml
@@ -132,9 +132,9 @@ second attempt
 
 	<define name="MAG_45_HACK" value="1"/>
 -->
-	<define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 2.9 )"/> <!-- previously 1.5-->
-	<define name="BODY_TO_IMU_THETA" value="RadOfDeg( -3.7 )"/> <!-- previously -1.5-->
-	<define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 0. )"/>
+	<define name="BODY_TO_IMU_PHI"   value="2.9 " unit="deg"/> <!-- previously 1.5-->
+	<define name="BODY_TO_IMU_THETA" value="-3.7 " unit="deg"/> <!-- previously -1.5-->
+	<define name="BODY_TO_IMU_PSI"   value="0. " unit="deg"/>
 
   </section>
 
@@ -174,25 +174,25 @@ second attempt
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
 	<!-- setpoints -->
-	<define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-	<define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-	<define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+	<define name="SP_MAX_PHI"     value="45." unit="deg"/>
+	<define name="SP_MAX_THETA"   value="45." unit="deg"/>
+	<define name="SP_MAX_R"       value="90." unit="deg/s"/>
 	<define name="DEADBAND_R"     value="250"/>
 
 	 <!-- reference -->
-	<define name="REF_OMEGA_P"  value="RadOfDeg(500)"/> <!--originally 800-->
+	<define name="REF_OMEGA_P"  value="500" unit="deg/s"/> <!--originally 800-->
 	<define name="REF_ZETA_P"   value="0.85"/>
-	<define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+	<define name="REF_MAX_P"    value="300." unit="deg/s"/>
 	<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-	<define name="REF_OMEGA_Q"  value="RadOfDeg(500)"/> <!-- originally 800-->
+	<define name="REF_OMEGA_Q"  value="500" unit="deg/s"/> <!-- originally 800-->
 	<define name="REF_ZETA_Q"   value="0.85"/>
-	<define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+	<define name="REF_MAX_Q"    value="300." unit="deg/s"/>
 	<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-	<define name="REF_OMEGA_R"  value="RadOfDeg(700)"/> <!-- originally 500-->
+	<define name="REF_OMEGA_R"  value="700" unit="deg/s"/> <!-- originally 500-->
 	<define name="REF_ZETA_R"   value="0.85"/>
-	<define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+	<define name="REF_MAX_R"    value="180." unit="deg/s"/>
 	<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
@@ -274,7 +274,7 @@ second attempt
 	  <define name="RADIO_KILL_SWITCH" value="4"/>
 	  <define name="MODEM_BAUD" value = "B57600"/>
 	  <define name="GPS_LAG" value = "0.8"/>
-	  <define name="BOOZ_START_DELAY" value = "3"/>
+	  <define name="ACTUATORS_START_DELAY" value = "3"/>
  <!-- <define name="KILL_MOTORS"/> -->
 	  <define name="FAILSAFE_GROUND_DETECT"/>
 	  <define name="USE_GPS_ACC4R" />
@@ -347,7 +347,7 @@ include $(CFG_BOOZ)/subsystems/ins_hff.makefile
 ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
 ap.CFLAGS += -DGPS_USE_LATLONG
 
-ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
+ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
 
  </makefile>
  -->
diff --git a/conf/airframes/UofAdelaide/A1000_LISA.xml b/conf/airframes/UofAdelaide/A1000_LISA.xml
index ee9f6b9dba..919d3ed2ea 100644
--- a/conf/airframes/UofAdelaide/A1000_LISA.xml
+++ b/conf/airframes/UofAdelaide/A1000_LISA.xml
@@ -61,9 +61,9 @@
 <define name="MAG_Y_SENS" value="4.13471628028" integer="16"/>
 <define name="MAG_Z_SENS" value="2.07762920804" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0.53 )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   -1.24 )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 0)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0.53 " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="-1.24 " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0" unit="deg"/>
 
   </section>
 
@@ -93,25 +93,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback
diff --git a/conf/airframes/UofAdelaide/A1000_NOVA.xml b/conf/airframes/UofAdelaide/A1000_NOVA.xml
index 448b1e2923..5bc4f2fbc9 100644
--- a/conf/airframes/UofAdelaide/A1000_NOVA.xml
+++ b/conf/airframes/UofAdelaide/A1000_NOVA.xml
@@ -62,9 +62,9 @@
 	<define name="MAG_Z_SENS" value=" 2.98000294999" integer="16"/>
 
 
-	<define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-	<define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-	<define name="BODY_TO_IMU_PSI"   value="RadOfDeg(-90.)"/>
+	<define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+	<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+	<define name="BODY_TO_IMU_PSI"   value="-90." unit="deg"/>
 
   </section>
 
@@ -96,25 +96,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
 	<!-- setpoints -->
-	<define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-	<define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-	<define name="SP_MAX_R"       value="RadOfDeg(180.)"/><!-- Maximum speed in deg/s-->
+	<define name="SP_MAX_PHI"     value="45." unit="deg"/>
+	<define name="SP_MAX_THETA"   value="45." unit="deg"/>
+	<define name="SP_MAX_R"       value="180." unit="deg/s"/><!-- Maximum speed in deg/s-->
 	<define name="DEADBAND_R"     value="250"/>
 
 	 <!-- reference -->
-	<define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+	<define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
 	<define name="REF_ZETA_P"   value="0.85"/>
-	<define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+	<define name="REF_MAX_P"    value="300." unit="deg/s"/>
 	<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-	<define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+	<define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
 	<define name="REF_ZETA_Q"   value="0.85"/>
-	<define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+	<define name="REF_MAX_Q"    value="300." unit="deg/s"/>
 	<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-	<define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+	<define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
 	<define name="REF_ZETA_R"   value="0.85"/>
-	<define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+	<define name="REF_MAX_R"    value="180." unit="deg/s"/>
 	<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
@@ -232,7 +232,7 @@
 	  <configure  name="FLASH_MODE" value="IAP"/>   <!-- default for the board but putting it here as example-->
 	  <define name="RADIO_KILL_SWITCH" value="RADIO_CONTROL_SWITCH1"/>
 	  <define name="MODEM_BAUD" value = "B57600"/>
-	  <define name="BOOZ_START_DELAY" value = "3"/>
+	  <define name="ACTUATORS_START_DELAY" value = "3"/>
 	  <define name="GPS_LAG" value = "0.8"/>
 	  <!-- <define name="KILL_MOTORS"/> -->
 	  <define name="FAILSAFE_GROUND_DETECT"/>
@@ -296,7 +296,7 @@ ap.CFLAGS += -DGPS_LAG=0.8
 #-DUSE_GPS_ACC4R
 ap.CFLAGS += -DGPS_USE_LATLONG
 
-ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
+ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
 
   </makefile>
 -->
diff --git a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
index b4eb06b94e..062a08ed32 100644
--- a/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
+++ b/conf/airframes/UofAdelaide/booz2_NoVa_001_1000.xml
@@ -62,9 +62,9 @@
     <define name="MAG_Z_SENS" value=" 2.98000294999" integer="16"/>
 
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(-90.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-90." unit="deg"/>
 
   </section>
 
@@ -96,25 +96,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(180.)"/><!-- Maximum speed in deg/s-->
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="180." unit="deg/s"/><!-- Maximum speed in deg/s-->
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
@@ -250,7 +250,7 @@ ap.CFLAGS += -DGPS_LAG=0.8
 #-DUSE_GPS_ACC4R
 ap.CFLAGS += -DGPS_USE_LATLONG
 
-ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
+ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
 
 
   </makefile>
diff --git a/conf/airframes/UofAdelaide/booz2_a1000.xml b/conf/airframes/UofAdelaide/booz2_a1000.xml
index b31411f3d0..b42cb89a90 100755
--- a/conf/airframes/UofAdelaide/booz2_a1000.xml
+++ b/conf/airframes/UofAdelaide/booz2_a1000.xml
@@ -131,9 +131,9 @@ second attempt
 
     <define name="MAG_45_HACK" value="1"/>
 -->
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 2.9 )"/> <!-- previously 1.5-->
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg( -3.7 )"/> <!-- previously -1.5-->
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 0. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="2.9 " unit="deg"/> <!-- previously 1.5-->
+    <define name="BODY_TO_IMU_THETA" value="-3.7 " unit="deg"/> <!-- previously -1.5-->
+    <define name="BODY_TO_IMU_PSI"   value="0. " unit="deg"/>
 
   </section>
 
@@ -174,25 +174,25 @@ second attempt
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(500)"/> <!--originally 800-->
+    <define name="REF_OMEGA_P"  value="500" unit="deg/s"/> <!--originally 800-->
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(500)"/> <!-- originally 800-->
+    <define name="REF_OMEGA_Q"  value="500" unit="deg/s"/> <!-- originally 800-->
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(700)"/> <!-- originally 500-->
+    <define name="REF_OMEGA_R"  value="700" unit="deg/s"/> <!-- originally 500-->
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
@@ -306,7 +306,7 @@ include $(CFG_BOOZ)/subsystems/ins_hff.makefile
 ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
 ap.CFLAGS += -DGPS_USE_LATLONG
 
-ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
+ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
 
 
   </makefile>
diff --git a/conf/airframes/UofAdelaide/lisa_a1000.xml b/conf/airframes/UofAdelaide/lisa_a1000.xml
index a8c78a3bce..50be06bc31 100644
--- a/conf/airframes/UofAdelaide/lisa_a1000.xml
+++ b/conf/airframes/UofAdelaide/lisa_a1000.xml
@@ -58,9 +58,9 @@
     <define name="MAG_Y_NEUTRAL" value=" 49"/>
     <define name="MAG_Z_NEUTRAL" value="-66"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45." unit="deg"/>
 
   </section>
 
@@ -90,25 +90,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd
index 22808bfe3e..d1a225ab55 100644
--- a/conf/airframes/airframe.dtd
+++ b/conf/airframes/airframe.dtd
@@ -97,6 +97,7 @@ value CDATA #REQUIRED>
 name CDATA #REQUIRED
 value CDATA #IMPLIED
 unit CDATA #IMPLIED
+code_unit CDATA #IMPLIED
 integer CDATA #IMPLIED>
 
 <!ATTLIST configure
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index 82f1aaea4a..6f381ae18c 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -11,7 +11,6 @@
       <subsystem name="fdm"   type="nps"/>
     </target>
 
-    <define name="NO_FUCKING_STARTUP_DELAY"/>
 
     <subsystem name="telemetry"     type="transparent_usb"/>
     <subsystem name="radio_control" type="ppm"/>
@@ -103,9 +102,9 @@
     <define name="MAG_Y_NEUTRAL" value="1944"/>
     <define name="MAG_Z_NEUTRAL" value="2099"/>
 
-    <define name="BODY_TO_IMU_PHI" value="RadOfDeg(0.)" />
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)" />
-    <define name="BODY_TO_IMU_PSI" value="RadOfDeg(0.)" />
+    <define name="BODY_TO_IMU_PHI" value="0." unit="deg" />
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg" />
+    <define name="BODY_TO_IMU_PSI" value="0." unit="deg" />
   </section>
 
   <section name="AHRS" prefix="AHRS_">
@@ -144,25 +143,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI" value="RadOfDeg(45.)" />
-    <define name="SP_MAX_THETA" value="RadOfDeg(45.)" />
-    <define name="SP_MAX_R" value="RadOfDeg(90.)" />
+    <define name="SP_MAX_PHI" value="45." unit="deg" />
+    <define name="SP_MAX_THETA" value="45." unit="deg" />
+    <define name="SP_MAX_R" value="90." unit="deg/s" />
     <define name="DEADBAND_R" value="250" />
 
     <!-- reference -->
-    <define name="REF_OMEGA_P" value="RadOfDeg(800)" />
+    <define name="REF_OMEGA_P" value="800" unit="deg/s" />
     <define name="REF_ZETA_P" value="0.9" />
-    <define name="REF_MAX_P" value="RadOfDeg(300.)" />
+    <define name="REF_MAX_P" value="300." unit="deg/s" />
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)" />
 
-    <define name="REF_OMEGA_Q" value="RadOfDeg(800)" />
+    <define name="REF_OMEGA_Q" value="800" unit="deg/s" />
     <define name="REF_ZETA_Q" value="0.9" />
-    <define name="REF_MAX_Q" value="RadOfDeg(300.)" />
+    <define name="REF_MAX_Q" value="300." unit="deg/s" />
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)" />
 
-    <define name="REF_OMEGA_R" value="RadOfDeg(500)" />
+    <define name="REF_OMEGA_R" value="500" unit="deg/s" />
     <define name="REF_ZETA_R" value="0.9" />
-    <define name="REF_MAX_R" value="RadOfDeg(180.)" />
+    <define name="REF_MAX_R" value="180." unit="deg/s" />
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)" />
 
     <!-- feedback -->
diff --git a/conf/airframes/booz2_ppzuav.xml b/conf/airframes/booz2_ppzuav.xml
index 22d1e126a7..602e744aa7 100644
--- a/conf/airframes/booz2_ppzuav.xml
+++ b/conf/airframes/booz2_ppzuav.xml
@@ -47,9 +47,9 @@
     <define name="MAG_Y_NEUTRAL" value="5"/>
     <define name="MAG_Z_NEUTRAL" value="192"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
@@ -81,25 +81,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(180.)"/><!-- Maximum speed in deg/s-->
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="180." unit="deg/s"/><!-- Maximum speed in deg/s-->
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
diff --git a/conf/airframes/delta_wing_minimal_example.xml b/conf/airframes/delta_wing_minimal_example.xml
index b5adcf56e7..d9d1ce0dab 100644
--- a/conf/airframes/delta_wing_minimal_example.xml
+++ b/conf/airframes/delta_wing_minimal_example.xml
@@ -63,8 +63,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="35" unit="deg"/>
   </section>
 
   <section name="INFRARED" prefix="IR_">
@@ -125,9 +125,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/demo.xml b/conf/airframes/demo.xml
index d7fd2dc445..287d3a4570 100644
--- a/conf/airframes/demo.xml
+++ b/conf/airframes/demo.xml
@@ -66,7 +66,7 @@ demo4.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 demo4.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 demo4.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-demo4.srcs += downlink.c pprz_transport.c
+demo4.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 #
 # DATALINK : receives datalink messages. demonstrates the "settings" service
@@ -89,7 +89,7 @@ demo5.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 demo5.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 demo5.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-demo5.srcs += downlink.c pprz_transport.c
+demo5.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 demo5.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 
@@ -115,7 +115,7 @@ demo6.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c $(S
 demo6.srcs += $(SRC_ARCH)/usb_ser_hw.c
 
 demo6.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=UsbS
-demo6.srcs += downlink.c pprz_transport.c
+demo6.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 //demo6.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 
@@ -138,7 +138,7 @@ test_spk.srcs += $(SRC_ARCH)/armVIC.c
 
 test_spk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
 test_spk.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-test_spk.srcs += downlink.c $(SRC_ARCH)/usb_ser_hw.c pprz_transport.c
+test_spk.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c
 test_spk.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 test_spk.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
 
diff --git a/conf/airframes/demo_module.xml b/conf/airframes/demo_module.xml
index ad663b260d..3064e5abcc 100644
--- a/conf/airframes/demo_module.xml
+++ b/conf/airframes/demo_module.xml
@@ -118,9 +118,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="6000."/>
     <define name="PITCH_DGAIN" value="0."/>
@@ -181,7 +181,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ########## Modem
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ########## ADC
 ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3
diff --git a/conf/airframes/easy_glider_example.xml b/conf/airframes/easy_glider_example.xml
index 9c72763f2a..c5e961ff91 100644
--- a/conf/airframes/easy_glider_example.xml
+++ b/conf/airframes/easy_glider_example.xml
@@ -81,9 +81,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -111,9 +108,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.10800004005"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="15184.5644531"/>
     <define name="PITCH_DGAIN" value="1.5"/>
diff --git a/conf/airframes/easystar_ets_example.xml b/conf/airframes/easystar_ets_example.xml
index 9b7fe86452..7fcba768ea 100644
--- a/conf/airframes/easystar_ets_example.xml
+++ b/conf/airframes/easystar_ets_example.xml
@@ -66,8 +66,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(40)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="40" unit="deg"/>
   </section>
 
   <section name="INFRARED" prefix="IR_">
@@ -143,9 +143,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.7"/>
-    <define name="ROLL_MAX_SETPOINT" value="RadOfDeg(35)" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="RadOfDeg(25)" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="RadOfDeg(-25)" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="35" unit="deg"/>
+    <define name="PITCH_MAX_SETPOINT" value="25" unit="deg"/>
+    <define name="PITCH_MIN_SETPOINT" value="-25" unit="deg"/>
     <define name="PITCH_PGAIN" value="20000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
     <define name="ELEVATOR_OF_ROLL" value="2500"/>
@@ -172,8 +172,8 @@
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
     <define name="DEFAULT_THROTTLE" value="0.4" unit="%"/>
-    <define name="DEFAULT_ROLL" value="RadOfDeg(15)" unit="rad"/>
-    <define name="DEFAULT_PITCH" value="RadOfDeg(0)" unit="rad"/>
+    <define name="DEFAULT_ROLL" value="15" unit="deg"/>
+    <define name="DEFAULT_PITCH" value="0" unit="deg"/>
     <define name="HOME_RADIUS" value="90" unit="m"/>
   </section>
 
diff --git a/conf/airframes/easystar_example.xml b/conf/airframes/easystar_example.xml
index 4d3eff6b68..7a8a513af7 100644
--- a/conf/airframes/easystar_example.xml
+++ b/conf/airframes/easystar_example.xml
@@ -59,8 +59,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(40)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="40" unit="deg"/>
   </section>
 
   <section name="INFRARED" prefix="IR_">
@@ -130,9 +130,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.7"/>
-    <define name="ROLL_MAX_SETPOINT" value="RadOfDeg(35)" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="RadOfDeg(25)" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="RadOfDeg(-25)" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="35" unit="deg"/>
+    <define name="PITCH_MAX_SETPOINT" value="25" unit="deg"/>
+    <define name="PITCH_MIN_SETPOINT" value="-25" unit="deg"/>
     <define name="PITCH_PGAIN" value="20000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
     <define name="ELEVATOR_OF_ROLL" value="2500"/>
@@ -159,8 +159,8 @@
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
     <define name="DEFAULT_THROTTLE" value="0.4" unit="%"/>
-    <define name="DEFAULT_ROLL" value="RadOfDeg(15)" unit="rad"/>
-    <define name="DEFAULT_PITCH" value="RadOfDeg(0)" unit="rad"/>
+    <define name="DEFAULT_ROLL" value="15" unit="deg"/>
+    <define name="DEFAULT_PITCH" value="0" unit="deg"/>
     <define name="HOME_RADIUS" value="90" unit="m"/>
   </section>
 
diff --git a/conf/airframes/esden/jt_lisam.xml b/conf/airframes/esden/jt_lisam.xml
index 7376de7e3c..047a87ce3e 100644
--- a/conf/airframes/esden/jt_lisam.xml
+++ b/conf/airframes/esden/jt_lisam.xml
@@ -51,9 +51,9 @@
 
   <section name="IMU" prefix="IMU_">
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 180. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(   0. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="180. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0. " unit="deg"/>
 
   </section>
 
@@ -83,28 +83,28 @@
 
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
     <define name="SP_MAX_PSI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="SP_MAX_P"       value="RadOfDeg(90.)"/>
     <define name="DEADBAND_R"     value="250"/>
     <define name="DEADBAND_A"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_R"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_R"  value="600" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.90"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_R"    value="400." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.90"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(500.)"/>
+    <define name="REF_MAX_Q"    value="500." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_P"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_P"  value="600" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(220.)"/>
+    <define name="REF_MAX_P"    value="220." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
@@ -176,7 +176,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_m_1.0">
-<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+<!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
       <subsystem name="actuators"     type="pwm_supervision"/>
       <subsystem name="telemetry"     type="transparent"/>
@@ -194,7 +194,7 @@
       <subsystem name="actuators"     type="mkk"/>
     </target>
 
-    <subsystem name="imu"           type="aspirin"/>
+    <subsystem name="imu"           type="aspirin_v1.0"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="ahrs"          type="int_cmpl_quat"/>
     <subsystem name="stabilization" type="quaternion"/>
diff --git a/conf/airframes/esden/lisa_asctec.xml b/conf/airframes/esden/lisa_asctec.xml
index f29c34b3a0..b274d95895 100644
--- a/conf/airframes/esden/lisa_asctec.xml
+++ b/conf/airframes/esden/lisa_asctec.xml
@@ -68,9 +68,9 @@
     <define name="MAG_XZ_SENS" value="0.0" integer="16"/>
     <define name="MAG_YZ_SENS" value="0.0" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45. " unit="deg"/>
 
   </section>
 
@@ -100,25 +100,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
@@ -184,7 +184,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_l_1.1">
-<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+<!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
       <subsystem name="actuators"     type="asctec_v2"/>
       <subsystem name="telemetry"     type="transparent"/>
diff --git a/conf/airframes/esden/lisa_asctec_aspirin.xml b/conf/airframes/esden/lisa_asctec_aspirin.xml
index 01e6493d1e..cc8015e2ef 100644
--- a/conf/airframes/esden/lisa_asctec_aspirin.xml
+++ b/conf/airframes/esden/lisa_asctec_aspirin.xml
@@ -68,9 +68,9 @@
     <define name="MAG_XZ_SENS" value="0.0" integer="16"/>
     <define name="MAG_YZ_SENS" value="0.0" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45. " unit="deg"/>
 
   </section>
 
@@ -100,25 +100,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
@@ -186,7 +186,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_l_1.1">
-<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+<!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
       <subsystem name="actuators"     type="asctec_v2"/>
       <subsystem name="telemetry"     type="transparent"/>
@@ -200,7 +200,7 @@
       <subsystem name="actuators"     type="mkk"/>
     </target>
 
-    <subsystem name="imu"           type="aspirin"/>
+    <subsystem name="imu"           type="aspirin_v1.0"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="int_cmpl_euler"/>
diff --git a/conf/airframes/esden/lisa_m_pwm.xml b/conf/airframes/esden/lisa_m_pwm.xml
index 004a3f0490..c56c61c399 100644
--- a/conf/airframes/esden/lisa_m_pwm.xml
+++ b/conf/airframes/esden/lisa_m_pwm.xml
@@ -82,9 +82,9 @@
     <define name="MAG_XZ_SENS" value="0.0" integer="16"/>
     <define name="MAG_YZ_SENS" value="0.0" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45. " unit="deg"/>
 
   </section>
 
@@ -114,25 +114,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_P"    value="400." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_Q"    value="400." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
@@ -206,7 +206,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_m_1.0">
-<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+<!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
       <subsystem name="actuators"     type="pwm_supervision"/>
       <subsystem name="telemetry"     type="transparent"/>
@@ -221,7 +221,7 @@
       <subsystem name="actuators"     type="mkk"/>
     </target>
 
-    <subsystem name="imu"           type="aspirin"/>
+    <subsystem name="imu"           type="aspirin_v1.0"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="int_cmpl_quat"/>
diff --git a/conf/airframes/esden/lisa_pwm_aspirin.xml b/conf/airframes/esden/lisa_pwm_aspirin.xml
index 0cd3a409f0..378eab40d3 100644
--- a/conf/airframes/esden/lisa_pwm_aspirin.xml
+++ b/conf/airframes/esden/lisa_pwm_aspirin.xml
@@ -56,9 +56,9 @@
 
   <section name="IMU" prefix="IMU_">
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 225. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="225. " unit="deg"/>
 
   </section>
 
@@ -88,25 +88,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_P"  value="600" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_P"    value="400." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_Q"  value="600" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_Q"    value="400." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
@@ -181,7 +181,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_l_1.1">
-<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+<!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
       <subsystem name="actuators"     type="pwm_supervision"/>
       <subsystem name="telemetry"     type="transparent"/>
@@ -198,7 +198,7 @@
       <subsystem name="actuators"     type="mkk"/>
     </target>
 
-    <subsystem name="imu"           type="aspirin"/>
+    <subsystem name="imu"           type="aspirin_v1.0"/>
     <subsystem name="gps"           type="ublox">
 	    <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
diff --git a/conf/airframes/esden/synerani_4B.xml b/conf/airframes/esden/synerani_4B.xml
index c92116b483..ff6b2fed00 100644
--- a/conf/airframes/esden/synerani_4B.xml
+++ b/conf/airframes/esden/synerani_4B.xml
@@ -59,9 +59,9 @@
 
   <section name="IMU" prefix="IMU_">
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 180. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  90. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(   0. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="180. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="90. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0. " unit="deg"/>
 
   </section>
 
@@ -91,25 +91,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_P"  value="600" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_P"    value="400." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(600)"/>
+    <define name="REF_OMEGA_Q"  value="600" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(400.)"/>
+    <define name="REF_MAX_Q"    value="400." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
@@ -184,7 +184,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_l_1.1">
-<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+<!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="spektrum"/>
       <subsystem name="actuators"     type="pwm_supervision"/>
       <subsystem name="telemetry"     type="transparent"/>
@@ -201,7 +201,7 @@
       <subsystem name="actuators"     type="mkk"/>
     </target>
 
-    <subsystem name="imu"           type="aspirin"/>
+    <subsystem name="imu"           type="aspirin_v1.0"/>
     <subsystem name="gps"           type="ublox">
 	    <configure name="GPS_BAUD"    value="B57600"/>
     </subsystem>
diff --git a/conf/airframes/example_twog_analogimu.xml b/conf/airframes/example_twog_analogimu.xml
index 7dd2238793..5d07183008 100644
--- a/conf/airframes/example_twog_analogimu.xml
+++ b/conf/airframes/example_twog_analogimu.xml
@@ -69,9 +69,9 @@
     <define name="MAG_Y_NEUTRAL" value="512"/>
     <define name="MAG_Z_NEUTRAL" value="512"/>
 
-    <define name="BODY_TO_IMU_PHI" value="RadOfDeg(0.)" />
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)" />
-    <define name="BODY_TO_IMU_PSI" value="RadOfDeg(0.)" />
+    <define name="BODY_TO_IMU_PHI" value="0." unit="deg" />
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg" />
+    <define name="BODY_TO_IMU_PSI" value="0." unit="deg" />
   </section>
 
   <section name="AUTO1" prefix="AUTO1_">
@@ -82,8 +82,8 @@
   <!-- settings for the Analog IMU -->
 
   <section name="INS" prefix="INS_">
-    <define name="ROLL_NEUTRAL_DEFAULT" value="RadOfDeg(0)" unit="deg"/>
-    <define name="PITCH_NEUTRAL_DEFAULT" value="RadOfDeg(0)" unit="deg"/>
+    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
   </section>
 
   <!-- 3S LiPo with 1050mAh, connected to an 20A ESC -->
@@ -144,9 +144,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.85"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="12000."/>
diff --git a/conf/airframes/flixr_discovery.xml b/conf/airframes/flixr_discovery.xml
index c0519b5ff2..a63d413dd6 100644
--- a/conf/airframes/flixr_discovery.xml
+++ b/conf/airframes/flixr_discovery.xml
@@ -16,9 +16,10 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
       <configure name="AHRS_CORRECT_FREQUENCY" value="60"/>
       <configure name="AHRS_ALIGNER_LED" value="1"/>
       <configure name="CPU_LED" value="2"/>
-      <!--define name="IMU_B2_VERSION_1_0"/-->
+      <!-- for airspeed sensor
       <define name="USE_I2C0"/>
       <define name="I2C0_VIC_SLOT" value="12"/>
+      -->
     </target>
     <target name="sim"          board="pc"/>
 
@@ -37,10 +38,12 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
     <!-- Control -->
     <subsystem name="control" type="new"/>
     <subsystem name="navigation" type="extra"/>
-    <subsystem name="ahrs" type="float_dcm"/>
+    <subsystem name="ahrs" type="float_cmpl_rmat"/>
 
     <!-- Sensors -->
-    <subsystem name="imu" type="b2_v1.0"/>
+    <subsystem name="imu" type="b2_v1.0">
+      <configure name="NO_MAG" value="1"/>
+    </subsystem>
     <subsystem name="gps" type="ublox"/>
     <subsystem name="current_sensor">
       <configure name="ADC_CURRENT_SENSOR" value="ADC_3"/>
@@ -66,10 +69,10 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
     <load name="adc_generic.xml">
       <configure name="ADC_CHANNEL_GENERIC1" value="ADC_4"/> <!-- voltage sensor -->
     </load>
-    <load name="airspeed_ets.xml">
+    <!--load name="airspeed_ets.xml">
       <define name="SENSOR_SYNC_SEND"/>
-      <!--define name="USE_AIRSPEED"/-->
-    </load>
+      <define name="USE_AIRSPEED"/>
+    </load-->
   </modules>
 
 <!-- Define here to which CONNECTOR NUMBER the servo is connected to, on the autopilot cicuit board -->
@@ -177,6 +180,14 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
     <define name="BODY_TO_IMU_PSI" value="0"/>
   </section>
 
+  <!-- Munich -->
+  <section name="AHRS" prefix="AHRS_">
+    <define name="H_X" value="0.4357869"/>
+    <define name="H_Y" value="0.0177010"/>
+    <define name="H_Z" value="0.8998758"/>
+    <define name="PROPAGATE_RMAT" value="1"/>
+  </section>
+
   <section name="INS" prefix="INS_">
     <define name="ROLL_NEUTRAL_DEFAULT" value="0."/>
     <define name="PITCH_NEUTRAL_DEFAULT" value="0."/>
@@ -184,8 +195,8 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
 
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(55)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="55" unit="deg"/>
+    <define name="MAX_PITCH" value="35" unit="deg"/>
   </section>
 
 
@@ -230,9 +241,6 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
 
     <!-- The Glide definitions are used for calculating the touch down point during auto landing -->
     <!--
-    <define name="GLIDE_AIRSPEED" value="10."/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="10." unit="deg"/>
     -->
   </section>
 
@@ -306,9 +314,9 @@ http://paparazzi.enac.fr/w/index.php?title=Theory_of_Operation
     <!-- maybe handy to avoid over corner with lots of wind and small airframe -->
     <!-- <define name="COURSE_SLEW_INCREMENT" value="RadOfDeg(4)"/> -->
 
-    <define name="ROLL_MAX_SETPOINT" value="RadOfDeg(50)" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="RadOfDeg(35)" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="RadOfDeg(-30)" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="50" unit="deg"/>
+    <define name="PITCH_MAX_SETPOINT" value="35" unit="deg"/>
+    <define name="PITCH_MIN_SETPOINT" value="-30" unit="deg"/>
 
 
     <define name="ROLL_ATTITUDE_GAIN" value="7500."/>
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml
new file mode 100644
index 0000000000..b5576ec155
--- /dev/null
+++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml
@@ -0,0 +1,217 @@
+<!-- this is a quadrotor frame equiped with Lisa/M and MKK motor controllers -->
+
+<airframe name="fraser">
+
+  <firmware name="rotorcraft">
+    <target name="ap" board="lisa_m_1.0">
+      <subsystem name="radio_control" type="spektrum"/>
+      <define name="RADIO_MODE" value="RADIO_AUX2"/>
+      <define name="RADIO_KILL_SWITCH" value="RADIO_GEAR"/>
+      <define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value="\{1,1,-1,1,-1,-1,-1,1,1,1,1,1\}"/>
+      <define name="RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT" value="UART5"/>
+      <define name="OVERRIDE_UART5_IRQ_HANDLER"/>
+      <define name="ACTUATORS_START_DELAY" value="2"/>
+      <define name="USE_INS_NAV_INIT"/>
+      <!--configure name="SYS_TIME_LED" value="none"/-->
+      <configure name="AHRS_ALIGNER_LED" value="3"/>
+    </target>
+    <target name="sim" board="pc">
+      <subsystem name="fdm"           type="nps"/>
+      <subsystem name="radio_control" type="ppm"/>
+    </target>
+
+    <subsystem name="telemetry"     type="transparent"/>
+    <subsystem name="actuators"     type="mkk"/>
+    <subsystem name="imu"           type="aspirin_v1.5"/>
+    <subsystem name="gps"           type="ublox"/>
+    <subsystem name="stabilization" type="euler"/>
+    <subsystem name="ahrs"          type="int_cmpl_euler"/>
+  </firmware>
+
+  <firmware name="lisa_m_test_progs">
+    <target name="test_sys_time"       board="lisa_m_1.0"/>
+    <target name="test_led"	           board="lisa_m_1.0"/>
+    <target name="test_uart_lisam"     board="lisa_m_1.0"/>
+    <target name="test_servos"         board="lisa_m_1.0"/>
+    <target name="test_telemetry"      board="lisa_m_1.0"/>
+    <target name="test_imu_aspirin"    board="lisa_m_1.0"/>
+    <target name="test_rc_spektrum"    board="lisa_m_1.0"/>
+    <target name="test_baro"           board="lisa_m_1.0"/>
+    <!--<target name="test_imu"           board="lisa_m_1.0"/>
+    <target name="test_rc_ppm"			board="lisa_m_1.0"/>
+    <target name="test_adc"			board="lisa_m_1.0"/>
+    <target name="test_hmc5843"			board="lisa_m_1.0"/>
+    <target name="test_itg3200"			board="lisa_m_1.0"/>
+    <target name="test_adxl345"			board="lisa_m_1.0"/>
+    <target name="test_esc_mkk_simple"		board="lisa_m_1.0"/>
+    <target name="test_esc_asctecv1_simple"	board="lisa_m_1.0"/>
+    <target name="test_actuators_mkk"		board="lisa_m_1.0"/>
+    <target name="test_actuators_asctecv1"	board="lisa_m_1.0"/-->
+  </firmware>
+
+  <modules main_freq="512">
+    <!--load name="sys_mon.xml"/-->
+  </modules>
+
+  <commands>
+    <axis name="PITCH"  failsafe_value="0"/>
+    <axis name="ROLL"   failsafe_value="0"/>
+    <axis name="YAW"    failsafe_value="0"/>
+    <axis name="THRUST" failsafe_value="0"/>
+  </commands>
+
+  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
+    <define name="NB" value="4"/>
+    <define name="ADDR" value="{ 0x54, 0x52, 0x56, 0x58 }"/>
+  </section>
+
+  <section name="SUPERVISION" prefix="SUPERVISION_">
+    <define name="MIN_MOTOR" value="2"/>
+    <define name="MAX_MOTOR" value="210"/>
+    <define name="TRIM_A" value="0"/>
+    <define name="TRIM_E" value="0"/>
+    <define name="TRIM_R" value="0"/>
+    <define name="NB_MOTOR" value="4"/>
+    <define name="SCALE" value="256"/>
+    <define name="ROLL_COEF"   value="{   0,    0, -256,  256 }"/>
+    <define name="PITCH_COEF"  value="{ 256, -256,    0,    0 }"/>
+    <define name="YAW_COEF"    value="{ -256,  -256, 256, 256 }"/>
+    <define name="THRUST_COEF" value="{ 256,  256,  256,  256 }"/>
+  </section>
+
+  <section name="IMU" prefix="IMU_">
+    <define name="ACCEL_X_NEUTRAL" value="11"/>
+    <define name="ACCEL_Y_NEUTRAL" value="11"/>
+    <define name="ACCEL_Z_NEUTRAL" value="-25"/>
+
+    <define name="MAG_X_NEUTRAL" value="-98"/>
+    <define name="MAG_Y_NEUTRAL" value="-252"/>
+    <define name="MAG_Z_NEUTRAL" value="-122"/>
+    <define name="MAG_X_SENS" value="4.04659389111" integer="16"/>
+    <define name="MAG_Y_SENS" value="4.07651737489" integer="16"/>
+    <define name="MAG_Z_SENS" value="3.77693466173" integer="16"/>
+
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-45." unit="deg"/>
+  </section>
+
+  <section name="AHRS" prefix="AHRS_">
+    <define name="PROPAGATE_FREQUENCY" value="512"/>
+    <define name="MAG_UPDATE_YAW_ONLY"/>
+    <define name="H_X" value="0.3770441"/>
+    <define name="H_Y" value="0.0193986"/>
+    <define name="H_Z" value="0.9259921"/>
+  </section>
+
+  <section name="INS" prefix="INS_">
+    <define name="BARO_SENS" value="3.3" integer="16"/>
+  </section>
+
+  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_P" value="10000" />
+    <define name="SP_MAX_Q" value="10000" />
+    <define name="SP_MAX_R" value="10000" />
+    <define name="DEADBAND_P" value="0" />
+    <define name="DEADBAND_Q" value="0" />
+    <define name="DEADBAND_R" value="200" />
+    <define name="REF_TAU" value="4" />
+
+    <!-- feedback -->
+    <define name="GAIN_P" value="-400" />
+    <define name="GAIN_Q" value="-400" />
+    <define name="GAIN_R" value="-350" />
+
+    <define name="IGAIN_P" value="-75" />
+    <define name="IGAIN_Q" value="-75" />
+    <define name="IGAIN_R" value="-50" />
+
+    <!-- feedforward -->
+    <define name="DDGAIN_P" value="300" />
+    <define name="DDGAIN_Q" value="300" />
+    <define name="DDGAIN_R" value="300" />
+  </section>
+
+
+  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
+    <!-- setpoints -->
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
+    <define name="DEADBAND_R"     value="250"/>
+
+    <!-- reference -->
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
+    <define name="REF_ZETA_P"   value="0.85"/>
+    <define name="REF_MAX_P"    value="400." unit="deg/s"/>
+    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
+    <define name="REF_ZETA_Q"   value="0.85"/>
+    <define name="REF_MAX_Q"    value="400." unit="deg/s"/>
+    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
+
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
+    <define name="REF_ZETA_R"   value="0.85"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
+    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
+
+    <!-- feedback -->
+    <define name="PHI_PGAIN"  value="-1000"/>
+    <define name="PHI_DGAIN"  value="-400"/>
+    <define name="PHI_IGAIN"  value="-200"/>
+
+    <define name="THETA_PGAIN"  value="-1000"/>
+    <define name="THETA_DGAIN"  value="-400"/>
+    <define name="THETA_IGAIN"  value="-200"/>
+
+    <define name="PSI_PGAIN"  value="-1000"/>
+    <define name="PSI_DGAIN"  value="-400"/>
+    <define name="PSI_IGAIN"  value="-10"/>
+
+    <!-- feedforward -->
+    <define name="PHI_DDGAIN"   value=" 300"/>
+    <define name="THETA_DDGAIN" value=" 300"/>
+    <define name="PSI_DDGAIN"   value=" 300"/>
+  </section>
+
+  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+    <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
+    <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
+    <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
+    <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
+    <define name="MAX_SUM_ERR" value="2000000"/>
+    <define name="HOVER_KP"    value="-150"/>
+    <define name="HOVER_KD"    value="-80"/>
+    <define name="HOVER_KI"    value="-20"/>
+    <!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
+    <define name="RC_CLIMB_COEF" value ="163"/>
+    <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
+    <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
+    <!--define name="INV_M" value ="0.21"/-->
+  </section>
+
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <define name="PGAIN" value="-100"/>
+    <define name="DGAIN" value="-100"/>
+    <define name="IGAIN" value="-0"/>
+  </section>
+
+  <section name="SIMULATOR" prefix="NPS_">
+    <define name="ACTUATOR_NAMES"  value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
+    <define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
+    <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;"/>
+  </section>
+
+  <section name="AUTOPILOT">
+    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
+    <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_Z_HOLD"/>
+    <define name="MODE_AUTO2"  value="AP_MODE_HOVER_Z_HOLD"/>
+  </section>
+
+  <section name="BAT">
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+  </section>
+
+</airframe>
diff --git a/conf/airframes/funjet_cam_example.xml b/conf/airframes/funjet_cam_example.xml
index 4c2119ea84..3857117f77 100644
--- a/conf/airframes/funjet_cam_example.xml
+++ b/conf/airframes/funjet_cam_example.xml
@@ -156,9 +156,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/funjet_example.xml b/conf/airframes/funjet_example.xml
index 7d84d9886e..04a48470ca 100644
--- a/conf/airframes/funjet_example.xml
+++ b/conf/airframes/funjet_example.xml
@@ -147,9 +147,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml
index a0900bb850..afbd4bdac3 100644
--- a/conf/airframes/jsbsim.xml
+++ b/conf/airframes/jsbsim.xml
@@ -131,9 +131,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="10000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -184,8 +184,8 @@
     <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
     <define name="JSBSIM_INIT"	value="&quot;Malolo1-IC&quot;"/>
     <define name="JSBSIM_LAUNCHSPEED"	value="15.0"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
   <firmware name="fixedwing">
diff --git a/conf/airframes/mentor_tum.xml b/conf/airframes/mentor_tum.xml
index 9c81ae44a0..3965ca9ada 100644
--- a/conf/airframes/mentor_tum.xml
+++ b/conf/airframes/mentor_tum.xml
@@ -89,8 +89,8 @@
 
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(55)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="55" unit="deg"/>
+    <define name="MAX_PITCH" value="35" unit="deg"/>
   </section>
 
 
@@ -170,9 +170,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.1"/>
 
-    <define name="ROLL_MAX_SETPOINT"  value="RadOfDeg(50)" unit="rad"/>
-    <define name="PITCH_MAX_SETPOINT" value="RadOfDeg(30)" unit="rad"/>
-    <define name="PITCH_MIN_SETPOINT" value="RadOfDeg(-25)" unit="rad"/>
+    <define name="ROLL_MAX_SETPOINT"  value="50" unit="deg"/>
+    <define name="PITCH_MAX_SETPOINT" value="30" unit="deg"/>
+    <define name="PITCH_MIN_SETPOINT" value="-25" unit="deg"/>
 
     <define name="PITCH_PGAIN" value="7000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -210,8 +210,8 @@
 
   <section name="SIMU">
     <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
 </airframe>
diff --git a/conf/airframes/microjet_example.xml b/conf/airframes/microjet_example.xml
index 90df88adb1..6ce9c7e8f0 100644
--- a/conf/airframes/microjet_example.xml
+++ b/conf/airframes/microjet_example.xml
@@ -96,9 +96,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -126,9 +123,9 @@
     <define name="COURSE_PGAIN" value="1.0"/>
     <define name="COURSE_DGAIN" value="0.3"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="12000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -161,8 +158,8 @@
 
   <section name="SIMU">
     <define name="JSBSIM_MODEL" value="&quot;Malolo1&quot;"/>
-    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
-    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
+    <define name="JSBSIM_IR_ROLL_NEUTRAL" value="0." unit="deg"/>
+    <define name="JSBSIM_IR_PITCH_NEUTRAL" value="0." unit="deg"/>
   </section>
 
 
diff --git a/conf/airframes/mm/extra/press_t.xml b/conf/airframes/mm/extra/press_t.xml
index a69e2332f0..e4dd555dd4 100644
--- a/conf/airframes/mm/extra/press_t.xml
+++ b/conf/airframes/mm/extra/press_t.xml
@@ -136,9 +136,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -234,11 +234,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/extra/probe_t.xml b/conf/airframes/mm/extra/probe_t.xml
index bb2516274f..3a825469f9 100644
--- a/conf/airframes/mm/extra/probe_t.xml
+++ b/conf/airframes/mm/extra/probe_t.xml
@@ -156,9 +156,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/extra/rctx.xml b/conf/airframes/mm/extra/rctx.xml
index ad468276c0..768e25e0ea 100644
--- a/conf/airframes/mm/extra/rctx.xml
+++ b/conf/airframes/mm/extra/rctx.xml
@@ -33,7 +33,7 @@ ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DUSE_DOWNLINK_BROADCAST
 
diff --git a/conf/airframes/mm/extra/turbine_trigger.xml b/conf/airframes/mm/extra/turbine_trigger.xml
index ab1534728a..962034a273 100644
--- a/conf/airframes/mm/extra/turbine_trigger.xml
+++ b/conf/airframes/mm/extra/turbine_trigger.xml
@@ -134,9 +134,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
     <define name="PITCH_PGAIN" value="5500."/>
diff --git a/conf/airframes/mm/fixed-wing/drops.xml b/conf/airframes/mm/fixed-wing/drops.xml
index 704efc3951..2a459de7be 100644
--- a/conf/airframes/mm/fixed-wing/drops.xml
+++ b/conf/airframes/mm/fixed-wing/drops.xml
@@ -175,9 +175,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0340000391"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -250,11 +250,11 @@ ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_AUTO1
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
@@ -289,7 +289,7 @@ sim.ARCH = sitl
 sim.TARGET = autopilot
 sim.TARGETDIR = autopilot
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
diff --git a/conf/airframes/mm/fixed-wing/funjet43.xml b/conf/airframes/mm/fixed-wing/funjet43.xml
index bad75fc193..7f4e96289a 100644
--- a/conf/airframes/mm/fixed-wing/funjet43.xml
+++ b/conf/airframes/mm/fixed-wing/funjet43.xml
@@ -89,8 +89,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="35" unit="deg"/>
   </section>
 
   <section name="INS" prefix="INS_">
@@ -158,9 +158,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/funjetdca.xml b/conf/airframes/mm/fixed-wing/funjetdca.xml
index c565719202..ecae589dab 100644
--- a/conf/airframes/mm/fixed-wing/funjetdca.xml
+++ b/conf/airframes/mm/fixed-wing/funjetdca.xml
@@ -169,9 +169,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/funjetdcb.xml b/conf/airframes/mm/fixed-wing/funjetdcb.xml
index a973e2d92b..66bf71f34b 100644
--- a/conf/airframes/mm/fixed-wing/funjetdcb.xml
+++ b/conf/airframes/mm/fixed-wing/funjetdcb.xml
@@ -169,9 +169,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/funjetdcc.xml b/conf/airframes/mm/fixed-wing/funjetdcc.xml
index ce05264b26..060a0fe0df 100644
--- a/conf/airframes/mm/fixed-wing/funjetdcc.xml
+++ b/conf/airframes/mm/fixed-wing/funjetdcc.xml
@@ -169,9 +169,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/funjeteth1.xml b/conf/airframes/mm/fixed-wing/funjeteth1.xml
index b3e415b87c..0870c9534a 100644
--- a/conf/airframes/mm/fixed-wing/funjeteth1.xml
+++ b/conf/airframes/mm/fixed-wing/funjeteth1.xml
@@ -132,9 +132,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.8"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.62" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.62" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -211,11 +211,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/mm/fixed-wing/funjeteth2.xml b/conf/airframes/mm/fixed-wing/funjeteth2.xml
index a627605669..063c142bd6 100644
--- a/conf/airframes/mm/fixed-wing/funjeteth2.xml
+++ b/conf/airframes/mm/fixed-wing/funjeteth2.xml
@@ -133,9 +133,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.8"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.49" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.49" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -212,11 +212,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/mm/fixed-wing/funjetfmi1.xml b/conf/airframes/mm/fixed-wing/funjetfmi1.xml
index 3a1eff6726..4954c287c2 100644
--- a/conf/airframes/mm/fixed-wing/funjetfmi1.xml
+++ b/conf/airframes/mm/fixed-wing/funjetfmi1.xml
@@ -131,9 +131,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -214,11 +214,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetfmi2.xml b/conf/airframes/mm/fixed-wing/funjetfmi2.xml
index a879ba9e71..58086f71da 100644
--- a/conf/airframes/mm/fixed-wing/funjetfmi2.xml
+++ b/conf/airframes/mm/fixed-wing/funjetfmi2.xml
@@ -130,9 +130,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -213,11 +213,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetfmi3.xml b/conf/airframes/mm/fixed-wing/funjetfmi3.xml
index f4c3a8ae6b..e62562e87c 100644
--- a/conf/airframes/mm/fixed-wing/funjetfmi3.xml
+++ b/conf/airframes/mm/fixed-wing/funjetfmi3.xml
@@ -130,9 +130,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -213,11 +213,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi1.xml b/conf/airframes/mm/fixed-wing/funjetgfi1.xml
index 2c24b5de95..c44eeda85a 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi1.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi1.xml
@@ -129,9 +129,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -191,11 +191,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi3.xml b/conf/airframes/mm/fixed-wing/funjetgfi3.xml
index d2274b8f88..2b4b01312b 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi3.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi3.xml
@@ -41,8 +41,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(30)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(30)"/>
+    <define name="MAX_ROLL" value="30" unit="deg"/>
+    <define name="MAX_PITCH" value="30" unit="deg"/>
   </section>
 
   <section name="adc" prefix="ADC_CHANNEL_">
@@ -128,9 +128,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.45"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.4" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.4" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="9000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -202,10 +202,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-#TRANSPARENT ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#TRANSPARENT ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi4.xml b/conf/airframes/mm/fixed-wing/funjetgfi4.xml
index 3fabcb51e5..f0f50420f9 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi4.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi4.xml
@@ -128,9 +128,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.8"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="7600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -191,11 +191,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi5.xml b/conf/airframes/mm/fixed-wing/funjetgfi5.xml
index fc0f6d91ac..c821c0e45c 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi5.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi5.xml
@@ -130,9 +130,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -193,11 +193,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi6.xml b/conf/airframes/mm/fixed-wing/funjetgfi6.xml
index 33bcb68f23..a166e63423 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi6.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi6.xml
@@ -130,9 +130,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -193,11 +193,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi7.xml b/conf/airframes/mm/fixed-wing/funjetgfi7.xml
index ab7e259cd0..185f3698ec 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi7.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi7.xml
@@ -130,9 +130,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -193,11 +193,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi8.xml b/conf/airframes/mm/fixed-wing/funjetgfi8.xml
index 78f459ac1b..57b1e4ede7 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi8.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi8.xml
@@ -164,9 +164,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/funjetgfi9.xml b/conf/airframes/mm/fixed-wing/funjetgfi9.xml
index 2c42fefc98..7976264c7e 100644
--- a/conf/airframes/mm/fixed-wing/funjetgfi9.xml
+++ b/conf/airframes/mm/fixed-wing/funjetgfi9.xml
@@ -130,9 +130,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -193,11 +193,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/funjetlisa.xml b/conf/airframes/mm/fixed-wing/funjetlisa.xml
index fbdb116858..5bd30d0a9a 100644
--- a/conf/airframes/mm/fixed-wing/funjetlisa.xml
+++ b/conf/airframes/mm/fixed-wing/funjetlisa.xml
@@ -81,8 +81,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="35" unit="deg"/>
   </section>
 
   <section name="INFRARED" prefix="IR_">
@@ -143,9 +143,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/funjetlisam.xml b/conf/airframes/mm/fixed-wing/funjetlisam.xml
index e92cd84a6e..7551dc03ad 100644
--- a/conf/airframes/mm/fixed-wing/funjetlisam.xml
+++ b/conf/airframes/mm/fixed-wing/funjetlisam.xml
@@ -81,8 +81,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(35)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="35" unit="deg"/>
   </section>
 
   <section name="INFRARED" prefix="IR_">
@@ -143,9 +143,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/funjetmm.xml b/conf/airframes/mm/fixed-wing/funjetmm.xml
index 4773cd625a..f7fca1b037 100644
--- a/conf/airframes/mm/fixed-wing/funjetmm.xml
+++ b/conf/airframes/mm/fixed-wing/funjetmm.xml
@@ -201,9 +201,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/funjetmm2.xml b/conf/airframes/mm/fixed-wing/funjetmm2.xml
index bde533338f..d9f0245330 100644
--- a/conf/airframes/mm/fixed-wing/funjetmm2.xml
+++ b/conf/airframes/mm/fixed-wing/funjetmm2.xml
@@ -136,9 +136,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -219,11 +219,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
index 9f620479fe..8d69163c7b 100644
--- a/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
+++ b/conf/airframes/mm/fixed-wing/fw_ins_arduimu.xml
@@ -161,9 +161,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/merlin.xml b/conf/airframes/mm/fixed-wing/merlin.xml
index b55b5dfc6c..a582746903 100644
--- a/conf/airframes/mm/fixed-wing/merlin.xml
+++ b/conf/airframes/mm/fixed-wing/merlin.xml
@@ -124,9 +124,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/mm/fixed-wing/miniwing.xml b/conf/airframes/mm/fixed-wing/miniwing.xml
index bad55f07ff..82784142de 100644
--- a/conf/airframes/mm/fixed-wing/miniwing.xml
+++ b/conf/airframes/mm/fixed-wing/miniwing.xml
@@ -128,9 +128,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.8"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.62" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.62" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -187,7 +187,7 @@ ap.CFLAGS += -DRADIO_CONTROL -DUSE_RC_TELEMETRY
 ap.srcs += radio_control.c ppm_telemetry.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/mm/fixed-wing/slowfast.xml b/conf/airframes/mm/fixed-wing/slowfast.xml
index 278acd7ea2..304ff94bf2 100644
--- a/conf/airframes/mm/fixed-wing/slowfast.xml
+++ b/conf/airframes/mm/fixed-wing/slowfast.xml
@@ -159,9 +159,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.80" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.80" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="10500."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -229,11 +229,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/slowfast2.xml b/conf/airframes/mm/fixed-wing/slowfast2.xml
index 76cc201370..5ff7706fd8 100644
--- a/conf/airframes/mm/fixed-wing/slowfast2.xml
+++ b/conf/airframes/mm/fixed-wing/slowfast2.xml
@@ -175,9 +175,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.80" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.80" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="10500."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -244,11 +244,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/fixed-wing/twinstarmm.xml b/conf/airframes/mm/fixed-wing/twinstarmm.xml
index fa67dc651a..5b3a9e56c9 100644
--- a/conf/airframes/mm/fixed-wing/twinstarmm.xml
+++ b/conf/airframes/mm/fixed-wing/twinstarmm.xml
@@ -153,9 +153,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -212,11 +212,11 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #XBEE
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
@@ -244,7 +244,7 @@ ap.srcs += subsystems/navigation/snav.c
 # include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.ARCHDIR = $(ARCHI)
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
diff --git a/conf/airframes/mm/hangar/black_one.xml b/conf/airframes/mm/hangar/black_one.xml
index 4e0e07c577..9b2c1d9b65 100644
--- a/conf/airframes/mm/hangar/black_one.xml
+++ b/conf/airframes/mm/hangar/black_one.xml
@@ -150,9 +150,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.85"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="8000."/>
@@ -207,7 +207,7 @@
 </section>
 
 <section name="CAM" prefix="CAM_">
-  <define name="PHI0" value="-45" unit="deg"/>
+  <define name="PHI0" value="-45" unit="deg" code_unit="deg"/>
 </section>
 
  <makefile>
@@ -227,10 +227,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/mm/hangar/glass_one1.xml b/conf/airframes/mm/hangar/glass_one1.xml
index cbf6b6ac19..655ecdc7a6 100644
--- a/conf/airframes/mm/hangar/glass_one1.xml
+++ b/conf/airframes/mm/hangar/glass_one1.xml
@@ -138,10 +138,10 @@
   </section>
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
-    <define name="COURSE_PGAIN" value="1.2"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="COURSE_PGAIN" value="-1.2"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="7000."/>
@@ -217,10 +217,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/mm/hangar/glass_one2.xml b/conf/airframes/mm/hangar/glass_one2.xml
index b27c8e4803..bb876e7ab3 100644
--- a/conf/airframes/mm/hangar/glass_one2.xml
+++ b/conf/airframes/mm/hangar/glass_one2.xml
@@ -123,9 +123,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.2"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="7000."/>
@@ -191,10 +191,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/mm/hangar/glass_one3.xml b/conf/airframes/mm/hangar/glass_one3.xml
index 851b9050f3..ab98cb2d09 100644
--- a/conf/airframes/mm/hangar/glass_one3.xml
+++ b/conf/airframes/mm/hangar/glass_one3.xml
@@ -140,9 +140,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.2"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.63" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.63" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="7000."/>
@@ -218,10 +218,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/mm/hangar/lila.xml b/conf/airframes/mm/hangar/lila.xml
index ec7257b5fc..6f1fe960ef 100644
--- a/conf/airframes/mm/hangar/lila.xml
+++ b/conf/airframes/mm/hangar/lila.xml
@@ -142,9 +142,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.78"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
     <define name="PITCH_PGAIN" value="2500."/>
@@ -215,11 +215,11 @@ ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
@@ -248,7 +248,7 @@ ap.srcs += subsystems/navigation/snav.c
 # Config for SITL simulation
 sim.ARCHDIR = $(ARCHI)
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl_a.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
diff --git a/conf/airframes/mm/hangar/mac06a.xml b/conf/airframes/mm/hangar/mac06a.xml
index 7da365d2da..9adde5f810 100644
--- a/conf/airframes/mm/hangar/mac06a.xml
+++ b/conf/airframes/mm/hangar/mac06a.xml
@@ -135,7 +135,7 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.8"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="5000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -211,10 +211,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #XBEE  ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#XBEE  ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#XBEE  ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/mm/hangar/red_one.xml b/conf/airframes/mm/hangar/red_one.xml
index e82f049333..2bf8952545 100644
--- a/conf/airframes/mm/hangar/red_one.xml
+++ b/conf/airframes/mm/hangar/red_one.xml
@@ -149,9 +149,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.4"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.65" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.65" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="8000."/>
@@ -206,14 +206,14 @@
 </section>
 
 <section name="CAM" prefix="CAM_">
-  <define name="PHI0"          value="0" unit="deg"/>
-  <define name="ROLL_MAX"      value="90" unit="deg"/>
-  <define name="ROLL_NEUTRAL"  value="45" unit="deg"/>
-  <define name="ROLL_MIN"      value="0"  unit="deg"/>
-  <define name="THETA0"        value="45" unit="deg"/>
-  <define name="PITCH_MAX"     value="90" unit="deg"/>
-  <define name="PITCH_NEUTRAL" value="45" unit="deg"/>
-  <define name="PITCH_MIN"     value="0"  unit="deg"/>
+  <define name="PHI0"          value="0" unit="deg" code_unit="deg"/>
+  <define name="ROLL_MAX"      value="90" unit="deg" code_unit="deg"/>
+  <define name="ROLL_NEUTRAL"  value="45" unit="deg" code_unit="deg"/>
+  <define name="ROLL_MIN"      value="0"  unit="deg" code_unit="deg"/>
+  <define name="THETA0"        value="45" unit="deg" code_unit="deg"/>
+  <define name="PITCH_MAX"     value="90" unit="deg" code_unit="deg"/>
+  <define name="PITCH_NEUTRAL" value="45" unit="deg" code_unit="deg"/>
+  <define name="PITCH_MIN"     value="0"  unit="deg" code_unit="deg"/>
 </section>
 
   <section name="DATALINK" prefix="DATALINK_">
@@ -238,10 +238,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/mm/rotor/qmk1.xml b/conf/airframes/mm/rotor/qmk1.xml
index ba57d38764..e8562ff786 100644
--- a/conf/airframes/mm/rotor/qmk1.xml
+++ b/conf/airframes/mm/rotor/qmk1.xml
@@ -68,9 +68,9 @@
     <define name="MAG_Y_SENS" value="4.86013203775" integer="16"/>
     <define name="MAG_Z_SENS" value="2.5947951512" integer="16"/>
 -->
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(   0. )"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(  45.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0. " unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="45." unit="deg"/>
 
   </section>
 
@@ -100,25 +100,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
@@ -187,7 +187,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_l_1.0">
-<!--      <define name="BOOZ_START_DELAY" value="1"/> -->
+<!--      <define name="ACTUATORS_START_DELAY" value="1"/> -->
       <subsystem name="radio_control" type="ppm"/>
       <subsystem name="actuators"     type="mkk"/>
     </target>
diff --git a/conf/airframes/obsolete/autoplato.xml b/conf/airframes/obsolete/autoplato.xml
index e2f8a9e24a..f4cd31c167 100644
--- a/conf/airframes/obsolete/autoplato.xml
+++ b/conf/airframes/obsolete/autoplato.xml
@@ -22,11 +22,11 @@ test_usb.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 #test_usb.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
 #test_usb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #test_usb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-#test_usb.srcs += downlink.c pprz_transport.c
+#test_usb.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_usb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
 test_usb.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-test_usb.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/usb_ser_hw.c pprz_transport.c
+test_usb.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c
 # datalink.c
 test_usb.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 test_usb.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
diff --git a/conf/airframes/obsolete/booz2_Aron.xml b/conf/airframes/obsolete/booz2_Aron.xml
index 879ebcb016..8512e7990a 100644
--- a/conf/airframes/obsolete/booz2_Aron.xml
+++ b/conf/airframes/obsolete/booz2_Aron.xml
@@ -92,25 +92,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(400)"/>
+    <define name="REF_OMEGA_P"  value="400" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(400)"/>
+    <define name="REF_OMEGA_Q"  value="400" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
diff --git a/conf/airframes/obsolete/booz2_NoVa.xml b/conf/airframes/obsolete/booz2_NoVa.xml
index 07af854a2e..472114d6a7 100644
--- a/conf/airframes/obsolete/booz2_NoVa.xml
+++ b/conf/airframes/obsolete/booz2_NoVa.xml
@@ -96,25 +96,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(180.)"/><!-- Maximum speed in deg/s-->
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="180." unit="deg/s"/><!-- Maximum speed in deg/s-->
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
@@ -248,7 +248,7 @@ include $(CFG_BOOZ)/subsystems/ins_hff.makefile
 ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
 ap.CFLAGS += -DGPS_USE_LATLONG
 
-ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
+ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
 
 
   </makefile>
diff --git a/conf/airframes/obsolete/booz2_NoVa_001.xml b/conf/airframes/obsolete/booz2_NoVa_001.xml
index 350eca0093..c6e20076fd 100644
--- a/conf/airframes/obsolete/booz2_NoVa_001.xml
+++ b/conf/airframes/obsolete/booz2_NoVa_001.xml
@@ -62,9 +62,9 @@
     <define name="MAG_Z_SENS" value="-2.98000294999" integer="16"/>
 
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(-90.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-90." unit="deg"/>
 
   </section>
 
@@ -96,25 +96,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(180.)"/><!-- Maximum speed in deg/s-->
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="180." unit="deg/s"/><!-- Maximum speed in deg/s-->
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
@@ -248,7 +248,7 @@ include $(CFG_BOOZ)/subsystems/ins_hff.makefile
 ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
 ap.CFLAGS += -DGPS_USE_LATLONG
 
-ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
+ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
 
 
   </makefile>
diff --git a/conf/airframes/obsolete/booz2_NoVa_002.xml b/conf/airframes/obsolete/booz2_NoVa_002.xml
index 0dca405b1e..c243de3ae4 100644
--- a/conf/airframes/obsolete/booz2_NoVa_002.xml
+++ b/conf/airframes/obsolete/booz2_NoVa_002.xml
@@ -61,9 +61,9 @@
     <define name="MAG_Y_NEUTRAL" value="20"/>
     <define name="MAG_Z_NEUTRAL" value="-30"/>
 
-   <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(-90.)"/>
+   <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-90." unit="deg"/>
 
   </section>
 
@@ -96,25 +96,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(180.)"/><!-- Maximum speed in deg/s-->
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="180." unit="deg/s"/><!-- Maximum speed in deg/s-->
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
@@ -249,7 +249,7 @@ include $(CFG_BOOZ)/subsystems/ins_hff.makefile
 ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R
 ap.CFLAGS += -DGPS_USE_LATLONG
 
-ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DBOOZ_START_DELAY=3
+ap.CFLAGS += -DUSE_ADAPT_HOVER -DUSE_INS_NAV_INIT -DACTUATORS_START_DELAY=3
 
 
   </makefile>
diff --git a/conf/airframes/obsolete/booz2_a2.xml b/conf/airframes/obsolete/booz2_a2.xml
index b34c272223..c6dc404e3c 100644
--- a/conf/airframes/obsolete/booz2_a2.xml
+++ b/conf/airframes/obsolete/booz2_a2.xml
@@ -65,9 +65,9 @@
     <define name="MAG_Y_SENS" value="-4.2959321" integer="16"/>
     <define name="MAG_Z_SENS" value="-4.0988408" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 1.5)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(-1.5)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 0. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="1.5" unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="-1.5" unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0. " unit="deg"/>
 
   </section>
 
@@ -86,25 +86,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
diff --git a/conf/airframes/obsolete/booz2_a3.xml b/conf/airframes/obsolete/booz2_a3.xml
index f546148b32..7e7755e84e 100644
--- a/conf/airframes/obsolete/booz2_a3.xml
+++ b/conf/airframes/obsolete/booz2_a3.xml
@@ -52,9 +52,9 @@
     <define name="MAG_Y_SENS" value="20." integer="16"/>
     <define name="MAG_Z_SENS" value="15." integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 14.5)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg( -8.5)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(-45. )"/>
+    <define name="BODY_TO_IMU_PHI"   value="14.5" unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="-8.5" unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-45. " unit="deg"/>
 
   </section>
 
@@ -73,25 +73,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
    <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
diff --git a/conf/airframes/obsolete/booz2_a4.xml b/conf/airframes/obsolete/booz2_a4.xml
index 7fa08869a7..337c76d244 100644
--- a/conf/airframes/obsolete/booz2_a4.xml
+++ b/conf/airframes/obsolete/booz2_a4.xml
@@ -54,9 +54,9 @@
     <define name="MAG_X_SENS" value="-3.03386034" integer="16"/>
     <define name="MAG_Y_SENS" value=" 4.99085017" integer="16"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg( 0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg( 0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg( 0.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
 
   </section>
 
diff --git a/conf/airframes/obsolete/booz2_a5.xml b/conf/airframes/obsolete/booz2_a5.xml
index fc2c37f70a..bcdd61a563 100644
--- a/conf/airframes/obsolete/booz2_a5.xml
+++ b/conf/airframes/obsolete/booz2_a5.xml
@@ -75,9 +75,9 @@
     <define name="MAG_Y_NEUTRAL" value="24"/>
     <define name="MAG_Z_NEUTRAL" value="28"/>
 
-    <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_THETA" value="RadOfDeg(  0.)"/>
-    <define name="BODY_TO_IMU_PSI"   value="RadOfDeg(-45.)"/>
+    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="-45." unit="deg"/>
 
   </section>
 
@@ -96,25 +96,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
     <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
     <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml
index a349ec4aa2..9711a48c3f 100644
--- a/conf/airframes/obsolete/booz2_s1.xml
+++ b/conf/airframes/obsolete/booz2_s1.xml
@@ -88,25 +88,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
    <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
diff --git a/conf/airframes/obsolete/booz2_x1.xml b/conf/airframes/obsolete/booz2_x1.xml
index 899ff28868..7ce46bd0d3 100644
--- a/conf/airframes/obsolete/booz2_x1.xml
+++ b/conf/airframes/obsolete/booz2_x1.xml
@@ -99,25 +99,25 @@
   <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
 
   <!-- setpoints -->
-    <define name="SP_MAX_PHI"     value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_THETA"   value="RadOfDeg(45.)"/>
-    <define name="SP_MAX_R"       value="RadOfDeg(90.)"/>
+    <define name="SP_MAX_PHI"     value="45." unit="deg"/>
+    <define name="SP_MAX_THETA"   value="45." unit="deg"/>
+    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
     <define name="DEADBAND_R"     value="250"/>
 
      <!-- reference -->
-    <define name="REF_OMEGA_P"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_P"   value="0.85"/>
-    <define name="REF_MAX_P"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
     <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_Q"  value="RadOfDeg(800)"/>
+    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
     <define name="REF_ZETA_Q"   value="0.85"/>
-    <define name="REF_MAX_Q"    value="RadOfDeg(300.)"/>
+    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
     <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
 
-    <define name="REF_OMEGA_R"  value="RadOfDeg(500)"/>
+    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
     <define name="REF_ZETA_R"   value="0.85"/>
-    <define name="REF_MAX_R"    value="RadOfDeg(180.)"/>
+    <define name="REF_MAX_R"    value="180." unit="deg/s"/>
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
    <!-- feedback -->
diff --git a/conf/airframes/obsolete/csc.xml b/conf/airframes/obsolete/csc.xml
deleted file mode 100644
index 8fb6f91640..0000000000
--- a/conf/airframes/obsolete/csc.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-<airframe name="CSC">
-
-  <servos>
-    <servo name="S1" no="0" min="1000" neutral="1500" max="2000"/>
-    <servo name="S2" no="1" min="1000" neutral="1500" max="2000"/>
-    <servo name="S3" no="3" min="1000" neutral="1500" max="2000"/>
-    <servo name="S4" no="2" min="1000" neutral="1500" max="2000"/>
-  </servos>
-
-  <makefile>
-
-ap.CFLAGS += -DPWM_SERVO_4 -DPWM_SERVO_5 -DPWM_SERVO_0 # -DPWM_SERVO_3
-ap.CFLAGS += -DACTUATORS=\"servos_direct_hw.h\"
-ap.srcs += $(SRC_ARCH)/servos_direct_hw.c
-CSC_ID = 0
-SRC_BOOZ = booz/
-
-
-include $(PAPARAZZI_SRC)/conf/autopilot/csc.makefile
-include $(PAPARAZZI_SRC)/conf/autopilot/subsystems/csc_gps.makefile
-
-  </makefile>
-
-</airframe>
diff --git a/conf/airframes/obsolete/csc_ap.xml b/conf/airframes/obsolete/csc_ap.xml
deleted file mode 100644
index a1017e56ea..0000000000
--- a/conf/airframes/obsolete/csc_ap.xml
+++ /dev/null
@@ -1,60 +0,0 @@
-<airframe name="CSCAP">
-
-<!-- commands section -->
-  <servos>
-    <servo name="AILERON"  no="0" min="1000" neutral="1523" max="2000"/>
-    <servo name="ELEVATOR" no="1" min="2000" neutral="1500" max="1000"/>
-    <servo name="RUDDER" no="2" min="1000" neutral="1500" max="2000"/>
-    <servo name="DRAG1" no="3" min="1000" neutral="1500" max="2000"/>
-    <servo name="DRAG2" no="4" min="1000" neutral="1500" max="2000"/>
-    <servo name="DRAG3" no="5" min="1000" neutral="1500" max="2000"/>
-    <servo name="DRAG4" no="6" min="1000" neutral="1500" max="2000"/>
-  </servos>
-
-  <csc_boards>
-   <board id="0">
-      <msg id="SERVO_CMD_ID" type="ServoCmd">
-       <field_map field="servos[0]" servo_id="SERVO_AILERON"/>
-       <field_map field="servos[1]" servo_id="SERVO_ELEVATOR"/>
-       <field_map field="servos[2]" servo_id="SERVO_RUDDER"/>
-      </msg>
-   </board>
-   <board id="1">
-      <msg id="SERVO_CMD_ID" type="ServoCmd">
-       <field_map field="servos[0]" servo_id="SERVO_DRAG1"/>
-       <field_map field="servos[1]" servo_id="SERVO_DRAG2"/>
-       <field_map field="servos[2]" servo_id="SERVO_DRAG3"/>
-       <field_map field="servos[3]" servo_id="SERVO_DRAG4"/>
-      </msg>
-   </board>
-  </csc_boards>
-
-
-  <commands>
-    <axis name="ROLL"     failsafe_value="0"/>
-    <axis name="PITCH"    failsafe_value="0"/>
-    <axis name="YAW"    failsafe_value="0"/>
-  </commands>
-
-  <rc_commands>
-    <set command="ROLL"     value="@ROLL"/>
-    <set command="PITCH"    value="@PITCH"/>
-    <set command="YAW"    value="@YAW"/>
-  </rc_commands>
-
-  <command_laws>
-    <set servo="ELEVATOR" value="@PITCH"/>
-    <set servo="AILERON" value="@ROLL"/>
-    <set servo="RUDDER" value="@YAW"/>
-    <set servo="DRAG1" value="csc_drag_yaw * @YAW + csc_drag_pitch * @PITCH"/>
-    <set servo="DRAG2" value="-csc_drag_yaw * @YAW + csc_drag_pitch * @PITCH"/>
-    <set servo="DRAG3" value="-csc_drag_yaw * @YAW + -csc_drag_pitch * @PITCH"/>
-    <set servo="DRAG4" value="csc_drag_yaw * @YAW + -csc_drag_pitch * @PITCH"/>
-  </command_laws>
-
-<makefile>
-include $(PAPARAZZI_SRC)/conf/autopilot/pwm_input.makefile
-include $(PAPARAZZI_SRC)/conf/autopilot/csc_ap.makefile
-</makefile>
-
-</airframe>
diff --git a/conf/airframes/obsolete/dc_mc.xml b/conf/airframes/obsolete/dc_mc.xml
index 76e6aff207..db35c9c065 100644
--- a/conf/airframes/obsolete/dc_mc.xml
+++ b/conf/airframes/obsolete/dc_mc.xml
@@ -27,7 +27,7 @@ main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 main.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-main.srcs += downlink.c pprz_transport.c
+main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 main.srcs +=  $(SRC_ARCH)/dc_mc_link.c
 main.srcs +=  $(SRC_ARCH)/dc_mc_power.c
diff --git a/conf/airframes/obsolete/easystar2.xml b/conf/airframes/obsolete/easystar2.xml
index 49e6fc9c75..cbda2b48f6 100644
--- a/conf/airframes/obsolete/easystar2.xml
+++ b/conf/airframes/obsolete/easystar2.xml
@@ -47,8 +47,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(40)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="40" unit="deg"/>
   </section>
 
   <section name="adc" prefix="ADC_CHANNEL_">
@@ -131,9 +131,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.7"/>
-    <define name="ROLL_MAX_SETPOINT" value="RadOfDeg(35)" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="RadOfDeg(25)" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="RadOfDeg(-25)" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="35" unit="deg"/>
+    <define name="PITCH_MAX_SETPOINT" value="25" unit="deg"/>
+    <define name="PITCH_MIN_SETPOINT" value="-25" unit="deg"/>
     <define name="PITCH_PGAIN" value="20000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
     <define name="ELEVATOR_OF_ROLL" value="2500"/>
@@ -160,8 +160,8 @@
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
     <define name="DEFAULT_THROTTLE" value="0.4" unit="%"/>
-    <define name="DEFAULT_ROLL" value="RadOfDeg(15)" unit="rad"/>
-    <define name="DEFAULT_PITCH" value="RadOfDeg(0)" unit="rad"/>
+    <define name="DEFAULT_ROLL" value="15" unit="deg"/>
+    <define name="DEFAULT_PITCH" value="0" unit="deg"/>
     <define name="HOME_RADIUS" value="90" unit="m"/>
   </section>
 
@@ -193,7 +193,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # Telemetry configuration
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/example.xml b/conf/airframes/obsolete/example.xml
index e225a43a4d..e92a3e3f4a 100644
--- a/conf/airframes/obsolete/example.xml
+++ b/conf/airframes/obsolete/example.xml
@@ -28,7 +28,7 @@ example_gps.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 example_gps.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 example_gps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-example_gps.srcs += downlink.c pprz_transport.c
+example_gps.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 example_gps.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
 example_gps.CFLAGS += -DUSE_GPS -DUBX -DGPS_LINK=Uart0 -DDOWNLINK_GPS_DEVICE=Uart0 -DGPS_BAUD=38400
diff --git a/conf/airframes/obsolete/hitl_usb.xml b/conf/airframes/obsolete/hitl_usb.xml
index 8fb94ecbd6..b2d5a21c1b 100644
--- a/conf/airframes/obsolete/hitl_usb.xml
+++ b/conf/airframes/obsolete/hitl_usb.xml
@@ -40,9 +40,6 @@
 
 	<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-	<define name="GLIDE_AIRSPEED" value="10"/>
-	<define name="GLIDE_VSPEED" value="3."/>
-	<define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -70,9 +67,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
 	<define name="COURSE_PGAIN" value="1.10800004005"/>
 
-	<define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-	<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-	<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+	<define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+	<define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+	<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 	<define name="PITCH_PGAIN" value="15184.5644531"/>
 	<define name="PITCH_DGAIN" value="1.5"/>
@@ -122,14 +119,14 @@ ap.srcs += commands.c
 # Through USB
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
 ap.CFLAGS += -DDOWNLINK_FBW_DEVICE=UsbS -DDOWNLINK_AP_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-ap.srcs += pprz_transport.c
+ap.srcs += subsystems/datalink/pprz_transport.c
 
 #XBEE ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=XBeeTransport -DUSE_USB_SERIAL
 #XBEE ap.CFLAGS += -DDOWNLINK_FBW_DEVICE=UsbS -DDOWNLINK_AP_DEVICE=UsbS -DXBEE_UART=UsbS -DDATALINK=XBEE
-#XBEE ap.srcs += xbee.c
+#XBEE ap.srcs += subsystems/datalink/xbee.c
 
 
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/usb_ser_hw.c datalink.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/usb_ser_hw.c datalink.c
 ap.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
 
@@ -155,7 +152,7 @@ ap.CFLAGS += -DGPS_USE_LATLONG # simulating LEA 5H
 # Config for SITL simulation
 sim.ARCHDIR = $(ARCH)
 sim.CFLAGS += -DSITL -DAP -DFBW -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED
-sim.srcs = latlong.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
+sim.srcs = latlong.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 sim.srcs += subsystems/navigation/nav_survey_rectangle.c subsystems/navigation/nav_line.c subsystems/navigation/traffic_info.c
 
diff --git a/conf/airframes/obsolete/kalscott_easystar.xml b/conf/airframes/obsolete/kalscott_easystar.xml
index bb655ba791..8fc7177256 100644
--- a/conf/airframes/obsolete/kalscott_easystar.xml
+++ b/conf/airframes/obsolete/kalscott_easystar.xml
@@ -34,8 +34,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(50)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(40)"/>
+    <define name="MAX_ROLL" value="50" unit="deg"/>
+    <define name="MAX_PITCH" value="40" unit="deg"/>
   </section>
 
   <section name="adc" prefix="ADC_CHANNEL_">
@@ -111,9 +111,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.7"/>
-    <define name="ROLL_MAX_SETPOINT" value="RadOfDeg(35)" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="RadOfDeg(25)" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="RadOfDeg(-25)" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="35" unit="deg"/>
+    <define name="PITCH_MAX_SETPOINT" value="25" unit="deg"/>
+    <define name="PITCH_MIN_SETPOINT" value="-25" unit="deg"/>
     <define name="PITCH_PGAIN" value="20000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
     <define name="ELEVATOR_OF_ROLL" value="2500"/>
@@ -140,8 +140,8 @@
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
     <define name="DEFAULT_THROTTLE" value="0.4" unit="%"/>
-    <define name="DEFAULT_ROLL" value="RadOfDeg(15)" unit="rad"/>
-    <define name="DEFAULT_PITCH" value="RadOfDeg(0)" unit="rad"/>
+    <define name="DEFAULT_ROLL" value="15" unit="deg"/>
+    <define name="DEFAULT_PITCH" value="0" unit="deg"/>
     <define name="HOME_RADIUS" value="90" unit="m"/>
   </section>
 
@@ -173,7 +173,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # Telemetry configuration
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/malolo_sim.xml b/conf/airframes/obsolete/malolo_sim.xml
index 79b0daece4..9bde735206 100644
--- a/conf/airframes/obsolete/malolo_sim.xml
+++ b/conf/airframes/obsolete/malolo_sim.xml
@@ -140,9 +140,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="10000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -233,7 +233,7 @@ sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
 sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV
 
 sim.srcs = $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/ivy_transport.c
-sim.srcs += latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c
+sim.srcs += latlong.c radio_control.c subsystems/datalink/downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c
 sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c
 
 sim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c
diff --git a/conf/airframes/obsolete/microjet5.xml b/conf/airframes/obsolete/microjet5.xml
index 6ac052ccb9..e6390f07fd 100644
--- a/conf/airframes/obsolete/microjet5.xml
+++ b/conf/airframes/obsolete/microjet5.xml
@@ -137,9 +137,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.8"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="10000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -187,7 +187,7 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/microjet5_tp_auto.xml b/conf/airframes/obsolete/microjet5_tp_auto.xml
index 628cde47ea..959828e6a7 100644
--- a/conf/airframes/obsolete/microjet5_tp_auto.xml
+++ b/conf/airframes/obsolete/microjet5_tp_auto.xml
@@ -97,9 +97,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.4"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_ATTITUDE_GAIN" value="1500"/>
@@ -192,10 +192,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c latlong.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c latlong.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/microjet6.xml b/conf/airframes/obsolete/microjet6.xml
index a5429418f5..cbce102e34 100644
--- a/conf/airframes/obsolete/microjet6.xml
+++ b/conf/airframes/obsolete/microjet6.xml
@@ -99,9 +99,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -135,9 +132,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.55"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="5000."/>
@@ -213,11 +210,11 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # API mode
 # ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B57600
-# ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+# ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 # transparent
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/microjetI.xml b/conf/airframes/obsolete/microjetI.xml
index 101e692925..1e816a3d29 100644
--- a/conf/airframes/obsolete/microjetI.xml
+++ b/conf/airframes/obsolete/microjetI.xml
@@ -96,9 +96,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="external leds">
@@ -139,9 +136,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.4"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="12500."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -200,11 +197,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B19200
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 # transparent
 # ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-# ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+# ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/microjetII.xml b/conf/airframes/obsolete/microjetII.xml
index b6b4dd62a9..f522378c6c 100644
--- a/conf/airframes/obsolete/microjetII.xml
+++ b/conf/airframes/obsolete/microjetII.xml
@@ -97,9 +97,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="external leds">
@@ -140,9 +137,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.25"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="9500."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -200,11 +197,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B19200
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 # transparent
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/microjetIII.xml b/conf/airframes/obsolete/microjetIII.xml
index 30a2278716..89c5e2b519 100644
--- a/conf/airframes/obsolete/microjetIII.xml
+++ b/conf/airframes/obsolete/microjetIII.xml
@@ -97,9 +97,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -127,9 +124,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.4"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="12000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -189,11 +186,11 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 # transparent
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/microjet_raw_makefile.xml b/conf/airframes/obsolete/microjet_raw_makefile.xml
index eccb995e13..4de83c91ff 100644
--- a/conf/airframes/obsolete/microjet_raw_makefile.xml
+++ b/conf/airframes/obsolete/microjet_raw_makefile.xml
@@ -142,9 +142,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.8"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="10000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -189,7 +189,7 @@
  ap.CFLAGS += -DRADIO_CONTROL
  ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
  ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
- ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
  ap.CFLAGS += -DINTER_MCU
  ap.srcs += inter_mcu.c
  ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3
diff --git a/conf/airframes/obsolete/minimag1.xml b/conf/airframes/obsolete/minimag1.xml
index 35554734cc..e047ab17d6 100644
--- a/conf/airframes/obsolete/minimag1.xml
+++ b/conf/airframes/obsolete/minimag1.xml
@@ -129,9 +129,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.2"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="12000."/>
@@ -188,7 +188,7 @@ ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/minimag_fs.xml b/conf/airframes/obsolete/minimag_fs.xml
index 4aa25d85d5..5fe2a47676 100644
--- a/conf/airframes/obsolete/minimag_fs.xml
+++ b/conf/airframes/obsolete/minimag_fs.xml
@@ -143,9 +143,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.4"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="6000."/>
@@ -195,7 +195,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/mmlaas_N1_carto_cam.xml b/conf/airframes/obsolete/mmlaas_N1_carto_cam.xml
index 5f17de74f1..be58d5e523 100644
--- a/conf/airframes/obsolete/mmlaas_N1_carto_cam.xml
+++ b/conf/airframes/obsolete/mmlaas_N1_carto_cam.xml
@@ -128,9 +128,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="10000."/>
@@ -211,7 +211,7 @@ ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/uart_hw.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/nova.xml b/conf/airframes/obsolete/nova.xml
index 256c6ae89e..ea7eac180d 100644
--- a/conf/airframes/obsolete/nova.xml
+++ b/conf/airframes/obsolete/nova.xml
@@ -14,7 +14,7 @@ timu.srcs = nova_test_imu.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/arm
 timu.CFLAGS += -DUSE_LED  -DUSE_LED -DTIME_LED=1
 
 timu.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-timu.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c pprz_transport.c
+timu.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c subsystems/datalink/pprz_transport.c
 
 timu.srcs += $(SRC_ARCH)/ADS8344.c
 
diff --git a/conf/airframes/obsolete/osam_xsens_twog.xml b/conf/airframes/obsolete/osam_xsens_twog.xml
index b92359fb40..425ae845d6 100644
--- a/conf/airframes/obsolete/osam_xsens_twog.xml
+++ b/conf/airframes/obsolete/osam_xsens_twog.xml
@@ -81,8 +81,8 @@
   </command_laws>
 
   <section name="AUTO1" prefix="AUTO1_">
-    <define name="MAX_ROLL" value="RadOfDeg(45)"/>
-    <define name="MAX_PITCH" value="RadOfDeg(45)"/>
+    <define name="MAX_ROLL" value="45" unit="deg"/>
+    <define name="MAX_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="adc" prefix="ADC_CHANNEL_">
@@ -167,9 +167,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.296"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="4639.1"/>
     <define name="PITCH_DGAIN" value="0"/>
diff --git a/conf/airframes/obsolete/slayer1.xml b/conf/airframes/obsolete/slayer1.xml
index ea87e3b03f..a097cfc2da 100644
--- a/conf/airframes/obsolete/slayer1.xml
+++ b/conf/airframes/obsolete/slayer1.xml
@@ -133,9 +133,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.88"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="10000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -204,10 +204,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600 -DXBEE868
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c latlong.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c latlong.c subsystems/datalink/xbee.c
 
 #transp ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-#transp ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#transp ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/slayer3.xml b/conf/airframes/obsolete/slayer3.xml
index c5e96ef9e5..62c18d9fe3 100644
--- a/conf/airframes/obsolete/slayer3.xml
+++ b/conf/airframes/obsolete/slayer3.xml
@@ -133,9 +133,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.88"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="10000."/>
@@ -206,10 +206,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT   ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-#TRANSPARENT ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#TRANSPARENT ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/slayerJH.xml b/conf/airframes/obsolete/slayerJH.xml
index b1598cca8b..e1a8f7255b 100644
--- a/conf/airframes/obsolete/slayerJH.xml
+++ b/conf/airframes/obsolete/slayerJH.xml
@@ -123,9 +123,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.88"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
     <define name="PITCH_PGAIN" value="6000."/>
     <define name="PITCH_DGAIN" value="0."/>
@@ -188,11 +188,11 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #MAXSTREAM
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 #TRANSPARENT
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/slicer1.xml b/conf/airframes/obsolete/slicer1.xml
index a25ccd7404..163152da50 100644
--- a/conf/airframes/obsolete/slicer1.xml
+++ b/conf/airframes/obsolete/slicer1.xml
@@ -137,9 +137,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.3"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="10000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -169,15 +169,15 @@
   </section>
 
   <section name="CAM" prefix="CAM_">
-    <define name="TILT_MAX"      value="30" unit="deg"/>
-    <define name="TILT_NEUTRAL"  value="0" unit="deg"/>
-    <define name="TILT_MIN"      value="-30"  unit="deg"/>
-    <define name="TILT0"        value="0" unit="deg"/>
+    <define name="TILT_MAX"      value="30" unit="deg" code_unit="deg"/>
+    <define name="TILT_NEUTRAL"  value="0" unit="deg" code_unit="deg"/>
+    <define name="TILT_MIN"      value="-30"  unit="deg" code_unit="deg"/>
+    <define name="TILT0"        value="0" unit="deg" code_unit="deg"/>
 
-    <define name="PAN_MAX"     value="45" unit="deg"/>
-    <define name="PAN_NEUTRAL" value="0" unit="deg"/>
-    <define name="PAN_MIN"     value="-45"  unit="deg"/>
-    <define name="PAN0"          value="0" unit="deg"/>
+    <define name="PAN_MAX"     value="45" unit="deg" code_unit="deg"/>
+    <define name="PAN_NEUTRAL" value="0" unit="deg" code_unit="deg"/>
+    <define name="PAN_MIN"     value="-45"  unit="deg" code_unit="deg"/>
+    <define name="PAN0"          value="0" unit="deg" code_unit="deg"/>
   </section>
 
  <makefile>
@@ -200,7 +200,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # Radiotronix
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/slicer2.xml b/conf/airframes/obsolete/slicer2.xml
index 5c2e05f245..c8798feae4 100644
--- a/conf/airframes/obsolete/slicer2.xml
+++ b/conf/airframes/obsolete/slicer2.xml
@@ -131,9 +131,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.3"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="10000."/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -182,7 +182,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # Radiotronix
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/spirit.xml b/conf/airframes/obsolete/spirit.xml
index 11058c5bcb..d3804aaf2a 100644
--- a/conf/airframes/obsolete/spirit.xml
+++ b/conf/airframes/obsolete/spirit.xml
@@ -106,9 +106,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -136,9 +133,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.499000012875"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="19703.390625"/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -199,7 +196,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # transparent
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/spirit_proto.xml b/conf/airframes/obsolete/spirit_proto.xml
index 9792785325..9ae2acdc93 100644
--- a/conf/airframes/obsolete/spirit_proto.xml
+++ b/conf/airframes/obsolete/spirit_proto.xml
@@ -95,9 +95,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -125,9 +122,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.499000012875"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="19703.390625"/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -187,7 +184,7 @@ ap.srcs += poradio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # transparent
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/storm1.xml b/conf/airframes/obsolete/storm1.xml
index 66a7d0b18d..3ada34c50a 100644
--- a/conf/airframes/obsolete/storm1.xml
+++ b/conf/airframes/obsolete/storm1.xml
@@ -122,9 +122,6 @@
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="12.5"/>
-    <define name="GLIDE_VSPEED" value="2."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
 
   </section>
 
@@ -159,9 +156,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.2"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="7000"/>
@@ -241,17 +238,17 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -#DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B57600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 
 ap.CFLAGS += -DTELEMETRY_MODE_FBW=1
 
 #TRANSPARENT   ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-#TRANSPARENT ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#TRANSPARENT ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/tiny2.xml b/conf/airframes/obsolete/tiny2.xml
index 50f6823029..8003be219f 100644
--- a/conf/airframes/obsolete/tiny2.xml
+++ b/conf/airframes/obsolete/tiny2.xml
@@ -128,9 +128,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.2"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="12000."/>
@@ -211,10 +211,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 #XBEE ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DXBEE_UART=Uart1 -DDATALINK=XBEE -DUART1_BAUD=B57600
-#XBEE ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+#XBEE ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/tiny_hitl.xml b/conf/airframes/obsolete/tiny_hitl.xml
index 456d46fa6c..c3f70f7f3b 100644
--- a/conf/airframes/obsolete/tiny_hitl.xml
+++ b/conf/airframes/obsolete/tiny_hitl.xml
@@ -91,9 +91,6 @@
 
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
 
-    <define name="GLIDE_AIRSPEED" value="10"/>
-    <define name="GLIDE_VSPEED" value="3."/>
-    <define name="GLIDE_PITCH" value="45" unit="deg"/>
   </section>
 
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
@@ -121,9 +118,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.10800004005"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="15184.5644531"/>
     <define name="PITCH_DGAIN" value="1.5"/>
@@ -179,7 +176,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # transparent
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU
 ap.srcs += inter_mcu.c
diff --git a/conf/airframes/obsolete/tl.xml b/conf/airframes/obsolete/tl.xml
index 9412650b8c..6a77eca47f 100644
--- a/conf/airframes/obsolete/tl.xml
+++ b/conf/airframes/obsolete/tl.xml
@@ -101,7 +101,7 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # transparent
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c pprz_transport.c $(TL)/tl_telemetry.c datalink.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c subsystems/datalink/pprz_transport.c $(TL)/tl_telemetry.c datalink.c
 
 ap.CFLAGS += -DTL_AP_MODE_AUTO=TL_AP_MODE_NAV -DDT_VFILTER="(1./60.)"
 ap.srcs += $(TL)/tl_autopilot.c $(TL)/tl_control.c $(TL)/tl_estimator.c $(TL)/tl_vfilter.c
@@ -135,7 +135,7 @@ tmm.srcs = $(TL)/tl_test_mm_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_AR
 
 tmm.CFLAGS += -DUSE_LED  -DUSE_LED -DTIME_LED=1
 tmm.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DUART0_BAUD=B57600
-tmm.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c pprz_transport.c
+tmm.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c subsystems/datalink/pprz_transport.c
 
 tmm.srcs += micromag.c $(SRC_ARCH)/micromag_hw.c
 
@@ -150,7 +150,7 @@ timu.srcs = $(TL)/tl_test_imu_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_
 
 timu.CFLAGS += -DUSE_LED  -DUSE_LED -DTIME_LED=1
 timu.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DUART0_BAUD=B57600
-timu.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c pprz_transport.c
+timu.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c subsystems/datalink/pprz_transport.c
 
 timu.srcs += $(TL)/tl_imu.c
 timu.CFLAGS += -DADC -DUSE_ADC_5 -DADC_CHANNEL_GR=ADC_5 -DUSE_ADC_6 -DADC_CHANNEL_RM=ADC_6 -DUSE_ADC_7 -DADC_CHANNEL_ACCEL=ADC_7
diff --git a/conf/airframes/obsolete/twinjet1.xml b/conf/airframes/obsolete/twinjet1.xml
index 8299709104..b9d9f5b702 100644
--- a/conf/airframes/obsolete/twinjet1.xml
+++ b/conf/airframes/obsolete/twinjet1.xml
@@ -82,9 +82,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.53" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.53" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="6000."/>
     <define name="PITCH_DGAIN" value="0."/>
@@ -184,7 +184,7 @@ fbw.CFLAGS += -DRADIO_CONTROL
 fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 fbw.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DUART0_BAUD=B9600
-fbw.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/mcu_periph/uart_arch.c
+fbw.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE
 fbw.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
@@ -197,10 +197,10 @@ ap.CFLAGS += -DAP -DRADIO_CONTROL_AUTO1 -DBOARD_CONFIG=\"classix.h\" -DUSE_LED -
 ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_ap.c main.c
 
 #ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-#ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/navigation/traffic_info.c xbee.c
+#ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/navigation/traffic_info.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0  -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0
 ap.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
@@ -238,7 +238,7 @@ ap.CFLAGS += -DUSE_LIGHT
 
 
 test.CFLAGS += -DFBW -DBOARD_CONFIG=\"classix.h\" -DUSE_LED -DTIME_LED=1 -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING  -DUSE_UART0 -DDATALINK -DPPRZ_INPUT -DPPRZ_UART=Uart0 -DUART0_BAUD=B9600
-test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4017_hw.c main.c
+test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c subsystems/datalink/pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4017_hw.c main.c
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
diff --git a/conf/airframes/obsolete/twinstar1.xml b/conf/airframes/obsolete/twinstar1.xml
index 059d460af1..725d44586c 100644
--- a/conf/airframes/obsolete/twinstar1.xml
+++ b/conf/airframes/obsolete/twinstar1.xml
@@ -84,9 +84,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.4"/>
     <define name="COURSE_PRE_BANK_CORRECTION" value="1."/>
-    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.73" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="10000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
@@ -182,7 +182,7 @@ ap.CFLAGS += -DHITL
 
 # Downlink on uart0
 ap.CFLAGS += -DUSE_UART0 -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0 -DUART0_BAUD=B38400
-ap.EXTRA_SRCS += pprz_transport.c downlink.c
+ap.EXTRA_SRCS += subsystems/datalink/pprz_transport.c subsystems/datalink/downlink.c
 
 # Datalink (on uart0) used to emulate sensors (infrared and gps)
 ap.EXTRA_SRCS += subsystems/navigation/traffic_info.c datalink.c
diff --git a/conf/airframes/obsolete/twinstar6.xml b/conf/airframes/obsolete/twinstar6.xml
index 78284c2386..515270a9d7 100644
--- a/conf/airframes/obsolete/twinstar6.xml
+++ b/conf/airframes/obsolete/twinstar6.xml
@@ -192,7 +192,7 @@ fbw.CFLAGS += -DRADIO_CONTROL
 fbw.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 fbw.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DUART0_BAUD=B9600
-fbw.srcs += downlink.c pprz_transport.c $(SRC_ARCH)/mcu_periph/uart_arch.c
+fbw.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 fbw.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_SLAVE
 fbw.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
@@ -205,7 +205,7 @@ ap.CFLAGS += -DAP -DRADIO_CONTROL_AUTO1 -DBOARD_CONFIG=\"classix.h\" -DUSE_LED -
 ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_ap.c main.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_AP_DEVICE=Uart0  -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/navigation/traffic_info.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/navigation/traffic_info.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DINTER_MCU -DMCU_SPI_LINK -DUSE_SPI -DSPI_MASTER
 ap.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c
@@ -238,7 +238,7 @@ ap.srcs += dc.c
 
 
 test.CFLAGS += -DFBW -DBOARD_CONFIG=\"classix.h\" -DUSE_LED -DTIME_LED=1 -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING  -DUSE_UART0 -DDATALINK -DPPRZ_INPUT -DPPRZ_UART=Uart0 -DUART0_BAUD=B9600
-test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4017_hw.c main.c
+test.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c subsystems/datalink/pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4017_hw.c main.c
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
diff --git a/conf/airframes/obsolete/twisted.xml b/conf/airframes/obsolete/twisted.xml
index 19ebf649b7..d5c3433c06 100644
--- a/conf/airframes/obsolete/twisted.xml
+++ b/conf/airframes/obsolete/twisted.xml
@@ -56,7 +56,7 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B19200
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DUSE_BARO_MS5534A
 ap.srcs += spi.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/baro_MS5534A.c
diff --git a/conf/airframes/obsolete/tyto1.xml b/conf/airframes/obsolete/tyto1.xml
index a9c94d1bef..9f77189137 100644
--- a/conf/airframes/obsolete/tyto1.xml
+++ b/conf/airframes/obsolete/tyto1.xml
@@ -147,9 +147,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.23"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.4" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.4" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
 
@@ -216,10 +216,10 @@ ap.CFLAGS += -DRADIO_CONTROL
 ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-# ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c xbee.c
+# ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/xbee.c
 
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
diff --git a/conf/airframes/obsolete/vor.xml b/conf/airframes/obsolete/vor.xml
index 57f2edc770..2405679172 100644
--- a/conf/airframes/obsolete/vor.xml
+++ b/conf/airframes/obsolete/vor.xml
@@ -24,7 +24,7 @@ main.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B9600
 main.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 main.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-main.srcs += downlink.c pprz_transport.c
+main.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 main.srcs += firmwares/vor/vor_int_demod_decim.c
 
diff --git a/conf/airframes/obsolete/xxx1.xml b/conf/airframes/obsolete/xxx1.xml
index 854df9d9c7..29c376bf5b 100644
--- a/conf/airframes/obsolete/xxx1.xml
+++ b/conf/airframes/obsolete/xxx1.xml
@@ -107,7 +107,7 @@
     <define name="UNLOCKED_HOME_MODE" value="TRUE"/>
     <define name="RC_LOST_MODE" value="PPRZ_MODE_AUTO2"/>
 
-    <define name="CAM_PHI_MAX_DEG" value="45"/>
+    <define name="CAM_PHI_MAX" value="45" unit="deg"/>
     <define name="VIDEO_SWITCH_PIN" value="20"/>
   </section>
 
@@ -142,9 +142,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.75"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.4" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="rad"/>
 
 
     <define name="ROLL_PGAIN" value="13500."/>
@@ -222,10 +222,10 @@ ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
 
 # ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
 ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B57600
-ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 #TRANSPARENT   ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600
-#TRANSPARENT ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c
+#TRANSPARENT ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c
 
 
 ap.CFLAGS += -DINTER_MCU
@@ -279,7 +279,7 @@ sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_re
 setup_actuators.ARCHDIR = $(ARCHI)
 
 setup_actuators.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny.h\" -DUSE_LED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
-setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4015_hw.c main.c
+setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c subsystems/datalink/pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4015_hw.c main.c
 
   </makefile>
 </airframe>
diff --git a/conf/airframes/test_hb.xml b/conf/airframes/test_hb.xml
index 8d807a24e5..7026a0513d 100644
--- a/conf/airframes/test_hb.xml
+++ b/conf/airframes/test_hb.xml
@@ -166,9 +166,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.70" unit="rad"/> <!-- 0.5 -->
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/test_i2c.xml b/conf/airframes/test_i2c.xml
index b7546315d2..4da9eb0452 100644
--- a/conf/airframes/test_i2c.xml
+++ b/conf/airframes/test_i2c.xml
@@ -20,7 +20,7 @@ mb.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 mb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 mb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-mb.srcs += downlink.c pprz_transport.c
+mb.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 mb.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 
diff --git a/conf/airframes/twinjet_example.xml b/conf/airframes/twinjet_example.xml
index a7ef747dc9..a4bd629534 100644
--- a/conf/airframes/twinjet_example.xml
+++ b/conf/airframes/twinjet_example.xml
@@ -110,9 +110,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.53" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.53" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="6000."/>
     <define name="PITCH_DGAIN" value="0."/>
diff --git a/conf/airframes/twinjet_overo.xml b/conf/airframes/twinjet_overo.xml
index 725c6ec21e..90a9b62cb5 100644
--- a/conf/airframes/twinjet_overo.xml
+++ b/conf/airframes/twinjet_overo.xml
@@ -112,9 +112,9 @@
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.9"/>
 
-    <define name="ROLL_MAX_SETPOINT" value="0.53" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.53" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="PITCH_PGAIN" value="6000."/>
     <define name="PITCH_DGAIN" value="0."/>
diff --git a/conf/airframes/twinstar_example.xml b/conf/airframes/twinstar_example.xml
index a8400c985a..09d2582cee 100644
--- a/conf/airframes/twinstar_example.xml
+++ b/conf/airframes/twinstar_example.xml
@@ -165,9 +165,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="1.0"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.60" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="6000."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/usb_test.xml b/conf/airframes/usb_test.xml
index ffebe301d3..3946645082 100644
--- a/conf/airframes/usb_test.xml
+++ b/conf/airframes/usb_test.xml
@@ -145,9 +145,9 @@
 
   <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
     <define name="COURSE_PGAIN" value="0.8"/>
-    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="radians"/>
-    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
-    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
+    <define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
+    <define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
+    <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
 
     <define name="ROLL_PGAIN" value="7600."/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
diff --git a/conf/airframes/wind_tunnel.xml b/conf/airframes/wind_tunnel.xml
index 9c4df1af93..fbbe16b179 100644
--- a/conf/airframes/wind_tunnel.xml
+++ b/conf/airframes/wind_tunnel.xml
@@ -25,7 +25,7 @@ ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-ap.srcs += downlink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 ap.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 #ap.srcs += $(BOOZ)/booz_datalink.c
@@ -55,7 +55,7 @@ mb.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 mb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 mb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-mb.srcs += downlink.c pprz_transport.c
+mb.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 mb.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart0
 
diff --git a/conf/autopilot/booz2_test_progs.makefile b/conf/autopilot/booz2_test_progs.makefile
index 05fddfbcc9..42cb274d0c 100644
--- a/conf/autopilot/booz2_test_progs.makefile
+++ b/conf/autopilot/booz2_test_progs.makefile
@@ -43,7 +43,7 @@ test_downlink.ARCHDIR = $(ARCH)
 
 test_downlink.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) $(BOOZ_CFLAGS)
 test_downlink.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_downlink.srcs   += $(SRC_BOOZ_TEST)/booz2_test_downlink.c
+test_downlink.srcs   += $(SRC_BOOZ_TEST)/booz2_test_subsystems/datalink/downlink.c
 test_downlink.CFLAGS += -DUSE_LED
 test_downlink.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./10.))' -DTIME_LED=1
 test_downlink.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
@@ -53,7 +53,7 @@ test_downlink.srcs   += mcu_periph/uart.c
 test_downlink.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_downlink.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_downlink.srcs   += downlink.c pprz_transport.c
+test_downlink.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 
 #
@@ -73,7 +73,7 @@ test_max1168.srcs   += mcu_periph/uart.c
 test_max1168.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_max1168.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_max1168.srcs   += downlink.c pprz_transport.c
+test_max1168.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_max1168.CFLAGS += -DMAX1168_EOC_VIC_SLOT=8 -DSSP_VIC_SLOT=9
 test_max1168.srcs   += peripherals/max1168.c \
@@ -95,7 +95,7 @@ test_micromag.srcs += mcu_periph/uart.c
 test_micromag.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_micromag.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_micromag.srcs += downlink.c pprz_transport.c
+test_micromag.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 #test_micromag.CFLAGS += -I$(BOOZ)
 #test_micromag.srcs += pprz_debug.c
@@ -175,7 +175,7 @@ test_gps.srcs += mcu_periph/uart.c
 test_gps.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_gps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_gps.srcs += downlink.c pprz_transport.c
+test_gps.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_gps.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 test_gps.CFLAGS += -DGPS_LINK=Uart0 -DGPS_LED=2
@@ -201,7 +201,7 @@ test_modem.srcs += mcu_periph/uart.c
 test_modem.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_modem.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_modem.srcs += downlink.c pprz_transport.c
+test_modem.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 #test_modem.CFLAGS += -DBOOZ_ANALOG_BARO_LED=2 -DBOOZ_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))'
 #test_modem.srcs += $(BOOZ_PRIV)/booz_analog_baro.c
@@ -224,11 +224,11 @@ test_usb.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
 #test_usb.srcs += mcu_periph/uart.c
 #test_usb.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #test_usb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-#test_usb.srcs += downlink.c pprz_transport.c
+#test_usb.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_usb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
 test_usb.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-test_usb.srcs += downlink.c mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/usb_ser_hw.c pprz_transport.c
+test_usb.srcs += subsystems/datalink/downlink.c mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c
 # $(SRC_FIRMWARE)/datalink.c
 test_usb.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 test_usb.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
@@ -254,7 +254,7 @@ test_ami.srcs += mcu_periph/uart.c
 test_ami.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_ami.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_ami.srcs += downlink.c pprz_transport.c
+test_ami.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_ami.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
 test_ami.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
@@ -278,7 +278,7 @@ test_crista.srcs += mcu_periph/uart.c
 test_crista.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_crista.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0
-test_crista.srcs += downlink.c pprz_transport.c
+test_crista.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_crista.CFLAGS += -DFLOAT_T=float -DBOOZ2_IMU_TYPE=\"booz2_imu_crista.h\"
 test_crista.srcs += $(SRC_BOOZ)/booz2_imu.c
@@ -305,7 +305,7 @@ test_micromag2.srcs += mcu_periph/uart.c
 test_micromag2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_micromag2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_micromag2.srcs += downlink.c pprz_transport.c
+test_micromag2.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 
 
@@ -328,7 +328,7 @@ test_imu_b2.srcs += mcu_periph/uart.c
 test_imu_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_imu_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_imu_b2.srcs += downlink.c pprz_transport.c
+test_imu_b2.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_imu_b2.srcs += $(SRC_BOOZ)/booz_trig_int.c
 
@@ -358,10 +358,10 @@ test_rc_spektrum.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC
 #test_rc_spektrum.srcs += mcu_periph/uart.c
 #test_rc_spektrum.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-#test_rc_spektrum.srcs += downlink.c pprz_transport.c
+#test_rc_spektrum.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
 test_rc_spektrum.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-test_rc_spektrum.srcs += downlink.c $(SRC_ARCH)/usb_ser_hw.c pprz_transport.c
+test_rc_spektrum.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c
 test_rc_spektrum.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 test_rc_spektrum.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
 
@@ -391,10 +391,10 @@ test_rc_ppm.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 #test_rc_ppm.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
 #test_rc_ppm.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #test_rc_ppm.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-#test_rc_ppm.srcs += downlink.c pprz_transport.c
+#test_rc_ppm.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_rc_ppm.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_USB_SERIAL
 test_rc_ppm.CFLAGS += -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS -DDATALINK=PPRZ
-test_rc_ppm.srcs += downlink.c $(SRC_ARCH)/usb_ser_hw.c pprz_transport.c
+test_rc_ppm.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/usb_ser_hw.c subsystems/datalink/pprz_transport.c
 test_rc_ppm.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 test_rc_ppm.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
 
@@ -422,7 +422,7 @@ test_mc.srcs += mcu_periph/uart.c
 test_mc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_mc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_mc.srcs += downlink.c pprz_transport.c
+test_mc.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_mc.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
 test_mc.srcs += $(SRC_BOOZ_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c
@@ -448,7 +448,7 @@ test_buss_bldc.srcs += mcu_periph/uart.c
 test_buss_bldc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_buss_bldc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_buss_bldc.srcs += downlink.c pprz_transport.c
+test_buss_bldc.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_buss_bldc.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
 test_buss_bldc.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
@@ -471,7 +471,7 @@ test_amc.srcs += mcu_periph/uart.c
 test_amc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_amc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_amc.srcs += downlink.c pprz_transport.c
+test_amc.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_amc.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
 test_amc.srcs += $(SRC_FIRMWARE)/datalink.c
 
@@ -517,7 +517,7 @@ test_baro_24.srcs += mcu_periph/uart.c
 test_baro_24.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_baro_24.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_baro_24.srcs += downlink.c pprz_transport.c
+test_baro_24.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_baro_24.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11 -DI2C1_BUF_LEN=16
 test_baro_24.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile
index 1b5f29999b..cb065f7fc7 100644
--- a/conf/autopilot/lisa_l_test_progs.makefile
+++ b/conf/autopilot/lisa_l_test_progs.makefile
@@ -147,7 +147,7 @@ test_telemetry.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
 test_telemetry.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 test_telemetry.CFLAGS += -DUSE_$(MODEM_PORT)
 test_telemetry.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_telemetry.srcs += downlink.c pprz_transport.c
+test_telemetry.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_telemetry.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
 test_telemetry.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
@@ -175,7 +175,7 @@ test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
 test_baro.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
 test_baro.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_baro.srcs   += downlink.c pprz_transport.c
+test_baro.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_baro.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 test_baro.srcs   += mcu_periph/uart.c
 test_baro.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
@@ -214,7 +214,7 @@ test_rc_spektrum.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD
 test_rc_spektrum.srcs   += mcu_periph/uart.c
 test_rc_spektrum.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_rc_spektrum.srcs   += downlink.c pprz_transport.c
+test_rc_spektrum.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_rc_spektrum.CFLAGS += -DRADIO_CONTROL
 ifdef RADIO_CONTROL_LED
 test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
@@ -258,7 +258,7 @@ test_rc_ppm.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 test_rc_ppm.srcs   += mcu_periph/uart.c
 test_rc_ppm.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_rc_ppm.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_rc_ppm.srcs   += downlink.c pprz_transport.c
+test_rc_ppm.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_rc_ppm.CFLAGS += -DRADIO_CONTROL
 test_rc_ppm.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
 test_rc_ppm.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/ppm.h\"
@@ -300,7 +300,7 @@ test_adc.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_adc.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT)
 
 test_adc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_adc.srcs   += downlink.c pprz_transport.c
+test_adc.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_adc.srcs   += $(SRC_ARCH)/mcu_periph/adc_arch.c
 test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
@@ -335,7 +335,7 @@ COMMON_TEST_CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 COMMON_TEST_SRCS   += mcu_periph/uart.c
 COMMON_TEST_SRCS   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 COMMON_TEST_CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-COMMON_TEST_SRCS   += downlink.c pprz_transport.c
+COMMON_TEST_SRCS   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 COMMON_TEST_SRCS   += math/pprz_trig_int.c
 
 
@@ -392,7 +392,7 @@ test_imu_b2_2.srcs   += $(IMU_B2_2_SRCS)
 #
 # test IMU aspirin
 #
-IMU_ASPIRIN_CFLAGS = -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
+IMU_ASPIRIN_CFLAGS = -DIMU_TYPE_H=\"imu/imu_aspirin.h\"
 IMU_ASPIRIN_SRCS   = $(SRC_SUBSYSTEMS)/imu.c             \
                      $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
                      $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c
@@ -504,11 +504,10 @@ test_hmc5843.srcs += mcu_periph/uart.c
 test_hmc5843.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_hmc5843.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_hmc5843.srcs += downlink.c pprz_transport.c
+test_hmc5843.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_hmc5843.CFLAGS += -DUSE_I2C2
 test_hmc5843.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
-test_hmc5843.CFLAGS += -DIMU_OVERRIDE_CHANNELS
 test_hmc5843.CFLAGS += -DUSE_EXTI9_5_IRQ   # Mag Int on PB5
 
 
@@ -536,7 +535,7 @@ test_itg3200.srcs += mcu_periph/uart.c
 test_itg3200.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_itg3200.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_itg3200.srcs += downlink.c pprz_transport.c
+test_itg3200.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_itg3200.CFLAGS += -DUSE_I2C2
 test_itg3200.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
@@ -567,7 +566,7 @@ test_adxl345.srcs += mcu_periph/uart.c
 test_adxl345.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_adxl345.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_adxl345.srcs += downlink.c pprz_transport.c
+test_adxl345.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_adxl345.CFLAGS += -DUSE_EXTI2_IRQ   # Accel Int on PD2
 test_adxl345.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
@@ -639,7 +638,7 @@ test_actuators_mkk.srcs += mcu_periph/uart.c
 test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_actuators_mkk.srcs += downlink.c pprz_transport.c
+test_actuators_mkk.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c
 test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
@@ -672,7 +671,7 @@ test_actuators_asctecv1.srcs += mcu_periph/uart.c
 test_actuators_asctecv1.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_actuators_asctecv1.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_actuators_asctecv1.srcs += downlink.c pprz_transport.c
+test_actuators_asctecv1.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_actuators_asctecv1.srcs += $(SRC_FIRMWARE)/commands.c
 test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
@@ -703,11 +702,10 @@ test_bmp085.srcs += mcu_periph/uart.c
 test_bmp085.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_bmp085.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_bmp085.srcs += downlink.c pprz_transport.c
+test_bmp085.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_bmp085.CFLAGS += -DUSE_I2C2
 test_bmp085.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
-#test_bmp085.CFLAGS += -DIMU_OVERRIDE_CHANNELS
 #test_bmp085.CFLAGS += -DUSE_EXTI9_5_IRQ   # Mag Int on PB5
 
 
@@ -734,7 +732,7 @@ test_manual.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 test_manual.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 
 test_manual.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_manual.srcs   += downlink.c pprz_transport.c
+test_manual.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 
 test_manual.srcs += $(SRC_FIRMWARE)/commands.c
 
@@ -823,7 +821,7 @@ test_settings.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
 test_settings.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 test_settings.CFLAGS += -DUSE_$(MODEM_PORT)
 test_settings.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_settings.srcs   += downlink.c pprz_transport.c
+test_settings.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_settings.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
 test_settings.srcs   += mcu_periph/uart.c
 test_settings.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
diff --git a/conf/autopilot/lisa_m_test_progs.makefile b/conf/autopilot/lisa_m_test_progs.makefile
index e5c334f6f0..aa071e0b57 100644
--- a/conf/autopilot/lisa_m_test_progs.makefile
+++ b/conf/autopilot/lisa_m_test_progs.makefile
@@ -144,7 +144,7 @@ test_telemetry.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
 test_telemetry.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 test_telemetry.CFLAGS += -DUSE_$(MODEM_PORT)
 test_telemetry.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_telemetry.srcs += downlink.c pprz_transport.c
+test_telemetry.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_telemetry.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
 test_telemetry.srcs   += mcu_periph/uart.c
 test_telemetry.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
@@ -173,7 +173,7 @@ test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
 test_baro.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
 test_baro.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_baro.srcs   += downlink.c pprz_transport.c
+test_baro.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_baro.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 test_baro.srcs   += mcu_periph/uart.c
 test_baro.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
@@ -212,7 +212,7 @@ test_rc_spektrum.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD
 test_rc_spektrum.srcs   += mcu_periph/uart.c
 test_rc_spektrum.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_rc_spektrum.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_rc_spektrum.srcs   += downlink.c pprz_transport.c
+test_rc_spektrum.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_rc_spektrum.CFLAGS += -DRADIO_CONTROL
 ifdef RADIO_CONTROL_LED
 test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
@@ -258,7 +258,7 @@ test_rc_spektrum.srcs   += $(SRC_SUBSYSTEMS)/radio_control.c
 #test_rc_ppm.srcs   += mcu_periph/uart.c
 #test_rc_ppm.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #test_rc_ppm.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-#test_rc_ppm.srcs   += downlink.c pprz_transport.c
+#test_rc_ppm.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #test_rc_ppm.CFLAGS += -DRADIO_CONTROL
 #test_rc_ppm.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
 #test_rc_ppm.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/ppm.h\"
@@ -300,7 +300,7 @@ test_rc_spektrum.srcs   += $(SRC_SUBSYSTEMS)/radio_control.c
 #test_adc.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=$(MODEM_PORT)
 #
 #test_adc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-#test_adc.srcs   += downlink.c pprz_transport.c
+#test_adc.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_adc.srcs   += $(SRC_ARCH)/mcu_periph/adc_arch.c
 #test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
@@ -336,7 +336,7 @@ test_rc_spektrum.srcs   += $(SRC_SUBSYSTEMS)/radio_control.c
 #test_imu_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_imu_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-#test_imu_b2.srcs += downlink.c pprz_transport.c
+#test_imu_b2.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_imu_b2.srcs += math/pprz_trig_int.c
 #
@@ -380,7 +380,7 @@ test_rc_spektrum.srcs   += $(SRC_SUBSYSTEMS)/radio_control.c
 #test_imu_b2_2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_imu_b2_2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-#test_imu_b2_2.srcs += downlink.c pprz_transport.c
+#test_imu_b2_2.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_imu_b2_2.srcs += math/pprz_trig_int.c
 #
@@ -422,12 +422,12 @@ test_imu_aspirin.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
 test_imu_aspirin.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 test_imu_aspirin.CFLAGS += -DUSE_$(MODEM_PORT)
 test_imu_aspirin.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_imu_aspirin.srcs   += downlink.c pprz_transport.c
+test_imu_aspirin.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
 test_imu_aspirin.srcs   += mcu_periph/uart.c
 test_imu_aspirin.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 test_imu_aspirin.srcs   += math/pprz_trig_int.c
-test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
+test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_ASPIRIN_VERSION_1_0
 test_imu_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c             \
                         $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
                         $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c
@@ -439,7 +439,8 @@ test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ  # Gyro Int on PC14
 test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ    # Mag Int on PB5
 test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ      # Accel Int on PD2
 test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
-#
+
+
 ##
 ## test hmc5843
 ##
@@ -462,11 +463,10 @@ test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 #test_hmc5843.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_hmc5843.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-#test_hmc5843.srcs += downlink.c pprz_transport.c
+#test_hmc5843.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_hmc5843.CFLAGS += -DUSE_I2C2
 #test_hmc5843.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
-#test_hmc5843.CFLAGS += -DIMU_OVERRIDE_CHANNELS
 #test_hmc5843.CFLAGS += -DUSE_EXTI9_5_IRQ   # Mag Int on PB5
 #
 #
@@ -494,7 +494,7 @@ test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 #test_itg3200.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_itg3200.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-#test_itg3200.srcs += downlink.c pprz_transport.c
+#test_itg3200.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_itg3200.CFLAGS += -DUSE_I2C2
 #test_itg3200.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
@@ -525,7 +525,7 @@ test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 #test_adxl345.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_adxl345.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-#test_adxl345.srcs += downlink.c pprz_transport.c
+#test_adxl345.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_adxl345.CFLAGS += -DUSE_EXTI2_IRQ   # Accel Int on PD2
 #test_adxl345.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
@@ -597,7 +597,7 @@ test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 #test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-#test_actuators_mkk.srcs += downlink.c pprz_transport.c
+#test_actuators_mkk.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_actuators_mkk.srcs += $(SRC_BOOZ)/booz2_commands.c
 #test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
@@ -630,7 +630,7 @@ test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 #test_actuators_asctecv1.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_actuators_asctecv1.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-#test_actuators_asctecv1.srcs += downlink.c pprz_transport.c
+#test_actuators_asctecv1.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_actuators_asctecv1.srcs += $(SRC_BOOZ)/booz2_commands.c
 #test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
@@ -661,11 +661,10 @@ test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 #test_bmp085.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_bmp085.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-#test_bmp085.srcs += downlink.c pprz_transport.c
+#test_bmp085.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_bmp085.CFLAGS += -DUSE_I2C2
 #test_bmp085.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c
-##test_bmp085.CFLAGS += -DIMU_OVERRIDE_CHANNELS
 ##test_bmp085.CFLAGS += -DUSE_EXTI9_5_IRQ   # Mag Int on PB5
 #
 #
@@ -692,7 +691,7 @@ test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 #test_manual.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
 #
 #test_manual.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-#test_manual.srcs   += downlink.c pprz_transport.c
+#test_manual.srcs   += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 #
 #test_manual.srcs += $(SRC_BOOZ)/booz2_commands.c
 #
@@ -714,22 +713,22 @@ test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
 #test_manual.srcs   += $(SRC_SUBSYSTEMS)/radio_control.c                                 \
 #                  subsystems/radio_control/spektrum.c          \
 #                  $(SRC_ARCH)/subsystems/radio_control/spektrum_arch.c
+
+
+
 #
+# tunnel
 #
-#
-##
-## tunnel
-##
-#tunnel.ARCHDIR = $(ARCH)
-#tunnel.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-#tunnel.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-#tunnel.srcs   += $(SRC_AIRBORNE)/mcu.c \
-#                 $(SRC_ARCH)/mcu_arch.c \
-#                 $(SRC_LISA)/tunnel_hw.c          \
-#                 $(SRC_ARCH)/stm32_exceptions.c   \
-#                 $(SRC_ARCH)/stm32_vector_table.c
-#tunnel.CFLAGS += -DUSE_LED
-#tunnel.srcs   += $(SRC_ARCH)/led_hw.c
-#tunnel.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
-#tunnel.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-#tunnel.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+tunnel.ARCHDIR = $(ARCH)
+tunnel.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+tunnel.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
+tunnel.srcs   += $(SRC_AIRBORNE)/mcu.c \
+                 $(SRC_ARCH)/mcu_arch.c \
+                 $(SRC_LISA)/tunnel_hw.c          \
+                 $(SRC_ARCH)/stm32_exceptions.c   \
+                 $(SRC_ARCH)/stm32_vector_table.c
+tunnel.CFLAGS += -DUSE_LED
+tunnel.srcs   += $(SRC_ARCH)/led_hw.c
+tunnel.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+tunnel.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+tunnel.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
diff --git a/conf/autopilot/lisa_passthrough.makefile b/conf/autopilot/lisa_passthrough.makefile
index 74dea03fdf..3d0740fc99 100644
--- a/conf/autopilot/lisa_passthrough.makefile
+++ b/conf/autopilot/lisa_passthrough.makefile
@@ -40,7 +40,7 @@ stm_passthrough.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 # Telemetry
 stm_passthrough.CFLAGS += -DDOWNLINK
 stm_passthrough.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-stm_passthrough.srcs += downlink.c pprz_transport.c
+stm_passthrough.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 stm_passthrough.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
 stm_passthrough.srcs += mcu_periph/uart.c
 stm_passthrough.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
@@ -143,7 +143,7 @@ overo_test_passthrough.srcs    += $(SRC_FMS)/fms_periodic.c
 overo_test_passthrough.srcs    += $(SRC_FMS)/fms_spi_link.c
 overo_test_passthrough.srcs    += $(SRC_FMS)/fms_gs_com.c
 overo_test_passthrough.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_test_passthrough.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
+overo_test_passthrough.srcs    += $(SRC_FMS)/udp_transport2.c subsystems/datalink/downlink.c
 overo_test_passthrough.srcs    += $(SRC_FMS)/fms_network.c
 
 #
@@ -159,5 +159,5 @@ overo_blmc_calibrate.srcs    += $(SRC_FMS)/fms_periodic.c
 overo_blmc_calibrate.srcs    += $(SRC_FMS)/fms_spi_link.c
 #overo_blmc_calibrate.srcs    += $(SRC_FMS)/fms_gs_com.c
 #overo_blmc_calibrate.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-#overo_blmc_calibrate.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
+#overo_blmc_calibrate.srcs    += $(SRC_FMS)/udp_transport2.c subsystems/datalink/downlink.c
 overo_blmc_calibrate.srcs    += $(SRC_FMS)/fms_network.c
diff --git a/conf/autopilot/logger.makefile b/conf/autopilot/logger.makefile
index 1845dd204b..680131df66 100644
--- a/conf/autopilot/logger.makefile
+++ b/conf/autopilot/logger.makefile
@@ -50,6 +50,9 @@ ap.srcs += mcu.c
 #set SPI interface for SD card (0 or 1)
 ap.CFLAGS += -DHW_ENDPOINT_LPC2000_SPINUM=$(SPI_CHANNEL)
 
+#LPC2148 USB hw module needs at least 18MHz PCLK
+ap.CFLAGS += -DUSE_USB_HIGH_PCLK
+
 #efsl
 ap.CFLAGS += -I $(SRC_ARCH)/efsl/inc -I $(SRC_ARCH)/efsl/conf
 
diff --git a/conf/autopilot/obsolete/booz_test_progs.makefile b/conf/autopilot/obsolete/booz_test_progs.makefile
deleted file mode 100644
index 0574420200..0000000000
--- a/conf/autopilot/obsolete/booz_test_progs.makefile
+++ /dev/null
@@ -1,234 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# $Id$
-# Copyright (C) 2010 The Paparazzi Team
-#
-# This file is part of Paparazzi.
-#
-# Paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# Paparazzi 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 General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with Paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA.
-#
-#
-
-################################################################################
-#
-#
-#  Test program for the booz board
-#
-#
-#
-#
-# every "firmware" makefile should have a description of available targets
-# possible options for each of them, susbsystems and associated params for each of them
-#
-#
-#
-#
-################################################################################
-
-ARCH=lpc21
-
-SRC_ARCH=arch/$(ARCH)
-SRC_BOOZ=booz
-SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/lpc21
-SRC_BOARD=boards/$(BOARD)
-
-BOARD_CFG=\"boards/booz_1.0.h\"
-
-#
-# default configuration expected from board files
-#
-# SYS_TIME_LED = 1
-# MODEM_PORT   = UART1
-# MODEM_BAUD   = B57600
-
-
-#
-# test_telemetry : Sends ALIVE telemetry messages
-#
-# used configuration
-#   SYS_TIME_LED   :
-#   MODEM_PORT     :
-#   MODEM_BAUD     :
-#
-test_telemetry.ARCHDIR   = $(ARCH)
-test_telemetry.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_telemetry.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_telemetry.srcs   += test/test_telemetry.c \
-					 $(SRC_ARCH)/armVIC.c
-test_telemetry.CFLAGS += -DUSE_LED
-test_telemetry.CFLAGS += -DUSE_SYS_TIME
-test_telemetry.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_telemetry.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
-test_telemetry.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_telemetry.CFLAGS += -DUSE_$(MODEM_PORT)
-test_telemetry.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_telemetry.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_telemetry.srcs   += downlink.c pprz_transport.c
-test_telemetry.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-
-#
-# test_baro : reads barometers and sends values over telemetry
-#
-# configuration
-#   SYS_TIME_LED
-#   MODEM_PORT
-#   MODEM_BAUD
-#
-test_baro.ARCHDIR   = $(ARCH)
-test_baro.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_baro.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_ARCH) -I$(SRC_BOARD)
-test_baro.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_baro.srcs    = $(SRC_BOARD)/test_baro.c \
-					$(SRC_ARCH)/armVIC.c
-test_baro.CFLAGS += -DUSE_LED
-test_baro.CFLAGS += -DUSE_SYS_TIME
-test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_baro.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
-test_baro.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_baro.srcs   += downlink.c pprz_transport.c
-test_baro.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_baro.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_baro.srcs   += $(SRC_BOARD)/baro_board.c
-test_baro.CFLAGS += -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))'
-test_baro.CFLAGS += -DADC0_VIC_SLOT=2
-test_baro.CFLAGS += -DADC1_VIC_SLOT=3
-test_baro.srcs += $(SRC_BOOZ)/booz2_analog.c \
-				  $(SRC_BOOZ_ARCH)/booz2_analog_hw.c
-# tell me why this shit needs to know battery !!!!
-test_baro.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))'
-test_baro.srcs += $(SRC_FIRMWARE)/battery.c
-
-
-#
-# test_rc_spektrum :
-#
-# TODO
-#
-#
-#
-#
-
-
-
-#
-# test rc ppm
-#
-# configuration
-#   SYS_TIME_LED
-#   MODEM_PORT
-#   MODEM_BAUD
-#   RADIO_CONTROL_LED
-#
-test_rc_ppm.ARCHDIR   = $(ARCH)
-
-test_rc_ppm.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_rc_ppm.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -I$(SRC_BOARD)
-test_rc_ppm.CFLAGS += -DPERIPHERALS_AUTO_INIT
-test_rc_ppm.srcs   += $(SRC_BOOZ)/test/booz2_test_radio_control.c
-test_rc_ppm.CFLAGS += -DUSE_LED
-test_rc_ppm.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_rc_ppm.CFLAGS += -DTIME_LED=$(SYS_TIME_LED)
-test_rc_ppm.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-test_rc_ppm.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_rc_ppm.srcs   += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_rc_ppm.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_rc_ppm.srcs   += downlink.c pprz_transport.c
-test_rc_ppm.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
-test_rc_ppm.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/ppm.h\"
-test_rc_ppm.CFLAGS += -DRADIO_CONTROL_TYPE_PPM
-test_rc_ppm.srcs   += subsystmes/radio_control.c \
-					  subsystems/radio_control/ppm.c \
-					  $(SRC_ARCH)/subsystems/radio_control/ppm_arch.c
-
-
-
-#
-# simple test of mikrokopter motor controllers
-#
-test_esc_mkk_simple.ARCHDIR = $(ARCH)
-test_esc_mkk_simple.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_esc_mkk_simple.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_esc_mkk_simple.srcs = test/test_esc_mkk_simple.c		\
-						   $(SRC_ARCH)/armVIC.c
-test_esc_mkk_simple.CFLAGS += -DUSE_LED
-test_esc_mkk_simple.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_esc_mkk_simple.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_esc_mkk_simple.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_esc_mkk_simple.CFLAGS += -DACTUATORS_MKK_DEV=i2c0
-test_esc_mkk_simple.CFLAGS += -DUSE_I2C0
-test_esc_mkk_simple.CFLAGS += -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
-test_esc_mkk_simple.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-#
-# test actuators mkk
-#
-test_actuators_mkk.ARCHDIR = $(ARCH)
-test_actuators_mkk.CFLAGS = -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_actuators_mkk.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_actuators_mkk.srcs = test/test_actuators.c \
-						  $(SRC_ARCH)/armVIC.c
-
-test_actuators_mkk.CFLAGS += -DUSE_LED
-
-test_actuators_mkk.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
-test_actuators_mkk.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_actuators_mkk.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_actuators_mkk.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_actuators_mkk.srcs += downlink.c pprz_transport.c
-
-test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c
-test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
-test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0
-test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEV=i2c0
-test_actuators_mkk.CFLAGS += -DUSE_I2C0
-test_actuators_mkk.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_actuators_mkk.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=10
-
-
-#
-# test ami601
-#
-test_ami601.ARCHDIR = $(ARCH)
-test_ami601.CFLAGS = -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_ami601.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_ami601.srcs = test/peripherals/test_ami601.c \
-				   $(SRC_ARCH)/armVIC.c
-
-test_ami601.CFLAGS += -DUSE_LED
-
-test_ami601.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
-test_ami601.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_ami601.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_ami601.CFLAGS += -DUSE_$(MODEM_PORT) -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
-test_ami601.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_ami601.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=$(MODEM_PORT)
-test_ami601.srcs += downlink.c pprz_transport.c
-
-test_ami601.CFLAGS += -DUSE_AMI601
-test_ami601.srcs += peripherals/ami601.c
-test_ami601.CFLAGS += -DUSE_I2C1  -DI2C1_SCLL=150 -DI2C1_SCLH=150 -DI2C1_VIC_SLOT=11
-test_ami601.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
diff --git a/conf/autopilot/obsolete/booz_test_progs.xml b/conf/autopilot/obsolete/booz_test_progs.xml
deleted file mode 100644
index 82cf02eebb..0000000000
--- a/conf/autopilot/obsolete/booz_test_progs.xml
+++ /dev/null
@@ -1,20 +0,0 @@
-<firmware name="booz_test_progs" boards="booz_v1.0"
-  description="test programs for booz boards">
-
-   <target name="test telemetry" description="Sends ALIVE telemetry messages">
-     <param name="MODEM_PORT" values="UART1,UART2,UART3"    default="UART2"/>
-     <param name="MODEM_BAUD" values="B9600,B57600,B115200" default="B57600"/>
-   </target>
-
-   <target name="test baro" description="reads barometers and sends values over telemetry">
-     <param name="MODEM_PORT" values="UART1,UART2,UART3"    default="UART2"/>
-     <param name="MODEM_BAUD" values="B9600,B57600,B115200" default="B57600"/>
-   </target>
-
-  <target name="test spektrum" description="sends RADIO_CONTROL messages on telemetry">
-     <param name="MODEM_PORT"         values="UART0,UART1"    default="UART1"/>
-     <param name="MODEM_BAUD"         values="B9600,B57600,B115200" default="B57600"/>
-     <param name="RADIO_CONTROL_LINK" values="UART0,UART1"   default="UART0"/>
-  </target>
-
-</firmware>
\ No newline at end of file
diff --git a/conf/autopilot/obsolete/conf_dc_mc.h b/conf/autopilot/obsolete/conf_dc_mc.h
deleted file mode 100644
index dcc469feb3..0000000000
--- a/conf/autopilot/obsolete/conf_dc_mc.h
+++ /dev/null
@@ -1,12 +0,0 @@
-#ifndef CONF_DC_MC_H
-#define CONF_DC_MC_H
-
-/* clock in MHz */
-#define CLOCK 16
-
-
-#define LED_1_BANK C
-#define LED_1_PIN  3
-
-
-#endif /* CONF_DC_MC_H */
diff --git a/conf/autopilot/obsolete/conf_motor_bench.h b/conf/autopilot/obsolete/conf_motor_bench.h
deleted file mode 100644
index 1b218a2682..0000000000
--- a/conf/autopilot/obsolete/conf_motor_bench.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#ifndef CONFIG_MOTOR_BENCH_H
-#define CONFIG_MOTOR_BENCH_H
-
-/* Master oscillator freq.       */
-#define FOSC (12000000) 
-/* PLL multiplier                */
-#define PLL_MUL (5)         
-/* CPU clock freq.               */
-#define CCLK (FOSC * PLL_MUL) 
-/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2   */
-#define PBSD_BITS 0x00    
-#define PBSD_VAL 4
-/* Peripheral bus clock freq.    */
-#define PCLK (CCLK / PBSD_VAL) 
-
-#define LED_1_BANK 1
-#define LED_1_PIN 24
-
-/* ADC */
-#define ADC_0 AdcBank0(4)
-//#define ADC_0 AdcBank0(0)
-#ifdef USE_ADC_0
-#ifndef USE_AD0
-#define USE_AD0
-#endif
-#define USE_AD0_4
-//#define USE_AD0_0
-#endif
-
-#define ADC_1 AdcBank0(1)
-#ifdef USE_ADC_1
-#ifndef USE_AD0
-#define USE_AD0
-#endif
-#define USE_AD0_1
-#endif
-
-
-
-
-#endif /* CONFIG_MOTOR_BENCH_H */
diff --git a/conf/autopilot/obsolete/conf_wt.h b/conf/autopilot/obsolete/conf_wt.h
deleted file mode 100644
index b881939c5f..0000000000
--- a/conf/autopilot/obsolete/conf_wt.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef CONFIG_WT_H
-#define CONFIG_WT_H
-
-/* Master oscillator freq.       */
-#define FOSC (12000000) 
-/* PLL multiplier                */
-#define PLL_MUL (5)         
-/* CPU clock freq.               */
-#define CCLK (FOSC * PLL_MUL) 
-/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2   */
-#define PBSD_BITS 0x00    
-#define PBSD_VAL 4
-/* Peripheral bus clock freq.    */
-#define PCLK (CCLK / PBSD_VAL) 
-
-#define LED_1_BANK 1
-#define LED_1_PIN 24
-
-#define SPI_SELECT_SLAVE0_PORT 0
-#define SPI_SELECT_SLAVE0_PIN  20
-
-#define SPI1_DRDY_PINSEL       PINSEL1
-#define SPI1_DRDY_PINSEL_BIT   0
-#define SPI1_DRDY_PINSEL_VAL   1
-#define SPI1_DRDY_EINT         0
-#define SPI1_DRDY_VIC_IT       VIC_EINT0
-
-
-#endif /* CONFIG_WT_H */
diff --git a/conf/autopilot/obsolete/csc.makefile b/conf/autopilot/obsolete/csc.makefile
deleted file mode 100644
index f30151ea99..0000000000
--- a/conf/autopilot/obsolete/csc.makefile
+++ /dev/null
@@ -1,136 +0,0 @@
-#
-# $Id$
-#
-# Copyright (C) 2009 Antoine Drouin
-#
-# This file is part of paparazzi.
-#
-# paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# paparazzi 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 General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA.
-#
-#
-
-ARCHI=arm7
-
-FLASH_MODE = ISP
-LPC21ISP_PORT = /dev/ttyUSB0
-
-LPC21ISP_BAUD = 38400
-LPC21ISP_XTAL = 12000
-
-
-LPC21ISP_CONTROL = -control
-
-LDSCRIPT=$(SRC_ARCH)/LPC2129-ROM.ld
-
-BOARD_CFG = \"csc_board_v1_0.h\"
-
-
-
-SRC_CSC=csc
-
-ap.ARCHDIR = $(ARCHI)
-
-ap.CFLAGS += -I$(SRC_CSC)
-ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-ap.srcs += $(SRC_CSC)/csc_main.c
-ap.CFLAGS += -DUSE_LED -DTIME_LED=1
-
-ap.CFLAGS += -DCSC_BOARD_ID=$(CSC_ID)
-
-ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-ap.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-ap.srcs += $(SRC_ARCH)/adc_hw.c
-ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_0 -DUSE_AD0_1
-
-#ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 -DUART0_VIC_SLOT=5
-#ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-#ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
-	                  -DDOWNLINK_DEVICE=Uart1
-#ap.srcs += downlink.c pprz_transport.c
-#ap.srcs += $(SRC_CSC)/csc_telemetry.c
-
-
-ap.CFLAGS += -DAP_LINK_CAN -DCAN_LED=2
-ap.CFLAGS += -DUSE_CAN1 -DCAN1_BTR=CANBitrate125k_3MHz
-ap.CFLAGS +=  -DCAN1_VIC_SLOT=3 -DCAN1_ERR_VIC_SLOT=7
-ap.srcs += $(SRC_CSC)/csc_can.c
-#ap.CFLAGS += -DUSE_CAN2 -DCAN2_BTR=CANBitrate125k_2MHz -DCAN2_VIC_SLOT=4
-
-#ap.CFLAGS += -DAP_LINK_UART -DPPRZ_UART=Uart1
-#ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-#ap.srcs += pprz_transport.c
-
-ap.srcs += $(SRC_CSC)/csc_ap_link.c
-
-ap.srcs += $(SRC_CSC)/csc_servos.c
-
-ap.srcs += $(SRC_CSC)/csc_adc.c
-
-#ap.CFLAGS += -DTHROTTLE_LINK=Uart0 -DTHROTTLE_LED=3 -DUSE_CSC_THROTTLE
-#ap.srcs += $(SRC_CSC)/csc_throttle.c
-
-ap.CFLAGS += -DSPEKTRUM_LINK=Uart1 -DUSE_UART1 -DUART1_BAUD=B115200 -DUART1_VIC_SLOT=6
-ap.srcs += $(SRC_CSC)/csc_rc_spektrum.c
-
-ap.CFLAGS += -DERROR_LED=4
-
-#
-#
-# test uart
-#
-test_uart.ARCHDIR = $(ARCHI)
-
-
-test_uart.CFLAGS += -I$(SRC_CSC)
-test_uart.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_uart.srcs += $(SRC_CSC)/csc_test_uart.c
-test_uart.CFLAGS += -DUSE_LED
-
-# -DTIME_LED=1
-test_uart.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-test_uart.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-test_uart.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_uart.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-test_uart.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
-	                  -DDOWNLINK_DEVICE=Uart1
-test_uart.srcs += downlink.c pprz_transport.c
-
-
-#
-# TEST CAN1
-#
-
-test_can1.ARCHDIR = $(ARCHI)
-
-
-test_can1.CFLAGS += -I$(SRC_CSC)
-test_can1.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_can1.srcs += $(SRC_CSC)/test_can1.c
-test_can1.CFLAGS += -DUSE_LED
-
-# -DTIME_LED=1
-test_can1.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-test_can1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-test_can1.CFLAGS += -DUSE_CAN1 -DCAN1_BTR=CANBitrate125k_2MHz
-test_can1.CFLAGS +=  -DCAN1_VIC_SLOT=3 -DCAN1_ERR_VIC_SLOT=7
-test_can1.srcs += $(SRC_CSC)/csc_can.c
-test_can1.CFLAGS += -DCSC_BOARD_ID=0
-
diff --git a/conf/autopilot/obsolete/csc_ap.makefile b/conf/autopilot/obsolete/csc_ap.makefile
deleted file mode 100644
index d1d6b55322..0000000000
--- a/conf/autopilot/obsolete/csc_ap.makefile
+++ /dev/null
@@ -1,117 +0,0 @@
-#
-# $Id$
-#
-# Copyright (C) 2009 Antoine Drouin, Allen H. Ibara
-#
-# This file is part of paparazzi.
-#
-# paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# paparazzi 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 General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA.
-#
-#
-
-ARCHI=arm7
-
-FLASH_MODE = ISP
-LPC21ISP_PORT = /dev/ttyUSB0
-
-LPC21ISP_BAUD = 38400
-LPC21ISP_XTAL = 12000
-
-
-LPC21ISP_CONTROL = -control
-
-LDSCRIPT=$(SRC_ARCH)/LPC2129-ROM.ld
-
-BOARD_CFG = \"csc_board_v1_0.h\"
-
-PERIODIC_FREQ = 40
-
-SRC_CSC=csc
-
-ap.ARCHDIR = $(ARCHI)
-
-ap.CFLAGS += -I$(SRC_CSC)
-ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-ap.srcs += $(SRC_CSC)/csc_ap_main.c
-ap.CFLAGS += -DUSE_LED -DTIME_LED=1
-
-ap.CFLAGS += -DCSC_BOARD_ID=$(CSC_ID)
-
-ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./40.))' -DTIMER0_VIC_SLOT=1
-ap.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-ap.srcs += $(SRC_ARCH)/adc_hw.c
-ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_0 -DUSE_AD0_1
-
-
-ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B230400 -DUART0_VIC_SLOT=5
-ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
-	                  -DDOWNLINK_DEVICE=Uart1
-ap.srcs += downlink.c pprz_transport.c $(SRC_CSC)/csc_telemetry.c
-ap.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
-ap.srcs += $(SRC_CSC)/csc_datalink.c
-
-ap.srcs += $(SRC_CSC)/csc_xsens.c
-ap.CFLAGS += -DXSENS1_LINK=Uart0
-
-ap.CFLAGS += -DAP_LINK_CAN -DCAN_LED=2
-ap.CFLAGS += -DUSE_CAN1 -DCAN1_BTR=CANBitrate125k_2MHz
-ap.CFLAGS +=  -DCAN1_VIC_SLOT=3 -DCAN1_ERR_VIC_SLOT=7
-ap.srcs += $(SRC_CSC)/csc_can.c
-#ap.CFLAGS += -DUSE_CAN2 -DCAN2_BTR=CANBitrate125k_2MHz -DCAN2_VIC_SLOT=4
-
-ap.srcs += $(SRC_CSC)/csc_ap_link.c
-
-#ap.srcs += $(SRC_CSC)/csc_servos.c
-#ap.CFLAGS += -DPWM_SERVO_5 -DPWM_SERVO_0
-ap.srcs += $(SRC_ARCH)/servos_csc.c
-
-ap.srcs += $(SRC_CSC)/csc_adc.c
-ap.CFLAGS += -DACTUATORS=\"servos_csc.h\"
-ap.srcs += commands.c actuators.c
-
-ap.CFLAGS += -DRADIO_CONTROL
-ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
-
-ap.srcs += $(SRC_CSC)/csc_autopilot.c
-
-ap.CFLAGS += -DERROR_LED=4
-
-#
-#
-# test uart
-#
-test_uart.ARCHDIR = $(ARCHI)
-
-
-test_uart.CFLAGS += -I$(SRC_CSC)
-test_uart.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_uart.srcs += $(SRC_CSC)/csc_test_uart.c
-test_uart.CFLAGS += -DUSE_LED
-
-# -DTIME_LED=1
-test_uart.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-test_uart.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-test_uart.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_uart.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-test_uart.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
-	                  -DDOWNLINK_DEVICE=Uart1
-test_uart.srcs += downlink.c pprz_transport.c
-
diff --git a/conf/autopilot/obsolete/csc_baro.makefile b/conf/autopilot/obsolete/csc_baro.makefile
deleted file mode 100644
index bb0065ad16..0000000000
--- a/conf/autopilot/obsolete/csc_baro.makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-# baro scp bits
-ap.CFLAGS += -DUSE_BARO_SCP -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0
-ap.srcs += $(SRC_CSC)/csc_baro.c mcu_periph/spi.c $(SRC_ARCH)/mcu_periph/spi_arch.c
-
diff --git a/conf/autopilot/obsolete/csc_board_v1_0.h b/conf/autopilot/obsolete/csc_board_v1_0.h
deleted file mode 100644
index a5b8c2066a..0000000000
--- a/conf/autopilot/obsolete/csc_board_v1_0.h
+++ /dev/null
@@ -1,66 +0,0 @@
-#ifndef CONFIG_CSC_V1_0_H
-#define CONFIG_CSC_V1_0_H
-
-
-/* A 60mhz servo driver !!!! this probably has more processing power than apollo13 */
-/* Master oscillator freq.       */
-#define FOSC (12000000) 
-/* PLL multiplier                */
-#define PLL_MUL (5)         
-
-//#define FOSC (14745600) 
-//#define PLL_MUL (4)         
-
-/* CPU clock freq.               */
-#define CCLK (FOSC * PLL_MUL) 
-
-/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2   */
-
-// 15MHz peripheral bus
-//#define PBSD_BITS 0x00    
-//#define PBSD_VAL 4
-
-// 30MHz peripheral bus
-#define PBSD_BITS 0x02    
-#define PBSD_VAL 2
-
-/* Peripheral bus clock freq. */
-#define PCLK (CCLK / PBSD_VAL) 
-
-/* Onboard LEDs */
-#define LED_1_BANK 1
-#define LED_1_PIN 18
-
-#define LED_2_BANK 1
-#define LED_2_PIN 17
-
-#define LED_3_BANK 1
-#define LED_3_PIN 16
-
-#define LED_4_BANK 1
-#define LED_4_PIN 31
-
-/* PPM : rc rx on P0.30 (ADC pico-blade pin 3)*/
-#define PPM_PINSEL PINSEL1
-#define PPM_PINSEL_VAL 0x3
-#define PPM_PINSEL_BIT 28
-
-#ifdef RADIO_CONTROL
-#define PPM_CRI TIR_CR0I
-#define PPM_CCR_CRF TCCR_CR0_F
-#define PPM_CCR_CRR TCCR_CR0_R
-#define PPM_CCR_CRI TCCR_CR0_I
-#define PPM_CR T0CR0
-#endif
-
-#define SPI_SELECT_SLAVE0_PORT 0
-#define SPI_SELECT_SLAVE0_PIN 20
-
-#define SPI1_DRDY_PINSEL PINSEL1
-#define SPI1_DRDY_PINSEL_BIT   0
-#define SPI1_DRDY_PINSEL_VAL   1
-#define SPI1_DRDY_EINT         0
-#define SPI1_DRDY_VIC_IT       VIC_EINT0
-
-
-#endif /* CONFIG_CSC_V1_0_H */
diff --git a/conf/autopilot/obsolete/data_logger_1_0.h b/conf/autopilot/obsolete/data_logger_1_0.h
deleted file mode 100644
index 6b762cc714..0000000000
--- a/conf/autopilot/obsolete/data_logger_1_0.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#ifndef CONFIG_DATA_LOGGER_H
-#define CONFIG_DATA_LOGGER_H
-
-/* Master oscillator freq.       */
-#define FOSC (12000000) 
-
-/* PLL multiplier                */
-#define PLL_MUL (5)         
-
-/* CPU clock freq.               */
-#define CCLK (FOSC * PLL_MUL) 
-
-/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2   */
-#define PBSD_BITS 0x00    
-#define PBSD_VAL 4
-
-/* Peripheral bus clock freq. */
-#define PCLK (CCLK / PBSD_VAL) 
-
-/* LEDs */
-#define LED_1_BANK 0
-#define LED_1_PIN 21
-
-#define LED_2_BANK 0
-#define LED_2_PIN 22
-
-#define LED_3_BANK 1
-#define LED_3_PIN 19
-
-/* ADC */
-#define ADC_CHANNEL_VSUPPLY AdcBank0(6)
-#ifndef USE_AD0
-#define USE_AD0
-#endif
-#define USE_AD0_6
-
-/* SPI for SD card */
-#define SPI1_DRDY_PINSEL PINSEL1
-#define SPI1_DRDY_PINSEL_BIT   0
-#define SPI1_DRDY_PINSEL_VAL   1
-#define SPI1_DRDY_EINT         0
-#define SPI1_DRDY_VIC_IT       VIC_EINT0
-
-#endif /* CONFIG_DATA_LOGGER_H */
diff --git a/conf/autopilot/obsolete/gps_ugear.makefile b/conf/autopilot/obsolete/gps_ugear.makefile
deleted file mode 100644
index accfc304e0..0000000000
--- a/conf/autopilot/obsolete/gps_ugear.makefile
+++ /dev/null
@@ -1,5 +0,0 @@
-# OSAM UGEAR
-
-#ap.CFLAGS += -DGPS
-
-$(TARGET).srcs += $(SRC_FIXEDWING)/gps.c $(SRC_FIXEDWING)/latlong.c
diff --git a/conf/autopilot/obsolete/lisa_test_progs.makefile b/conf/autopilot/obsolete/lisa_test_progs.makefile
deleted file mode 100644
index 233820d1e6..0000000000
--- a/conf/autopilot/obsolete/lisa_test_progs.makefile
+++ /dev/null
@@ -1,1392 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# $Id$
-# Copyright (C) 2010 Antoine Drouin
-#
-# This file is part of Paparazzi.
-#
-# Paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# Paparazzi 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 General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with Paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA.
-#
-#
-
-
-################################################################################
-#
-#
-#  Those babies run on the overo
-#
-#
-################################################################################
-
-SRC_CSC=csc
-
-# test spi link between overo and stm32
-overo_test_spi_link.ARCHDIR = omap
-overo_test_spi_link.CFLAGS  += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include
-overo_test_spi_link.CFLAGS  += -DOVERO_LINK_MSG_UP=AutopilotMessageFoo -DOVERO_LINK_MSG_DOWN=AutopilotMessageFoo
-overo_test_spi_link.srcs  = $(SRC_FMS)/overo_test_spi_link.c
-overo_test_spi_link.srcs += $(SRC_FMS)/fms_spi_link.c
-
-
-
-# test network based telemetry on overo
-overo_test_telemetry.ARCHDIR  = omap
-overo_test_telemetry.CFLAGS  += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include
-overo_test_telemetry.srcs     = $(SRC_FMS)/overo_test_telemetry.c
-overo_test_telemetry.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_test_telemetry.srcs    += $(SRC_FMS)/udp_transport.c downlink.c
-overo_test_telemetry.srcs    += $(SRC_FMS)/fms_network.c
-overo_test_telemetry.LDFLAGS += -levent
-
-# test network based telemetry on overo (using udp_transport2/messages2)
-overo_test_telemetry2.ARCHDIR  = omap
-overo_test_telemetry2.CFLAGS  += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include
-overo_test_telemetry2.srcs     = $(SRC_FMS)/overo_test_telemetry2.c
-overo_test_telemetry2.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_test_telemetry2.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
-overo_test_telemetry2.srcs    += $(SRC_FMS)/fms_network.c
-overo_test_telemetry2.LDFLAGS += -levent
-
-# test gps on overo
-overo_test_gps.ARCHDIR  = omap
-overo_test_gps.CFLAGS  += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include
-overo_test_gps.srcs     = $(SRC_FMS)/overo_test_gps.c
-overo_test_gps.CFLAGS  += -DFMS_PERIODIC_FREQ=500
-overo_test_gps.srcs    += $(SRC_FMS)/fms_periodic.c
-overo_test_gps.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_test_gps.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
-overo_test_gps.srcs    += $(SRC_FMS)/fms_network.c
-overo_test_gps.LDFLAGS += -levent
-
-
-# test periodic tasks on the overo
-overo_test_periodic.ARCHDIR  = omap
-overo_test_periodic.CFLAGS  += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include
-overo_test_periodic.srcs     = $(SRC_FMS)/overo_test_periodic.c
-overo_test_periodic.CFLAGS  += -DFMS_PERIODIC_FREQ=10
-overo_test_periodic.srcs    += $(SRC_FMS)/fms_periodic.c
-overo_test_periodic.srcs    += $(SRC_FMS)/fms_serial_port.c
-overo_test_periodic.LDFLAGS += -lrt
-overo_test_periodic.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_test_periodic.srcs    += $(SRC_FMS)/udp_transport.c downlink.c
-overo_test_periodic.srcs    += $(SRC_FMS)/fms_network.c
-overo_test_periodic.LDFLAGS += -levent
-overo_test_periodic.CFLAGS  += -DOVERO_LINK_MSG_UP=AutopilotMessageBethUp -DOVERO_LINK_MSG_DOWN=AutopilotMessageBethDown
-overo_test_periodic.srcs    += $(SRC_FMS)/fms_spi_link.c
-
-# test passthrough , aka using stm32 as io processor
-# this demonstrates
-#   -link with io processor
-#   -periodic event
-#   -telemetry and datalink
-#
-overo_test_passthrough.ARCHDIR  = omap
-overo_test_passthrough.LDFLAGS += -levent -lm
-overo_test_passthrough.CFLAGS  += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include
-overo_test_passthrough.CFLAGS  += -DOVERO_LINK_MSG_UP=AutopilotMessagePTUp -DOVERO_LINK_MSG_DOWN=AutopilotMessagePTDown
-overo_test_passthrough.srcs     = $(SRC_FMS)/overo_test_passthrough.c
-overo_test_passthrough.CFLAGS  += -DFMS_PERIODIC_FREQ=512
-overo_test_passthrough.srcs    += $(SRC_FMS)/fms_periodic.c
-overo_test_passthrough.srcs    += $(SRC_FMS)/fms_spi_link.c
-overo_test_passthrough.srcs    += $(SRC_FMS)/fms_gs_com.c
-overo_test_passthrough.CFLAGS  += -DDOWNLINK -DDOWNLINK_TRANSPORT=UdpTransport
-overo_test_passthrough.srcs    += $(SRC_FMS)/udp_transport2.c downlink.c
-overo_test_passthrough.srcs    += $(SRC_FMS)/fms_network.c
-
-
-
-################################################################################
-#
-#
-#  Those babies run on the stm32
-#
-#
-################################################################################
-
-ARCH=stm32
-SRC_ARCH=$(ARCH)
-SRC_LISA=lisa
-SRC_LISA_ARCH=$(SRC_LISA)/arch/$(ARCH)
-SRC_BOOZ=booz
-SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)
-
-SRC_FIRMWARE=firmwares/rotorcraft
-SRC_SUBSYSTEMS=subsystems
-
-#BOARD_CFG=\"boards/olimex_stm32-h103.h\"
-BOARD_CFG=\"boards/lisa_l_1.0.h\"
-#FLASH_MODE = SERIAL
-FLASH_MODE = JTAG
-
-#
-# test leds
-#
-test_led.ARCHDIR = $(ARCH)
-test_led.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_led.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_led.srcs += $(SRC_LISA)/test_led.c         \
-                 $(SRC_ARCH)/led_hw.c       \
-                 $(SRC_ARCH)/stm32_exceptions.c   \
-                 $(SRC_ARCH)/stm32_vector_table.c
-test_led.CFLAGS += -DUSE_LED
-
-
-#
-# test leds2
-#
-test_led2.ARCHDIR = $(ARCH)
-test_led2.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_led2.CFLAGS += -DBOARD_CONFIG=\"boards/lisa_0.99.h\"
-test_led2.srcs += $(SRC_LISA)/test_led2.c              \
-                 $(SRC_LISA)/exceptions.c              \
-                 $(SRC_LISA)/vector_table.c
-test_led2.CFLAGS += -DUSE_LED
-
-
-#
-# test periodic
-#
-test_periodic.ARCHDIR = $(ARCH)
-test_periodic.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_periodic.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_periodic.srcs += $(SRC_LISA)/test_periodic.c  \
-                      $(SRC_ARCH)/stm32_exceptions.c     \
-                      $(SRC_ARCH)/stm32_vector_table.c
-test_periodic.CFLAGS += -DUSE_LED
-test_periodic.srcs += $(SRC_ARCH)/led_hw.c
-test_periodic.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_periodic.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_periodic.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-
-#
-# test uart
-#
-test_uart.ARCHDIR = $(ARCH)
-test_uart.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_uart.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_uart.srcs = $(SRC_LISA)/test_uart.c         \
-                      $(SRC_LISA)/exceptions.c   \
-                      $(SRC_LISA)/vector_table.c
-test_uart.CFLAGS += -DUSE_LED
-test_uart.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_uart.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_uart.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_uart.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_uart.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-
-
-#
-# test telemetry1
-#
-test_telemetry1.ARCHDIR = $(ARCH)
-test_telemetry1.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_telemetry1.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_telemetry1.srcs = $(SRC_LISA)/test_telemetry.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-test_telemetry1.CFLAGS += -DUSE_LED
-test_telemetry1.srcs += $(SRC_ARCH)/led_hw.c
-test_telemetry1.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_telemetry1.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_telemetry1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_telemetry1.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_telemetry1.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_telemetry1.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_telemetry1.srcs += downlink.c pprz_transport.c
-
-
-#
-# test telemetry2
-#
-test_telemetry2.ARCHDIR = $(ARCH)
-test_telemetry2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_telemetry2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_telemetry2.srcs = $(SRC_LISA)/test_telemetry.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-test_telemetry2.CFLAGS += -DUSE_LED
-test_telemetry2.srcs += $(SRC_ARCH)/led_hw.c
-test_telemetry2.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_telemetry2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_telemetry2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_telemetry2.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_telemetry2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_telemetry2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_telemetry2.srcs += downlink.c pprz_transport.c
-
-
-#
-# test telemetry3
-#
-test_telemetry3.ARCHDIR = $(ARCH)
-test_telemetry3.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_telemetry3.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_telemetry3.srcs = $(SRC_LISA)/test_telemetry.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-test_telemetry3.CFLAGS += -DUSE_LED
-test_telemetry3.srcs += $(SRC_ARCH)/led_hw.c
-test_telemetry3.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_telemetry3.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_telemetry3.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_telemetry3.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B57600
-test_telemetry3.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_telemetry3.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart3
-test_telemetry3.srcs += downlink.c pprz_transport.c
-
-#
-# test datalink
-#
-test_datalink.ARCHDIR = $(ARCH)
-test_datalink.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_datalink.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_datalink.srcs = $(SRC_LISA)/test_datalink.c \
-                     $(SRC_ARCH)/stm32_exceptions.c   \
-                     $(SRC_ARCH)/stm32_vector_table.c
-test_datalink.CFLAGS += -DUSE_LED
-test_datalink.srcs += $(SRC_ARCH)/led_hw.c
-test_datalink.CFLAGS += -DUSE_SYS_TIME  -DSYS_TIME_LED=1
-test_datalink.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_datalink.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_datalink.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_datalink.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_datalink.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_datalink.srcs += downlink.c pprz_transport.c
-test_datalink.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart2
-#test_datalink.srcs += $(SRC_FIRMWARE)/datalink.c
-
-#
-# tunnel
-#
-tunnel.ARCHDIR = $(ARCH)
-tunnel.CFLAGS  = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-tunnel.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-tunnel.srcs += $(SRC_LISA)/test/lisa_tunnel.c \
-           $(SRC_ARCH)/stm32_exceptions.c  \
-               $(SRC_ARCH)/stm32_vector_table.c
-tunnel.CFLAGS += -DUSE_LED
-tunnel.srcs += $(SRC_ARCH)/led_hw.c
-tunnel.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-tunnel.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-tunnel.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-#tunnel.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B9600
-#tunnel.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B9600
-
-#tunnel.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
-#tunnel.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B38400
-
-tunnel.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-tunnel.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-
-#tunnel.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B115200
-#tunnel.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B115200
-tunnel.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-
-
-#
-# test float
-#
-test_float.ARCHDIR = $(ARCH)
-test_float.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_float.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_float.srcs = $(SRC_LISA)/test_float.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-test_float.CFLAGS += -DUSE_LED
-test_float.srcs += $(SRC_ARCH)/led_hw.c
-test_float.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_float.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_float.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_float.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_float.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_float.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_float.srcs += downlink.c pprz_transport.c
-test_float.srcs += lisa/plug_sys.c
-
-#
-# test bswap
-#
-test_bswap.ARCHDIR = $(ARCH)
-test_bswap.CFLAGS = -I$(SRC_LISA) -I$(ARCH)
-test_bswap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_bswap.srcs = $(SRC_LISA)/test/test_bswap.c \
-
-
-
-#
-# test rc 2.4
-#
-
-SRC_BOOZ = booz
-SRC_BOOZ_TEST = $(SRC_BOOZ)/test
-
-test_rc_24.ARCHDIR = $(ARCH)
-test_rc_24.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -DPERIPHERALS_AUTO_INIT
-test_rc_24.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_rc_24.srcs += $(SRC_BOOZ_TEST)/booz2_test_radio_control.c \
-                   $(SRC_ARCH)/stm32_exceptions.c   \
-                   $(SRC_ARCH)/stm32_vector_table.c
-
-test_rc_24.CFLAGS += -DUSE_LED
-test_rc_24.srcs += $(SRC_ARCH)/led_hw.c
-
-test_rc_24.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_rc_24.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_rc_24.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_rc_24.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_rc_24.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_rc_24.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_rc_24.srcs += downlink.c pprz_transport.c
-
-test_rc_24.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_LED=2
-test_rc_24.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
-test_rc_24.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=\"subsystems/radio_control/spektrum_dx7se.h\"
-test_rc_24.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B115200
-test_rc_24.CFLAGS += -DRADIO_CONTROL_LINK=Uart3
-test_rc_24.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
-                   $(SRC_BOOZ)/subsystems/radio_control/spektrum.c
-#                  $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-
-#
-# test servos
-#
-
-SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)
-
-test_servos.ARCHDIR = $(ARCH)
-test_servos.CFLAGS  = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_servos.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_servos.LDFLAGS += -lm
-test_servos.srcs += $(SRC_LISA)/test_servos.c   \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-test_servos.CFLAGS += -DUSE_LED
-test_servos.srcs += $(SRC_ARCH)/led_hw.c
-test_servos.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_servos.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_servos.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_servos.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
-
-
-#test_servos.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-#test_servos.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-#test_servos.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-#test_servos.srcs += downlink.c pprz_transport.c
-
-
-
-
-
-
-#
-# test IMU b2
-#
-test_imu_b2.ARCHDIR = $(ARCH)
-test_imu_b2.CFLAGS  = -I$(SRC_FIRMWARE) -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_imu_b2.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-test_imu_b2.srcs += $(SRC_BOOZ_TEST)/booz_test_imu.c \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-
-test_imu_b2.CFLAGS += -DUSE_LED
-test_imu_b2.srcs += $(SRC_ARCH)/led_hw.c
-
-test_imu_b2.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_imu_b2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_imu_b2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_imu_b2.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_imu_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_imu_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_imu_b2.srcs += downlink.c pprz_transport.c
-
-test_imu_b2.srcs += math/pprz_trig_int.c
-
-test_imu_b2.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
-test_imu_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
-test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu.c
-test_imu_b2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
-test_imu_b2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
-test_imu_b2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-test_imu_b2.srcs += peripherals/ms2100.c  $(SRC_ARCH)/peripherals/ms2100_arch.c
-
-
-#
-# test IMU crista
-#
-test_imu_crista.ARCHDIR = $(ARCH)
-test_imu_crista.CFLAGS  = -I$(SRC_FIRMWARE) -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_imu_crista.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_imu_crista.srcs += $(SRC_BOOZ_TEST)/booz_test_imu.c \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-
-test_imu_crista.CFLAGS += -DUSE_LED
-test_imu_crista.srcs += $(SRC_ARCH)/led_hw.c
-
-test_imu_crista.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_imu_crista.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_imu_crista.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_imu_crista.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_imu_crista.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_imu_crista.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_imu_crista.srcs += downlink.c pprz_transport.c
-
-test_imu_crista.srcs += math/pprz_trig_int.c
-
-test_imu_crista.CFLAGS += -DIMU_TYPE_H=\"imu/imu_crista.h\" -DIMU_OVERRIDE_CHANNELS
-test_imu_crista.srcs += $(SRC_SUBSYSTEMS)/imu.c             \
-                        $(SRC_SUBSYSTEMS)/imu/imu_crista.c \
-                        $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_crista_arch.c
-test_imu_crista.CFLAGS += -DUSE_DMA1_C4_IRQ
-
-
-#
-# test IMU aspirin
-#
-test_imu_aspirin.ARCHDIR = $(ARCH)
-test_imu_aspirin.CFLAGS  = -I$(SRC_FIRMWARE) -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH) -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_imu_aspirin.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_imu_aspirin.srcs += $(SRC_BOOZ_TEST)/booz_test_imu.c \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-
-test_imu_aspirin.CFLAGS += -DUSE_LED
-test_imu_aspirin.srcs += $(SRC_ARCH)/led_hw.c
-
-test_imu_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_imu_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_imu_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_imu_aspirin.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_imu_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_imu_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_imu_aspirin.srcs += downlink.c pprz_transport.c
-
-test_imu_aspirin.srcs += math/pprz_trig_int.c
-
-test_imu_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
-test_imu_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c             \
-                        $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
-                        $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_aspirin_arch.c
-
-test_imu_aspirin.CFLAGS += -DUSE_I2C2
-test_imu_aspirin.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_imu_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ  # Gyro Int on PC14
-test_imu_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ    # Mag Int on PB5
-test_imu_aspirin.CFLAGS += -DUSE_EXTI2_IRQ      # Accel Int on PD2
-test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
-
-#test_imu_aspirin.CFLAGS += -DI2C2_STOP_HANDLER=OnI2CDone -DI2C2_STOP_HANDLER_HEADER=\"imu/imu_aspirin_arch.h\"
-#test_imu_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ
-
-
-
-
-#
-# test motor controllers
-#
-test_mc.ARCHDIR = $(ARCH)
-test_mc.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_mc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_mc.srcs = $(SRC_LISA)/test_mc.c      \
-               $(SRC_ARCH)/stm32_exceptions.c   \
-               $(SRC_ARCH)/stm32_vector_table.c
-test_mc.CFLAGS += -DUSE_LED
-test_mc.srcs += $(SRC_ARCH)/led_hw.c
-test_mc.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_mc.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_mc.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-
-#
-# test motor controllers with interrupts
-#
-test_mc2.ARCHDIR = $(ARCH)
-test_mc2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_mc2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_mc2.srcs = $(SRC_LISA)/test_mc2.c           \
-                $(SRC_ARCH)/stm32_exceptions.c   \
-                $(SRC_ARCH)/stm32_vector_table.c
-test_mc2.CFLAGS += -DUSE_LED
-test_mc2.srcs += $(SRC_ARCH)/led_hw.c
-test_mc2.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_mc2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_mc2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_mc2.CFLAGS += -DUSE_I2C1
-test_mc2.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-#
-# test motor controllers asctec with interrupts
-#
-test_mc_asctec_v1_simple.ARCHDIR = $(ARCH)
-test_mc_asctec_v1_simple.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_mc_asctec_v1_simple.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_mc_asctec_v1_simple.srcs = $(SRC_LISA)/test/test_mc_asctec_v1_simple.c  \
-                                $(SRC_ARCH)/stm32_exceptions.c               \
-                                $(SRC_ARCH)/stm32_vector_table.c
-test_mc_asctec_v1_simple.CFLAGS += -DUSE_LED
-test_mc_asctec_v1_simple.srcs += $(SRC_ARCH)/led_hw.c
-test_mc_asctec_v1_simple.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_mc_asctec_v1_simple.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_mc_asctec_v1_simple.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_mc_asctec_v1_simple.CFLAGS += -DUSE_I2C1
-test_mc_asctec_v1_simple.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_mc_asctec_v1_simple.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_mc_asctec_v1_simple.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_mc_asctec_v1_simple.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_mc_asctec_v1_simple.srcs += downlink.c pprz_transport.c
-
-
-#
-# test motor controllers asctec2 with interrupts
-#
-test_mc5.ARCHDIR = $(ARCH)
-test_mc5.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_mc5.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_mc5.srcs = $(SRC_LISA)/test_mc5.c           \
-                $(SRC_ARCH)/stm32_exceptions.c   \
-                $(SRC_ARCH)/stm32_vector_table.c
-test_mc5.CFLAGS += -DUSE_LED
-test_mc5.srcs += $(SRC_ARCH)/led_hw.c
-test_mc5.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_mc5.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_mc5.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_mc5.CFLAGS += -DUSE_I2C1
-test_mc5.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-
-
-#
-# test actuators mkk
-#
-test_actuators_mkk.ARCHDIR = $(ARCH)
-test_actuators_mkk.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -DPERIPHERALS_AUTO_INIT
-test_actuators_mkk.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_actuators_mkk.srcs = $(SRC_LISA)/test/lisa_test_actuators_mkk.c \
-                          $(SRC_ARCH)/stm32_exceptions.c   \
-                          $(SRC_ARCH)/stm32_vector_table.c
-
-test_actuators_mkk.CFLAGS += -DUSE_LED
-test_actuators_mkk.srcs += $(SRC_ARCH)/led_hw.c
-
-test_actuators_mkk.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_actuators_mkk.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_actuators_mkk.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_actuators_mkk.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_actuators_mkk.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_actuators_mkk.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_actuators_mkk.srcs += downlink.c pprz_transport.c
-
-test_actuators_mkk.srcs += $(SRC_FIRMWARE)/commands.c
-test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
-test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1  -DUSE_TIM2_IRQ
-#test_actuators_mkk.CFLAGS += -DACTUATORS_ASCTEC_V2_PROTOCOL -DACTUATORS_ASCTEC_DEVICE=i2c1
-#test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
-test_actuators_mkk.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-test_actuators_mkk.CFLAGS += -DUSE_I2C1
-test_actuators_mkk.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-#
-# test actuators asctec
-#
-test_actuators_asctec.ARCHDIR = $(ARCH)
-test_actuators_asctec.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -DPERIPHERALS_AUTO_INIT
-test_actuators_asctec.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_actuators_asctec.srcs = $(SRC_LISA)/test/lisa_test_actuators_mkk.c \
-                          $(SRC_ARCH)/stm32_exceptions.c   \
-                          $(SRC_ARCH)/stm32_vector_table.c
-
-test_actuators_asctec.CFLAGS += -DUSE_LED
-test_actuators_asctec.srcs += $(SRC_ARCH)/led_hw.c
-
-test_actuators_asctec.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_actuators_asctec.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_actuators_asctec.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_actuators_asctec.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_actuators_asctec.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_actuators_asctec.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_actuators_asctec.srcs += downlink.c pprz_transport.c
-
-test_actuators_asctec.srcs += $(SRC_FIRMWARE)/commands.c
-test_actuators_asctec.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
-test_actuators_asctec.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
-# -DBOOZ_START_DELAY=3
-#  -DUSE_TIM2_IRQ
-test_actuators_asctec.CFLAGS += -DUSE_I2C1
-test_actuators_asctec.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-
-
-
-
-#
-# test motor controllers asctec
-#
-test_mc3.ARCHDIR = $(ARCH)
-test_mc3.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_mc3.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_mc3.srcs = $(SRC_LISA)/test_mc3.c      \
-                $(SRC_LISA)/exceptions.c    \
-                $(SRC_LISA)/vector_table.c
-test_mc3.CFLAGS += -DUSE_LED
-test_mc3.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_mc3.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_mc3.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-
-
-
-#
-# test baro
-#
-test_baro.ARCHDIR = $(ARCH)
-test_baro.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_baro.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_baro.srcs = $(SRC_LISA)/test_baro.c      \
-                 $(SRC_ARCH)/stm32_exceptions.c   \
-                 $(SRC_ARCH)/stm32_vector_table.c
-test_baro.CFLAGS += -DUSE_LED
-test_baro.srcs += $(SRC_ARCH)/led_hw.c
-test_baro.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_baro.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_baro.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_baro.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_baro.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_baro.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_baro.srcs += downlink.c pprz_transport.c
-
-
-#
-# test baro with interrupts
-#
-test_baro2.ARCHDIR = $(ARCH)
-test_baro2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_baro2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_baro2.srcs = $(SRC_LISA)/test_baro2.c      \
-                 $(SRC_ARCH)/stm32_exceptions.c   \
-                 $(SRC_ARCH)/stm32_vector_table.c
-test_baro2.CFLAGS += -DUSE_LED
-test_baro2.srcs += $(SRC_ARCH)/led_hw.c
-test_baro2.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_baro2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_baro2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_baro2.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_baro2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_baro2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_baro2.srcs += downlink.c pprz_transport.c
-
-test_baro2.srcs += $(SRC_LISA)/lisa_baro.c
-test_baro2.CFLAGS += -DUSE_I2C2
-test_baro2.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-#
-# another baro test with interrupts
-#
-test_baro3.ARCHDIR = $(ARCH)
-test_baro3.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_baro3.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_baro3.srcs = $(SRC_LISA)/test_baro3.c      \
-                  $(SRC_ARCH)/stm32_exceptions.c   \
-                  $(SRC_ARCH)/stm32_vector_table.c
-test_baro3.CFLAGS += -DUSE_LED
-test_baro3.srcs += $(SRC_ARCH)/led_hw.c
-test_baro3.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_baro3.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_baro3.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_baro3.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_baro3.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_baro3.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_baro3.srcs += downlink.c pprz_transport.c
-
-test_baro3.CFLAGS += -DUSE_I2C2
-test_baro3.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-
-
-#
-# test spi slave ( hardcoded SPI without DMA )
-#
-test_spi_slave.ARCHDIR = $(ARCH)
-test_spi_slave.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_spi_slave.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_spi_slave.srcs = $(SRC_LISA)/test_spi_slave.c     \
-                      $(SRC_ARCH)/stm32_exceptions.c   \
-                      $(SRC_ARCH)/stm32_vector_table.c
-test_spi_slave.CFLAGS += -DUSE_LED
-test_spi_slave.srcs += $(SRC_ARCH)/led_hw.c
-test_spi_slave.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_spi_slave.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_spi_slave.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_spi_slave.CFLAGS += -DUSE_SPI1_IRQ
-test_spi_slave.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_spi_slave.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_spi_slave.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_spi_slave.srcs += downlink.c pprz_transport.c
-
-
-#
-# test spi slave2  ( hardcoded SPI with DMA )
-#
-test_spi_slave2.ARCHDIR = $(ARCH)
-test_spi_slave2.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_spi_slave2.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_spi_slave2.srcs = $(SRC_LISA)/test_spi_slave2.c    \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-test_spi_slave2.CFLAGS += -DUSE_LED
-test_spi_slave2.srcs += $(SRC_ARCH)/led_hw.c
-test_spi_slave2.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_spi_slave2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_spi_slave2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-test_spi_slave2.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_spi_slave2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_spi_slave2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_spi_slave2.srcs += downlink.c pprz_transport.c
-
-
-
-#
-# test spi link between overo and stm32
-#
-stm_test_spi_link.ARCHDIR = $(ARCH)
-stm_test_spi_link.CFLAGS += -Ilisa -Ilisa/arch/$(ARCH) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-stm_test_spi_link.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-stm_test_spi_link.srcs += lisa/stm_test_spi_link.c       \
-                      $(SRC_ARCH)/stm32_exceptions.c   \
-                  $(SRC_ARCH)/stm32_vector_table.c
-
-stm_test_spi_link.CFLAGS += -DUSE_LED
-stm_test_spi_link.srcs += $(SRC_ARCH)/led_hw.c
-
-stm_test_spi_link.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-stm_test_spi_link.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-stm_test_spi_link.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-stm_test_spi_link.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-stm_test_spi_link.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-stm_test_spi_link.CFLAGS += -DUSE_OVERO_LINK -DOVERO_LINK_MSG_UP=AutopilotMessageFoo -DOVERO_LINK_MSG_DOWN=AutopilotMessageFoo
-stm_test_spi_link.CFLAGS += -DOVERO_LINK_LED_OK=3 -DOVERO_LINK_LED_KO=2 -DUSE_DMA1_C2_IRQ
-stm_test_spi_link.srcs += lisa/lisa_overo_link.c lisa/arch/stm32/lisa_overo_link_arch.c
-
-
-
-
-
-#
-# test static
-#
-test_static.ARCHDIR = $(ARCH)
-test_static.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_static.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_static.srcs = $(SRC_LISA)/test_static.c \
-                   $(SRC_ARCH)/stm32_exceptions.c   \
-                   $(SRC_ARCH)/stm32_vector_table.c
-
-
-
-#
-# test SC18IS600
-#
-test_sc18is600.ARCHDIR = $(ARCH)
-test_sc18is600.CFLAGS  =  -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_sc18is600.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-test_sc18is600.srcs += lisa/test/lisa_test_sc18is600.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-
-test_sc18is600.CFLAGS += -DUSE_LED
-test_sc18is600.srcs += $(SRC_ARCH)/led_hw.c
-
-test_sc18is600.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_sc18is600.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./128.))'
-test_sc18is600.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_sc18is600.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_sc18is600.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_sc18is600.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_sc18is600.srcs += downlink.c pprz_transport.c
-
-test_sc18is600.srcs += math/pprz_trig_int.c
-
-test_sc18is600.CFLAGS += -DUSE_EXTI2_IRQ  -DUSE_DMA1_C4_IRQ
-test_sc18is600.srcs += peripherals/sc18is600.c \
-                       $(SRC_ARCH)/peripherals/sc18is600_arch.c
-
-
-#
-# test Max1168
-#
-test_max1168.ARCHDIR = $(ARCH)
-test_max1168.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_max1168.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_max1168.srcs = $(SRC_LISA)/test/lisa_test_max1168.c \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-
-test_max1168.CFLAGS += -DUSE_LED
-test_max1168.srcs += $(SRC_ARCH)/led_hw.c
-
-test_max1168.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_max1168.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_max1168.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_max1168.CFLAGS += -DUSE_SPI2 -DUSE_EXTI2_IRQ -DUSE_DMA1_C4_IRQ -DMAX1168_HANDLES_DMA_IRQ
-test_max1168.srcs   += peripherals/max1168.c \
-                       $(SRC_ARCH)/peripherals/max1168_arch.c
-
-test_max1168.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_max1168.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_max1168.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_max1168.srcs += downlink.c pprz_transport.c
-
-#
-# test ms2100
-#
-test_ms2100.ARCHDIR = $(ARCH)
-test_ms2100.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_ms2100.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_ms2100.srcs = $(SRC_LISA)/test/lisa_test_ms2100.c \
-                   $(SRC_ARCH)/stm32_exceptions.c   \
-                   $(SRC_ARCH)/stm32_vector_table.c
-
-test_ms2100.CFLAGS += -DUSE_LED
-test_ms2100.srcs += $(SRC_ARCH)/led_hw.c
-
-test_ms2100.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_ms2100.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_ms2100.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_ms2100.CFLAGS += -DUSE_SPI2
-test_ms2100.CFLAGS += -DUSE_DMA1_C4_IRQ -DMS2100_HANDLES_DMA_IRQ
-test_ms2100.CFLAGS += -DUSE_SPI2_IRQ -DMS2100_HANDLES_SPI_IRQ
-test_ms2100.srcs   += peripherals/ms2100.c \
-                      $(SRC_ARCH)/peripherals/ms2100_arch.c
-
-test_ms2100.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_ms2100.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_ms2100.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
-test_ms2100.srcs += downlink.c pprz_transport.c
-
-#
-# test adxl345
-#
-test_adxl345.ARCHDIR = $(ARCH)
-test_adxl345.CFLAGS  =  -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_adxl345.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-test_adxl345.srcs += lisa/test/lisa_test_adxl345.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-
-test_adxl345.CFLAGS += -DUSE_LED
-test_adxl345.srcs += $(SRC_ARCH)/led_hw.c
-
-test_adxl345.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_adxl345.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_adxl345.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_adxl345.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_adxl345.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_adxl345.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_adxl345.srcs += downlink.c pprz_transport.c
-
-test_adxl345.CFLAGS += -DUSE_EXTI2_IRQ   # Acc  Int on PD2
-
-#
-# test adxl345 with DMA
-#
-test_adxl345_dma.ARCHDIR = $(ARCH)
-test_adxl345_dma.CFLAGS  =  -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_adxl345_dma.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-test_adxl345_dma.srcs += lisa/test/lisa_test_adxl345_dma.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-
-test_adxl345_dma.CFLAGS += -DUSE_LED
-test_adxl345_dma.srcs += $(SRC_ARCH)/led_hw.c
-
-test_adxl345_dma.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_adxl345_dma.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_adxl345_dma.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_adxl345_dma.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_adxl345_dma.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_adxl345_dma.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_adxl345_dma.srcs += downlink.c pprz_transport.c
-
-test_adxl345_dma.CFLAGS += -DUSE_EXTI2_IRQ   # Accel Int on PD2
-test_adxl345_dma.CFLAGS += -DUSE_DMA1_C4_IRQ # SPI2 Rx DMA
-
-
-
-
-#
-# test ITG3200
-#
-test_itg3200.ARCHDIR = $(ARCH)
-test_itg3200.CFLAGS  =  -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_itg3200.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-test_itg3200.srcs += lisa/test/lisa_test_itg3200.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-
-test_itg3200.CFLAGS += -DUSE_LED
-test_itg3200.srcs += $(SRC_ARCH)/led_hw.c
-
-test_itg3200.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_itg3200.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_itg3200.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_itg3200.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_itg3200.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_itg3200.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_itg3200.srcs += downlink.c pprz_transport.c
-
-test_itg3200.CFLAGS += -DUSE_I2C2
-test_itg3200.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_itg3200.CFLAGS += -DUSE_EXTI15_10_IRQ   # Gyro Int on PC14
-
-
-#
-# test hmc5843
-#
-test_hmc5843.ARCHDIR = $(ARCH)
-test_hmc5843.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -Ibooz -DPERIPHERALS_AUTO_INIT
-test_hmc5843.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_hmc5843.srcs = lisa/test/lisa_test_hmc5843.c         \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-test_hmc5843.CFLAGS += -DUSE_LED
-test_hmc5843.srcs += $(SRC_ARCH)/led_hw.c
-test_hmc5843.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_hmc5843.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_hmc5843.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_hmc5843.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_hmc5843.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_hmc5843.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_hmc5843.srcs += downlink.c pprz_transport.c
-
-test_hmc5843.CFLAGS += -DUSE_I2C2
-test_hmc5843.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_hmc5843.CFLAGS += -DIMU_OVERRIDE_CHANNELS
-test_hmc5843.CFLAGS += -DUSE_EXTI9_5_IRQ   # Mag Int on PB5
-
-
-
-
-
-#
-# test Aspirin ( rewired ) no sc18is600
-#
-test_aspirin.ARCHDIR = $(ARCH)
-test_aspirin.CFLAGS  =  -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_aspirin.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-test_aspirin.srcs += lisa/test/lisa_test_aspirin.c \
-                       $(SRC_ARCH)/stm32_exceptions.c   \
-                       $(SRC_ARCH)/stm32_vector_table.c
-
-test_aspirin.CFLAGS += -DUSE_LED
-test_aspirin.srcs += $(SRC_ARCH)/led_hw.c
-
-test_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-test_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_aspirin.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_aspirin.srcs += downlink.c pprz_transport.c
-
-test_aspirin.CFLAGS += -DUSE_I2C2
-test_aspirin.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-test_aspirin.CFLAGS += -DUSE_EXTI2_IRQ   # Gyro Int
-test_aspirin.CFLAGS += -DUSE_EXTI3_IRQ   # Mag  Int
-test_aspirin.CFLAGS += -DUSE_EXTI4_IRQ   # Acc  Int
-
-
-
-
-#
-#
-# Passing  STM32 telemetry through WIFI
-#
-#
-
-ptw.ARCHDIR = stm32
-ptw.CFLAGS += -I$(SRC_LISA) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-ptw.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-ptw.srcs = $(SRC_LISA)/test/lisa_test_stm_wifi_telemetry.c \
-          $(SRC_ARCH)/stm32_exceptions.c   \
-          $(SRC_ARCH)/stm32_vector_table.c
-
-# Leds
-ptw.CFLAGS += -DUSE_LED
-ptw.srcs += $(SRC_ARCH)/led_hw.c
-
-# Sys time
-ptw.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-ptw.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-ptw.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-# Link Overo
-ptw.CFLAGS += -DUSE_OVERO_LINK -DOVERO_LINK_MSG_UNION=AutopilotMessageTW
-ptw.CFLAGS += -DOVERO_LINK_LED_OK=3 -DOVERO_LINK_LED_KO=4 -DUSE_DMA1_C2_IRQ
-ptw.srcs += lisa/lisa_overo_link.c lisa/arch/stm32/lisa_overo_link_arch.c
-
-# Telemetry
-ptw.CFLAGS += -DUSE_OVERO_LINK_TELEMETRY
-#ptw.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-#ptw.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-ptw.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=OveroLinkTelemetry
-ptw.srcs += downlink.c pprz_transport.c
-
-# IMU
-ptw.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
-ptw.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
-ptw.srcs += $(SRC_SUBSYSTEMS)/imu.c
-ptw.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
-ptw.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
-ptw.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-ptw.srcs += peripherals/ms2100.c  $(SRC_ARCH)/peripherals/ms2100_arch.c
-ptw.srcs += math/pprz_trig_int.c
-
-ptw.srcs += $(SRC_FIRMWARE)/commands.c
-
-# Radio control
-ptw.CFLAGS += -DRADIO_CONTROL
-ptw.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
-ptw.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=\"subsystems/radio_control/spektrum_dx7se.h\"
-ptw.srcs += $(SRC_SUBSYSTEMS)/radio_control.c \
-           $(SRC_BOOZ)/subsystems/radio_control/spektrum.c
-ptw.CFLAGS += -DRADIO_CONTROL_LED=6
-ptw.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B115200
-ptw.CFLAGS += -DRADIO_CONTROL_LINK=Uart3
-ptw.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-# Actuators
-ptw.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
-ptw.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
-#ptw.srcs += $(SRC_BOOZ_ARCH)/actuators/actuators_mkk_arch.c
-ptw.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-ptw.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1  -DUSE_TIM2_IRQ
-ptw.CFLAGS += -DUSE_I2C1
-
-#
-# test csc servo
-#
-test_csc_servo.ARCHDIR = $(ARCH)
-test_csc_servo.CFLAGS = -I $(SRC_CSC) -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_csc_servo.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_csc_servo.srcs = $(SRC_CSC)/csc_protocol.c \
-        $(SRC_LISA)/test_csc_servo.c      \
-        $(SRC_ARCH)/stm32_exceptions.c   \
-        $(SRC_ARCH)/stm32_vector_table.c
-test_csc_servo.CFLAGS += -DUSE_LED
-test_csc_servo.srcs += $(SRC_ARCH)/led_hw.c
-test_csc_servo.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_csc_servo.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_csc_servo.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_csc_servo.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_csc_servo.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_csc_servo.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart2
-
-test_csc_servo.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_csc_servo.srcs += downlink.c pprz_transport.c
-
-# setting CAN prescaler to generate 3MHz time quanta, drift compensiation to 1
-# time quanta, bit section 1 to 3 time quanta and bit section 2 to 4 time quanta
-# resulting in a 375kHz CAN bitrate expected by the CSC.
-test_csc_servo.CFLAGS += \
-    -DUSE_CAN1 \
-    -DUSE_USB_LP_CAN1_RX0_IRQ \
-    -DCAN_PRESCALER=12 \
-    -DCAN_SJW_TQ=CAN_SJW_1tq \
-    -DCAN_BS1_TQ=CAN_BS1_3tq \
-    -DCAN_BS2_TQ=CAN_BS2_4tq \
-    -DCAN_ERR_RESUME=DISABLE
-test_csc_servo.srcs += can.c $(SRC_ARCH)/can_hw.c
-
-
-
-
-#
-# test GPS
-#
-test_gps.ARCHDIR = $(ARCH)
-test_gps.CFLAGS = -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_gps.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-test_gps.srcs += $(SRC_BOOZ_TEST)/booz2_test_gps.c \
-         $(SRC_ARCH)/stm32_exceptions.c    \
-         $(SRC_ARCH)/stm32_vector_table.c
-test_gps.CFLAGS += -DUSE_LED
-test_gps.srcs += $(SRC_ARCH)/led_hw.c
-test_gps.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_gps.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIME_LED=1
-test_gps.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_gps.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_gps.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_gps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_gps.srcs += downlink.c pprz_transport.c
-
-test_gps.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400
-test_gps.CFLAGS += -DGPS_LINK=Uart1 -DGPS_LED=3
-test_gps.srcs += $(SRC_BOOZ)/booz_gps.c
-#test_gps.CFLAGS += -DBOOZ_GPS_TYPE_H=\"gps/booz_gps_ubx.h\"
-#test_gps.srcs += $(SRC_BOOZ)/gps/booz_gps_ubx.c
-test_gps.CFLAGS += -DBOOZ_GPS_TYPE_H=\"gps/booz_gps_skytraq.h\"
-test_gps.srcs += $(SRC_BOOZ)/gps/booz_gps_skytraq.c
-
-
-
-#
-# test ADC
-#
-# test_adc.ARCHDIR = $(ARCH)
-# test_adc.CFLAGS = -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-# test_adc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-# test_adc.srcs += $(SRC_LISA)/test/lisa_test_adc.c \
-#        $(SRC_ARCH)/stm32_exceptions.c   \
-#        $(SRC_ARCH)/stm32_vector_table.c
-# test_adc.CFLAGS += -DUSE_LED
-# test_adc.srcs += $(SRC_ARCH)/led_hw.c
-# test_adc.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-# test_adc.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIME_LED=1
-# test_adc.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-#
-# test_adc.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-# test_adc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-#
-# test_adc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-# test_adc.srcs += downlink.c pprz_transport.c
-#
-# test_adc.srcs += $(SRC_ARCH)/adc_hw.c
-
-#
-# test adc
-#
-test_adc.ARCHDIR = $(ARCH)
-test_adc.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
-test_adc.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_adc.srcs = $(SRC_ARCH)/adc_hw.c \
-        $(SRC_LISA)/test_adc.c \
-        $(SRC_ARCH)/stm32_exceptions.c \
-        $(SRC_ARCH)/stm32_vector_table.c
-test_adc.CFLAGS += -DUSE_LED
-test_adc.srcs += $(SRC_ARCH)/led_hw.c
-test_adc.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_adc.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_adc.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_adc.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_adc.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_adc.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart2
-
-test_adc.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_adc.srcs += downlink.c pprz_transport.c
-
-test_adc.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4 -DUSE_ADC1_2_IRQ_HANDLER
-
-################################################################################
-#
-#
-#  Hardware test suite
-#
-#
-################################################################################
-test_board.ARCHDIR = $(ARCH)
-test_board.CFLAGS = -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-test_board.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_board.srcs = $(SRC_LISA)/test/test_board.c       \
-                     $(SRC_ARCH)/stm32_exceptions.c   \
-                     $(SRC_ARCH)/stm32_vector_table.c
-test_board.CFLAGS += -DUSE_LED
-test_board.srcs += $(SRC_ARCH)/led_hw.c
-test_board.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-test_board.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
-test_board.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-test_board.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-test_board.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-test_board.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart2
-
-test_board.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-test_board.srcs += downlink.c pprz_transport.c
-
-test_board.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
-test_board.CFLAGS += -DUSE_UART3 -DUART3_BAUD=B57600
-
-test_board.srcs += $(SRC_LISA)/lisa_baro.c
-test_board.CFLAGS += -DUSE_I2C2
-test_board.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-
-test_board.CFLAGS += -DUSE_I2C1
-
-test_board.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm.c $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
-
-
-
-
-################################################################################
-#
-#
-#  Tools for IMUs comparison
-#
-#
-################################################################################
-
-#
-# Spits every samples of one axis of gyro on IMU aspirin
-#
-hs_gyro_aspirin.ARCHDIR = $(ARCH)
-hs_gyro_aspirin.CFLAGS  =  -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-hs_gyro_aspirin.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-hs_gyro_aspirin.srcs += lisa/test/hs_gyro.c \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-
-hs_gyro_aspirin.CFLAGS += -DUSE_LED
-hs_gyro_aspirin.srcs += $(SRC_ARCH)/led_hw.c
-
-hs_gyro_aspirin.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-hs_gyro_aspirin.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-hs_gyro_aspirin.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-hs_gyro_aspirin.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-hs_gyro_aspirin.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-hs_gyro_aspirin.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-hs_gyro_aspirin.srcs += downlink.c pprz_transport.c
-
-hs_gyro_aspirin.srcs += math/pprz_trig_int.c
-
-hs_gyro_aspirin.CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
-hs_gyro_aspirin.srcs += $(SRC_SUBSYSTEMS)/imu.c             \
-                        $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
-                        $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_aspirin_arch.c
-
-hs_gyro_aspirin.CFLAGS += -DUSE_I2C2
-hs_gyro_aspirin.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
-hs_gyro_aspirin.CFLAGS += -DUSE_EXTI15_10_IRQ  # Gyro Int on PC14
-hs_gyro_aspirin.CFLAGS += -DUSE_EXTI9_5_IRQ    # Mag Int on PB5
-hs_gyro_aspirin.CFLAGS += -DUSE_EXTI2_IRQ      # Accel Int on PD2
-hs_gyro_aspirin.CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
-
-
-#
-# Spits every samples of one axis of gyro on IMU b2
-#
-hs_gyro_b2.ARCHDIR = $(ARCH)
-hs_gyro_b2.CFLAGS  =  -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-hs_gyro_b2.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-hs_gyro_b2.srcs += lisa/test/hs_gyro.c \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-
-hs_gyro_b2.CFLAGS += -DUSE_LED
-hs_gyro_b2.srcs += $(SRC_ARCH)/led_hw.c
-
-hs_gyro_b2.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-hs_gyro_b2.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-hs_gyro_b2.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-hs_gyro_b2.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-hs_gyro_b2.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-hs_gyro_b2.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-hs_gyro_b2.srcs += downlink.c pprz_transport.c
-
-hs_gyro_b2.srcs += math/pprz_trig_int.c
-
-hs_gyro_b2.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
-hs_gyro_b2.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2100
-hs_gyro_b2.srcs += $(SRC_SUBSYSTEMS)/imu.c
-hs_gyro_b2.CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
-hs_gyro_b2.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
-hs_gyro_b2.srcs += peripherals/max1168.c $(SRC_ARCH)/peripherals/max1168_arch.c
-hs_gyro_b2.srcs += peripherals/ms2100.c  $(SRC_ARCH)/peripherals/ms2100_arch.c
-
-#
-# Spits every samples of one axis of gyro on IMU crista
-#
-hs_gyro_crista.ARCHDIR = $(ARCH)
-hs_gyro_crista.CFLAGS  =  -I$(SRC_LISA) -I$(ARCH) -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH) -DPERIPHERALS_AUTO_INIT
-hs_gyro_crista.CFLAGS +=  -DBOARD_CONFIG=$(BOARD_CFG)
-hs_gyro_crista.srcs += lisa/test/hs_gyro.c \
-                    $(SRC_ARCH)/stm32_exceptions.c   \
-                    $(SRC_ARCH)/stm32_vector_table.c
-
-hs_gyro_crista.CFLAGS += -DUSE_LED
-hs_gyro_crista.srcs += $(SRC_ARCH)/led_hw.c
-
-hs_gyro_crista.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
-hs_gyro_crista.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-hs_gyro_crista.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-
-hs_gyro_crista.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
-hs_gyro_crista.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-hs_gyro_crista.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart2
-hs_gyro_crista.srcs += downlink.c pprz_transport.c
-
-hs_gyro_crista.srcs += math/pprz_trig_int.c
-
-hs_gyro_crista.CFLAGS += -DIMU_TYPE_H=\"imu/imu_crista.h\" -DIMU_OVERRIDE_CHANNELS
-hs_gyro_crista.srcs += $(SRC_SUBSYSTEMS)/imu.c             \
-                        $(SRC_SUBSYSTEMS)/imu/imu_crista.c \
-                        $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_crista_arch.c
-hs_gyro_crista.CFLAGS += -DUSE_DMA1_C4_IRQ
-
-hs_gyro_crista.CFLAGS += -DMEASURED_SENSOR=gyro_unscaled.p -DMEASURED_SENSOR_NB=0
diff --git a/conf/autopilot/obsolete/mercury.makefile b/conf/autopilot/obsolete/mercury.makefile
deleted file mode 100644
index 0bf68c81ef..0000000000
--- a/conf/autopilot/obsolete/mercury.makefile
+++ /dev/null
@@ -1,142 +0,0 @@
-#
-# $Id$
-#
-# Copyright (C) 2009 Antoine Drouin, Allen H. Ibara
-#
-# This file is part of paparazzi.
-#
-# paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# paparazzi 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 General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA.
-#
-#
-
-ARCHI=arm7
-
-FLASH_MODE = ISP
-LPC21ISP_PORT = /dev/ttyUSB0
-
-LPC21ISP_BAUD = 38400
-LPC21ISP_XTAL = 12000
-
-
-LPC21ISP_CONTROL = -control
-
-LDSCRIPT=$(SRC_ARCH)/LPC2129-ROM.ld
-
-BOARD_CFG = \"csc_board_v1_0.h\"
-
-PERIODIC_FREQ = 512
-
-SRC_CSC=csc
-SRC_CSC_ARCH=$(SRC_CSC)/$(ARCHI)
-SRC_BOOZ=booz
-SRC_SUBSYSTEMS=subsystems
-
-ap.ARCHDIR = $(ARCHI)
-
-ap.CFLAGS += -I$(SRC_CSC) -I$(SRC_BOOZ) -I$(SRC_CSC_ARCH)
-ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-ap.srcs += $(SRC_CSC)/mercury_main.c
-ap.CFLAGS += -DUSE_LED -DTIME_LED=1
-ap.CFLAGS += -DAHRS_ALIGNER_LED=2
-
-ap.CFLAGS += -DCSC_BOARD_ID=$(CSC_ID)
-
-ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-ap.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-ap.srcs += $(SRC_ARCH)/adc_hw.c
-ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_0 -DUSE_AD0_1
-
-
-ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B230400 -DUART0_VIC_SLOT=5
-ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
-                      -DDOWNLINK_DEVICE=Uart1
-ap.srcs += downlink.c pprz_transport.c $(SRC_CSC)/csc_telemetry.c
-ap.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
-ap.srcs += $(SRC_CSC)/csc_datalink.c
-
-ap.CFLAGS += -DPPRZ_TRIG_CONST=const
-ap.srcs += $(SRC_CSC)/mercury_xsens.c $(SRC_SUBSYSTEMS)/imu.c math/pprz_trig_int.c
-ap.CFLAGS += -DXSENS1_LINK=Uart0 -DIMU_TYPE_H=\"mercury_xsens.h\"
-
-ap.srcs += $(SRC_BOOZ)/ahrs/ahrs_cmpl_euler.c $(SRC_BOOZ)/ahrs/ahrs_aligner.c
-ap.CFLAGS += -DFLOAT_T=float
-
-ap.srcs += $(SRC_FIRMWARE)/stabilization.c
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_rate.c
-
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_euler_int.h\"
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_euler_int.c
-ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_euler_int.c
-
-
-# AHI copied from booz w/ light modifications for vertical control
-ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
-ap.srcs += $(SRC_CSC)/csc_booz2_ins.c $(SRC_CSC)/csc_booz2_hf_float.c $(SRC_CSC)/csc_booz2_vf_float.c
-ap.srcs += $(SRC_CSC)/csc_booz2_guidance_v.c
-
-
-ap.CFLAGS += -DAP_LINK_CAN # -DCAN_LED=2
-ap.CFLAGS += -DUSE_CAN1 -DCAN1_BTR=CANBitrate125k_3MHz
-ap.CFLAGS +=  -DCAN1_VIC_SLOT=3 -DCAN1_ERR_VIC_SLOT=7
-ap.srcs += $(SRC_CSC)/csc_can.c
-#ap.CFLAGS += -DUSE_CAN2 -DCAN2_BTR=CANBitrate125k_2MHz -DCAN2_VIC_SLOT=4
-
-ap.srcs += $(SRC_CSC)/csc_ap_link.c
-
-ap.srcs += $(SRC_CSC)/csc_adc.c
-
-ap.srcs += $(SRC_ARCH)/servos_csc.c
-ap.CFLAGS += -DACTUATORS=\"servos_csc.h\"
-ap.srcs += commands.c actuators.c
-
-ap.srcs += $(SRC_CSC_ARCH)/props_csc.c
-
-ap.CFLAGS += -DRADIO_CONTROL
-ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
-
-ap.srcs += $(SRC_CSC)/mercury_ap.c
-ap.srcs += $(SRC_CSC)/mercury_supervision.c
-
-
-ap.CFLAGS += -DERROR_LED=4
-
-#
-#
-# test uart
-#
-test_uart.ARCHDIR = $(ARCHI)
-
-
-test_uart.CFLAGS += -I$(SRC_CSC)
-test_uart.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_uart.srcs += $(SRC_CSC)/csc_test_uart.c
-test_uart.CFLAGS += -DUSE_LED
-
-# -DTIME_LED=1
-test_uart.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-test_uart.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-test_uart.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_uart.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-test_uart.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
-                      -DDOWNLINK_DEVICE=Uart1
-test_uart.srcs += downlink.c pprz_transport.c
diff --git a/conf/autopilot/obsolete/mercury_csc.makefile b/conf/autopilot/obsolete/mercury_csc.makefile
deleted file mode 100644
index b8f5cd04e8..0000000000
--- a/conf/autopilot/obsolete/mercury_csc.makefile
+++ /dev/null
@@ -1,134 +0,0 @@
-#
-# $Id$
-#
-# Copyright (C) 2009 Antoine Drouin
-#
-# This file is part of paparazzi.
-#
-# paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# paparazzi 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 General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA.
-#
-#
-
-ARCH=lpc21
-
-FLASH_MODE = ISP
-LPC21ISP_PORT = /dev/ttyUSB0
-
-LPC21ISP_BAUD = 38400
-LPC21ISP_XTAL = 12000
-
-
-LPC21ISP_CONTROL = -control
-
-LDSCRIPT=$(SRC_ARCH)/LPC2129-ROM.ld
-
-BOARD_CFG = \"csc_board_v1_0.h\"
-
-
-
-SRC_CSC=csc
-SRC_CSC_ARCH=$(SRC_CSC)/$(ARCH)
-
-ap.ARCHDIR = $(ARCH)
-
-ap.CFLAGS += -I$(SRC_CSC) -I$(SRC_CSC_ARCH)
-ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-ap.srcs += $(SRC_CSC)/mercury_csc_main.c
-ap.CFLAGS += -DUSE_LED -DTIME_LED=1
-
-ap.CFLAGS += -DCSC_BOARD_ID=$(CSC_ID)
-
-ap.CFLAGS += -DUSE_PWM_INPUT
-ap.CFLAGS += -DUSE_PWM_INPUT1
-ap.CFLAGS += -DUSE_PWM_INPUT2
-ap.CFLAGS += -DUSE_VANE_SENSOR
-# ap.CFLAGS += -DUSE_BAT_MONITOR
-
-ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-ap.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-#ap.srcs += $(SRC_ARCH)/adc_hw.c
-#ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_0 -DUSE_AD0_1
-
-
-#ap.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-#ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
-#					  -DDOWNLINK_DEVICE=Uart0
-#ap.srcs += downlink.c pprz_transport.c
-
-
-ap.CFLAGS += -DAP_LINK_CAN -DCAN_LED=2
-ap.CFLAGS += -DUSE_CAN1 #-DCAN1_BTR=CANBitrate125k_3MHz
-ap.CFLAGS +=  -DCAN1_VIC_SLOT=3 -DCAN1_ERR_VIC_SLOT=7
-ap.srcs += $(SRC_CSC)/csc_can.c
-ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600 -DUART0_VIC_SLOT=5
-#ap.CFLAGS += -DUSE_CAN2 -DCAN2_BTR=CANBitrate125k_2MHz -DCAN2_VIC_SLOT=4
-
-ap.CFLAGS += -DACTUATORS=\"servos_direct_hw.h\"
-ap.srcs += actuators.c
-ap.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-ap.srcs += $(SRC_ARCH)/servos_direct_hw.c
-ap.srcs += $(SRC_CSC)/csc_servos.c
-ap.srcs += $(SRC_CSC)/csc_bat_monitor.c
-ap.srcs += $(SRC_CSC)/csc_ap_link.c
-ap.srcs += $(SRC_CSC)/csc_adc.c
-
-ap.CFLAGS += -DERROR_LED=4
-
-#
-#
-# test uart
-#
-test_uart.ARCHDIR = $(ARCH)
-
-
-test_uart.CFLAGS += -I$(SRC_CSC)
-test_uart.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_uart.srcs += $(SRC_CSC)/csc_test_uart.c
-test_uart.CFLAGS += -DUSE_LED
-
-# -DTIME_LED=1
-test_uart.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-test_uart.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-test_uart.srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c
-
-test_uart.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DUART1_VIC_SLOT=6
-test_uart.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport \
-					  -DDOWNLINK_DEVICE=Uart1
-test_uart.srcs += downlink.c pprz_transport.c
-
-
-#
-# TEST CAN1
-#
-
-test_can1.ARCHDIR = $(ARCH)
-
-
-test_can1.CFLAGS += -I$(SRC_CSC)
-test_can1.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_can1.srcs += $(SRC_CSC)/test_can1.c
-test_can1.CFLAGS += -DUSE_LED
-
-# -DTIME_LED=1
-test_can1.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))' -DTIMER0_VIC_SLOT=1
-test_can1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-test_can1.CFLAGS += -DUSE_CAN1 -DCAN1_BTR=CANBitrate125k_2MHz
-test_can1.CFLAGS +=  -DCAN1_VIC_SLOT=3 -DCAN1_ERR_VIC_SLOT=7
-test_can1.srcs += $(SRC_CSC)/csc_can.c
-test_can1.CFLAGS += -DCSC_BOARD_ID=0
diff --git a/conf/autopilot/obsolete/pwm_input.makefile b/conf/autopilot/obsolete/pwm_input.makefile
deleted file mode 100644
index bf778afe89..0000000000
--- a/conf/autopilot/obsolete/pwm_input.makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-# Generic PWM input on lpc21xx using TIMER0 ISR
-ap.CFLAGS += -DUSE_PWM_INPUT -DUSE_PWM_INPUT1 -DUSE_PWM_INPUT2
-ap.srcs += $(SRC_ARCH)/pwm_input.c
-
diff --git a/conf/autopilot/obsolete/sitl.makefile b/conf/autopilot/obsolete/sitl.makefile
deleted file mode 100644
index b05421888e..0000000000
--- a/conf/autopilot/obsolete/sitl.makefile
+++ /dev/null
@@ -1,27 +0,0 @@
-sim.ARCHDIR = $(ARCH)
-sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO
-sim.srcs += latlong.c\
-            radio_control.c\
-            downlink.c\
-            commands.c\
-            gps.c\
-            inter_mcu.c\
-            subsystems/sensors/infrared.c\
-            $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c\
-            $(SRC_FIRMWARE)/guidance/guidance_v.c                 \
-            subsystems/nav.c\
-            estimator.c\
-            sys_time.c\
-            $(SRC_FIRMWARE)/main_fbw.c \
-            $(SRC_FIRMWARE)/main_ap.c \
-            $(SRC_FIRMWARE)/datalink.c \
-            $(SRC_ARCH)/ppm_hw.c \
-            $(SRC_ARCH)/sim_gps.c\
-            $(SRC_ARCH)/sim_ir.c \
-            $(SRC_ARCH)/sim_ap.c \
-            $(SRC_ARCH)/ivy_transport.c \
-            $(SRC_ARCH)/sim_adc_generic.c \
-            $(SRC_ARCH)/led_hw.c
-
-
-
diff --git a/conf/autopilot/obsolete/sitl_jsbsim.makefile b/conf/autopilot/obsolete/sitl_jsbsim.makefile
deleted file mode 100644
index 4b46e00a19..0000000000
--- a/conf/autopilot/obsolete/sitl_jsbsim.makefile
+++ /dev/null
@@ -1,39 +0,0 @@
-
-ifndef JSBSIM_INC
-JSBSIM_ROOT = /opt/jsbsim
-JSBSIM_INC = $(JSBSIM_ROOT)/include/JSBSim
-JSBSIM_LIB = $(JSBSIM_ROOT)/lib
-endif
-
-jsbsim.ARCHDIR = $(ARCH)
-
-# external libraries
-jsbsim.CFLAGS = -I$(SIMDIR) -I/usr/include `pkg-config glib-2.0 --cflags`
-jsbsim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -L/usr/lib
-
-# use the paparazzi-jsbsim package if it is installed, otherwise look for JSBsim under /opt/jsbsim
-ifndef JSBSIM_PKG
-JSBSIM_PKG = $(shell pkg-config JSBSim --exists && echo 'yes')
-endif
-ifeq ($(JSBSIM_PKG), yes)
-	jsbsim.CFLAGS  += `pkg-config JSBSim --cflags`
-	jsbsim.LDFLAGS += `pkg-config JSBSim --libs`
-else
-	JSBSIM_PKG = no
-	jsbsim.CFLAGS  += -I$(JSBSIM_INC)
-	jsbsim.LDFLAGS += -L$(JSBSIM_LIB) -lJSBSim
-endif
-
-jsbsim.CFLAGS += -DSITL -DAP -DFBW -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO -Ifirmwares/fixedwing
-jsbsim.srcs = $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_transport.c $(SRC_ARCH)/ivy_transport.c
-jsbsim.srcs += latlong.c downlink.c commands.c gps.c inter_mcu.c subsystems/sensors/infrared.c \
-               $(SRC_FIXEDWING)/stabilization/stabilization_attitude.c \
-               $(SRC_FIXEDWING)/guidance/guidance_v.c\
-               subsystems/nav.c estimator.c sys_time.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main_ap.c $(SRC_FIRMWARE)/datalink.c
-jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c
-# Choose in your airframe file type of airframe
-# jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
-# jsbsim.srcs += $(SIMDIR)/sim_ac_booz.c
-
-#jsbsim.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/ppm.h\" -DRADIO_CONTROL_TYPE_PPM
-#jsbsim.srcs	+= radio_control.c radio_control/ppm.c $(SRC_ARCH)/radio_control/ppm_arch.c
diff --git a/conf/autopilot/obsolete/sitl_link_pprz.makefile b/conf/autopilot/obsolete/sitl_link_pprz.makefile
deleted file mode 100644
index 313d5ee07f..0000000000
--- a/conf/autopilot/obsolete/sitl_link_pprz.makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-sim.ARCHDIR = $(ARCH)
-sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DUSE_INFRARED -DRADIO_CONTROL_SETTINGS -DSIM_UART -DDOWNLINK_AP_DEVICE=SimUart -DDOWNLINK_FBW_DEVICE=SimUart -DDATALINK=PPRZ
-sim.srcs = radio_control.c downlink.c pprz_transport.c commands.c gps.c inter_mcu.c subsystems/sensors/infrared.c $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c $(SRC_FIRMWARE)/guidance/guidance_v.c nav.c estimator.c cam.c sys_time.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main_ap.c rc_settings.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c  $(SRC_ARCH)/sim_uart.c $(SRC_FIRMWARE)/datalink.c
diff --git a/conf/autopilot/obsolete/sitl_link_xbee.makefile b/conf/autopilot/obsolete/sitl_link_xbee.makefile
deleted file mode 100644
index 6928643999..0000000000
--- a/conf/autopilot/obsolete/sitl_link_xbee.makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-sim.ARCHDIR = $(ARCH)
-sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=XBeeTransport -DUSE_INFRARED -DRADIO_CONTROL_SETTINGS -DSIM_UART -DSIM_XBEE -DXBEE_UART=SimUart
-sim.srcs = radio_control.c downlink.c xbee.c commands.c gps.c inter_mcu.c subsystems/sensors/infrared.c $(SRC_FIXEDWING)/stabilization/stabilization_attitude.c $(SRC_FIXEDWING)/guidance/guidance_v.c nav.c estimator.c cam.c sys_time.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main_ap.c rc_settings.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c  $(SRC_ARCH)/sim_uart.c
diff --git a/conf/autopilot/obsolete/testing.makefile b/conf/autopilot/obsolete/testing.makefile
deleted file mode 100644
index f75a6c8ab0..0000000000
--- a/conf/autopilot/obsolete/testing.makefile
+++ /dev/null
@@ -1,43 +0,0 @@
-## TODO This file probably needs to move to the board specific folder
-
-# a test program to setup actuators
-setup_actuators.ARCHDIR = $(ARCH)
-
-setup_actuators.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny.h\" -DUSE_LED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
-setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4015_hw.c main.c
-
-
-# a test program to tunnel between both uart
-tunnel.ARCHDIR = $(ARCH)
-
-tunnel.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny_2_1_1_usb.h\" -DUSE_LED
-tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c
-
-
-# A test program to monitor the ADC values
-test_adcs.ARCHDIR = $(ARCH)
-
-test_adcs.CFLAGS += -DBOARD_CONFIG=$(CONFIG) -DUSE_LED -DTIME_LED=1 -DUSE_ADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7
-test_adcs.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-test_adcs.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c xbee.c
-
-test_adcs.srcs += sys_time.c $(SRC_ARCH)/mcu_periph/adc_arch.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c test/test_adcs.c
-# pprz_transport.c
-
-
-# a configuration program to access both uart through usb
-usb_tunnel_0.ARCHDIR = $(ARCH)
-usb_tunnel_0.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny_2_1_1_usb.h\" -DUSE_UART0 -DUART0_BAUD=B115200
-usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED
-usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c
-usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c
-usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c
-usb_tunnel_0.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-usb_tunnel_1.ARCHDIR = $(ARCH)
-usb_tunnel_1.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny_2_1_1_usb.h\" -DUSE_UART1 -DUART1_BAUD=B115200
-usb_tunnel_1.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED
-usb_tunnel_1.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c
-usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c
-usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c
-usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
diff --git a/conf/autopilot/obsolete/tiny.makefile b/conf/autopilot/obsolete/tiny.makefile
deleted file mode 100644
index 71df9d2cea..0000000000
--- a/conf/autopilot/obsolete/tiny.makefile
+++ /dev/null
@@ -1,72 +0,0 @@
-# Makefile for the Tiny board (1 arm7tdmi, 1 LEA-LA)
-
-#
-# can we remove this file?
-# the makefiles under conf/boards/ should be used now
-#
-
-ARCHI=lpc21
-ARCH=lpc21
-
-ap.ARCHDIR = $(ARCHI)
-
-fbw.ARCHDIR = $(ARCHI)
-
-LPC21ISP_BAUD = 38400
-LPC21ISP_XTAL = 12000
-
-
-### default settings for tiny_2 and twog
-GPS_UART_NR	= 0
-GPS_BAUD	= 38400
-MODEM_UART_NR = 1
-MODEM_BAUD = 57600
-
-ADC_IR1 = ADC_1
-ADC_IR2 = ADC_2
-ADC_IR_TOP = ADC_0
-ADC_IR_NB_SAMPLES = 16
-ADC_GYRO_ROLL = ADC_3
-ADC_GYRO_NB_SAMPLES = 16
-
-# a test program to setup actuators
-setup_actuators.ARCHDIR = $(ARCHI)
-
-setup_actuators.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny.h\" -DUSE_LED -DTIME_LED=1 -DACTUATORS=\"servos_4015_hw.h\" -DSERVOS_4015 -DUSE_UART0 -DUART0_BAUD=B9600 -DDATALINK=PPRZ -DPPRZ_UART=Uart0
-setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c setup_actuators.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/servos_4015_hw.c main.c
-
-
-# a test program to tunnel between both uart
-tunnel.ARCHDIR = $(ARCHI)
-
-tunnel.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny_2_1_1_usb.h\" -DUSE_LED
-tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c
-
-
-# A test program to monitor the ADC values
-test_adcs.ARCHDIR = $(ARCHI)
-
-test_adcs.CFLAGS += -DBOARD_CONFIG=$(CONFIG) -DUSE_LED -DTIME_LED=1 -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7
-test_adcs.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B9600
-test_adcs.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c xbee.c
-
-test_adcs.srcs += sys_time.c $(SRC_ARCH)/adc_hw.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c test_adcs.c
-# pprz_transport.c
-
-
-# a configuration program to access both uart through usb
-usb_tunnel_0.ARCHDIR = $(ARCHI)
-usb_tunnel_0.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny_2_1_1_usb.h\" -DUSE_UART0 -DUART0_BAUD=B115200
-usb_tunnel_0.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED
-usb_tunnel_0.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c
-usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c
-usb_tunnel_0.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c
-usb_tunnel_0.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
-
-usb_tunnel_1.ARCHDIR = $(ARCHI)
-usb_tunnel_1.CFLAGS += -DFBW -DBOARD_CONFIG=\"tiny_2_1_1_usb.h\" -DUSE_UART1 -DUART1_BAUD=B115200
-usb_tunnel_1.CFLAGS += -DUSE_USB_LINE_CODING -DUSE_USB_SERIAL -DUSE_LED
-usb_tunnel_1.srcs += $(SRC_ARCH)/usb_tunnel.c $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c
-usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbinit.c
-usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdreq.c
-usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
diff --git a/conf/autopilot/obsolete/tiny_2_1_1_usb.h b/conf/autopilot/obsolete/tiny_2_1_1_usb.h
deleted file mode 100644
index 7e1cb57e54..0000000000
--- a/conf/autopilot/obsolete/tiny_2_1_1_usb.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#define USE_USB_HIGH_PCLK
-
-#include "tiny_2_1.h"
-
-#undef DefaultVoltageOfAdc
-#define DefaultVoltageOfAdc(adc) (0.0247311828*adc)
diff --git a/conf/autopilot/obsolete/twin_avr.h b/conf/autopilot/obsolete/twin_avr.h
deleted file mode 100644
index 6a03193ff1..0000000000
--- a/conf/autopilot/obsolete/twin_avr.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#define SLAVE_SPI_PORT   PORTB
-#define SLAVE_SPI_PIN    PINB
-#define SLAVE_SPI_SS_PIN 2
-
-#define SLAVE_SPI_DDR  DDRB
-#define SLAVE_SPI_MOSI_PIN 3
-#define SLAVE_SPI_MISO_PIN 4
-#define SLAVE_SPI_SCK_PIN  5
-
-#define MASTER_SPI_SS0_PIN  0
-#define MASTER_SPI_SS0_PORT PORTB
-#define MASTER_SPI_SS0_DDR  DDRB
-#define MASTER_SPI_IT0_PIN  7
-#define MASTER_SPI_IT0_PORT PORTE
-#define MASTER_SPI_IT0_DDR  DDRE
-
-#define MASTER_SPI_SS1_PIN  3
-#define MASTER_SPI_SS1_PORT PORTE
-#define MASTER_SPI_SS1_DDR  DDRE
-#define MASTER_SPI_IT1_PIN  6
-#define MASTER_SPI_IT1_PORT PORTE
-#define MASTER_SPI_IT1_DDR  DDRE 
-
-#define MASTER_SPI_SS2_PIN  2
-#define MASTER_SPI_SS2_PORT PORTE
-#define MASTER_SPI_SS2_DDR  DDRE
-#define MASTER_SPI_IT2_PIN  5
-#define MASTER_SPI_IT2_PORT PORTE
-#define MASTER_SPI_IT2_DDR  DDRE 
-
-
-#define MASTER_SPI_SCK_PIN  1
-#define MASTER_SPI_MOSI_PIN 2
-#define MASTER_SPI_MISO_PIN 3
-#define MASTER_SPI_PORT PORTB
-#define MASTER_SPI_DDR  DDRB
diff --git a/conf/autopilot/obsolete/twin_avr.makefile b/conf/autopilot/obsolete/twin_avr.makefile
deleted file mode 100644
index a2331af654..0000000000
--- a/conf/autopilot/obsolete/twin_avr.makefile
+++ /dev/null
@@ -1,21 +0,0 @@
-ARCH=avr
-
-ap.ARCHDIR = $(ARCH)
-ap.MCU = atmega128
-ap.LOW_FUSE  = a0
-ap.HIGH_FUSE = 99
-ap.EXT_FUSE  = ff
-ap.LOCK_FUSE = ff
-ap.CFLAGS += -DAP
-
-fbw.ARCHDIR = $(ARCH)
-fbw.MCU = atmega8
-fbw.LOW_FUSE  = 2e
-fbw.HIGH_FUSE = cb
-fbw.EXT_FUSE  = ff
-fbw.LOCK_FUSE = ff
-fbw.CFLAGS += -DFBW
-
-tunnel.ARCHDIR = $(ARCH)
-tunnel.MCU = atmega128
-tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c
diff --git a/conf/autopilot/obsolete/twin_mcu.makefile b/conf/autopilot/obsolete/twin_mcu.makefile
deleted file mode 100644
index 690f741285..0000000000
--- a/conf/autopilot/obsolete/twin_mcu.makefile
+++ /dev/null
@@ -1,6 +0,0 @@
-ap.srcs += $(SRC_FIRMWARE)/main_ap.c sys_time.c $(SRC_FIRMWARE)/main.c inter_mcu.c link_mcu.c gps_ubx.c gps.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c cam.c spi.c rc_settings.c latlong.c subsystems/navigation/nav_survey_rectangle.c
-ap.CFLAGS += -DMCU_SPI_LINK -DUSE_GPS -DUBX -DUSE_INFRARED -DRADIO_CONTROL -DINTER_MCU -DSPI_MASTER -DUSE_SPI -DNAV -DRADIO_CONTROL_SETTINGS
-
-fbw.srcs +=  sys_time.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main.c commands.c radio_control.c pprz_transport.c downlink.c  inter_mcu.c spi.c link_mcu.c
-fbw.CFLAGS += -DRADIO_CONTROL -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DINTER_MCU -DMCU_SPI_LINK -DUART0_BAUD=B38400 -DSPI_SLAVE -DUSE_SPI
-
diff --git a/conf/autopilot/obsolete/twin_mcu_avr.makefile b/conf/autopilot/obsolete/twin_mcu_avr.makefile
deleted file mode 100644
index 04ef99b046..0000000000
--- a/conf/autopilot/obsolete/twin_mcu_avr.makefile
+++ /dev/null
@@ -1,8 +0,0 @@
-include $(PAPARAZZI_SRC)/conf/autopilot/twin_avr.makefile
-include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu.makefile
-
-ap.srcs += $(SRC_ARCH)/adc_hw.c $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/sys_time_hw.c
-ap.CFLAGS += -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
-
-fbw.CFLAGS += -DACTUATORS=\"servos_4017.h\" -DSERVOS_4017 -DADC
-fbw.srcs += actuators.c $(SRC_ARCH)/adc_hw.c $(SRC_ARCH)/servos_4017.c $(SRC_ARCH)/ppm_hw.c  $(SRC_ARCH)/mcu_periph/uart_arch.c $(SRC_ARCH)/spi_hw.c $(SRC_ARCH)/sys_time_hw.c
diff --git a/conf/autopilot/obsolete/twog_booz.h b/conf/autopilot/obsolete/twog_booz.h
deleted file mode 100644
index 52af317962..0000000000
--- a/conf/autopilot/obsolete/twog_booz.h
+++ /dev/null
@@ -1,162 +0,0 @@
-#ifndef CONFIG_TINY_H
-#define CONFIG_TINY_H
-
-#ifdef SITL
-/* Dummy definitions: adc are unused anyway */
-#define AdcBank0(x) (x)
-#define AdcBank1(x) (x)
-#endif /* SITL */
-
-/* Master oscillator freq.       */
-#define FOSC (12000000) 
-
-/* PLL multiplier                */
-#define PLL_MUL (5)         
-
-/* CPU clock freq.               */
-#define CCLK (FOSC * PLL_MUL) 
-
-/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2   */
-#define PBSD_BITS 0x00    
-#define PBSD_VAL 4
-
-/* Peripheral bus clock freq. */
-#define PCLK (CCLK / PBSD_VAL) 
-
-#define LED_1_BANK 1
-#define LED_1_PIN 17
-
-#define LED_2_BANK 1
-#define LED_2_PIN 16
-
-#define LED_3_BANK 1
-#define LED_3_PIN 23
-
-#define LED_4_BANK 1
-#define LED_4_PIN 18
-
-#define POWER_SWITCH_LED 4
-
-#define LED_5_BANK 1
-#define LED_5_PIN 22
-
-#define CAM_SWITCH_LED 5
-
-#define LED_GPS_RESET_BANK 1
-#define LED_GPS_RESET_PIN 21
-
-#define Configure_GPS_RESET_Pin() LED_INIT(GPS_RESET)
-#define Set_GPS_RESET_Pin_LOW() LED_ON(GPS_RESET)
-#define Open_GPS_RESET_Pin() ClearBit(LED_DIR(GPS_RESET), LED_PIN(GPS_RESET))
-
-/* P0.5 aka MAT0.1  */
-#define SERVO_CLOCK_PIN  5
-#define SERVO_CLOCK_PINSEL PINSEL0
-#define SERVO_CLOCK_PINSEL_VAL 0x02
-#define SERVO_CLOCK_PINSEL_BIT 10
-/* p1.20          */
-#define SERVO_RESET_PIN 20
-
-
-
-#define ANALOG_BARO_PINSEL PINSEL0
-#define ANALOG_BARO_PINSEL_VAL 0x03
-#define ANALOG_BARO_PINSEL_BIT 20
-#define ANALOG_BARO_ADC 1
-
-/* PPM : rc rx on P0.6*/
-#define PPM_PINSEL PINSEL0
-#define PPM_PINSEL_VAL 0x02
-#define PPM_PINSEL_BIT 12
-#define PPM_CRI TIR_CR2I
-#define PPM_CCR_CRF TCCR_CR2_F
-#define PPM_CCR_CRR TCCR_CR2_R
-#define PPM_CCR_CRI TCCR_CR2_I
-#define PPM_CR T0CR2
-
-/* ADC */
-
-#define ADC_0 AdcBank1(6)
-#ifdef USE_ADC_0
-#ifndef USE_AD1
-#define USE_AD1
-#endif
-#define USE_AD1_6
-#endif
-
-#define ADC_1 AdcBank1(7)
-#ifdef USE_ADC_1
-#ifndef USE_AD1
-#define USE_AD1
-#endif
-#define USE_AD1_7
-#endif
-
-
-#define ADC_2 AdcBank0(4)
-#ifdef USE_ADC_2
-#ifndef USE_AD0
-#define USE_AD0
-#endif
-#define USE_AD0_4
-#endif
-
-#define ADC_3 AdcBank0(6)
-#ifdef USE_ADC_3
-#ifndef USE_AD0
-#define USE_AD0
-#endif
-#define USE_AD0_6
-#endif
-
-#define ADC_4 AdcBank0(3)
-#ifdef USE_ADC_4
-#ifndef USE_AD0
-#define USE_AD0
-#endif
-#define USE_AD0_3
-#endif
-
-#define ADC_5 AdcBank0(2)
-#ifdef USE_ADC_5
-#ifndef USE_AD0
-#define USE_AD0
-#endif
-#define USE_AD0_2
-#endif
-
-#define ADC_6 AdcBank0(1)
-#ifdef USE_ADC_6
-#ifndef USE_AD0
-#define USE_AD0
-#endif
-#define USE_AD0_1
-#endif
-
-#define ADC_7 AdcBank1(3)
-#ifdef USE_ADC_7
-#ifndef USE_AD1
-#define USE_AD1
-#endif
-#define USE_AD1_3
-#endif
-
-#define ADC_CHANNEL_VSUPPLY AdcBank1(5)
-#ifndef USE_AD1
-#define USE_AD1
-#endif
-#define USE_AD1_5
-
-
-#define DefaultVoltageOfAdc(adc) (0.01787109375*adc)
-
-#define SPI_SELECT_SLAVE0_PORT 0
-#define SPI_SELECT_SLAVE0_PIN 20
-
-#define SPI1_DRDY_PINSEL PINSEL1
-#define SPI1_DRDY_PINSEL_BIT   0
-#define SPI1_DRDY_PINSEL_VAL   1
-#define SPI1_DRDY_EINT         0
-#define SPI1_DRDY_VIC_IT       VIC_EINT0
-
-#endif /* CONFIG_TINY_H */
diff --git a/conf/autopilot/obsolete/v1_2.h b/conf/autopilot/obsolete/v1_2.h
deleted file mode 100644
index 7868158ca7..0000000000
--- a/conf/autopilot/obsolete/v1_2.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef CONFIG_V1_2_H
-#define CONFIG_V1_2_H
-
-#include "twin_avr.h"
-
-/* clock in MHz */
-#define CLOCK 16
-
-/* PPM input on mega8 ICP PB0 */
-#define PPM_DDR  DDRB
-#define PPM_PORT PORTB
-#define PPM_PIN  PB0
-
-/* 4017 servos */
-#define _4017_RESET_PORT        PORTC
-#define _4017_RESET_DDR         DDRC
-#define _4017_RESET_PIN         0
-
-#define _4017_CLOCK_PORT        PORTB
-#define _4017_CLOCK_DDR         DDRB
-#define _4017_CLOCK_PIN         PB1
-
-#define SERVO_OCR		OCR1A
-#define SERVO_ENABLE		OCIE1A
-#define SERVO_FLAG		OCF1A
-#define SERVO_FORCE		FOC1A
-#define SERVO_COM0		COM1A0
-#define SERVO_COM1		COM1A1
-
-#endif /* CONFIG_V1_2_H */
diff --git a/conf/autopilot/obsolete/v1_2.makefile b/conf/autopilot/obsolete/v1_2.makefile
deleted file mode 100644
index a33480eb03..0000000000
--- a/conf/autopilot/obsolete/v1_2.makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-include $(PAPARAZZI_SRC)/conf/autopilot/twin_avr.makefile
-
-LOCAL_CFLAGS += -DCTL_BRD_V1_2 -DBOARD_CONFIG=\"v1_2.h\"
diff --git a/conf/autopilot/obsolete/v1_2_1.h b/conf/autopilot/obsolete/v1_2_1.h
deleted file mode 100644
index 24c50c0418..0000000000
--- a/conf/autopilot/obsolete/v1_2_1.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef CONFIG_V1_2_1_H
-#define CONFIG_V1_2_1_H
-
-#include "twin_avr.h"
-
-/* clock in MHz */
-#define CLOCK 16
-
-/* PPM input on mega8 ICP PB0 */
-#define PPM_DDR  DDRB
-#define PPM_PORT PORTB
-#define PPM_PIN  PB0
-
-/* 4017 servos */
-#define _4017_RESET_PORT        PORTD
-#define _4017_RESET_DDR         DDRD
-#define _4017_RESET_PIN         7
-
-#define _4017_CLOCK_PORT        PORTB
-#define _4017_CLOCK_DDR         DDRB
-#define _4017_CLOCK_PIN         PB1
-
-#define SERVO_OCR		OCR1A
-#define SERVO_ENABLE		OCIE1A
-#define SERVO_FLAG		OCF1A
-#define SERVO_FORCE		FOC1A
-#define SERVO_COM0		COM1A0
-#define SERVO_COM1		COM1A1
-
-#define ADC_CHANNEL_VSUPPLY     3
-
-#endif /* CONFIG_V1_2_1_H */
diff --git a/conf/autopilot/obsolete/v1_2_1.makefile b/conf/autopilot/obsolete/v1_2_1.makefile
deleted file mode 100644
index d6a97a3661..0000000000
--- a/conf/autopilot/obsolete/v1_2_1.makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-include $(PAPARAZZI_SRC)/conf/autopilot/twin_avr.makefile
-
-LOCAL_CFLAGS += -DCTL_BRD_V1_2_1 -DBOARD_CONFIG=\"v1_2_1.h\"
diff --git a/conf/autopilot/obsolete/vane_sensor.makefile b/conf/autopilot/obsolete/vane_sensor.makefile
deleted file mode 100644
index b5f4baa631..0000000000
--- a/conf/autopilot/obsolete/vane_sensor.makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-# Vane sensors using generic PWM input
-ap.CFLAGS += -DUSE_VANE_SENSOR
-ap.srcs += $(SRC_CSC)/csc_vane.c
-
diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile
index c8e4b37502..00123dcd40 100644
--- a/conf/autopilot/rotorcraft.makefile
+++ b/conf/autopilot/rotorcraft.makefile
@@ -84,8 +84,10 @@ $(TARGET).CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./$(PERIODIC_FREQU
 ap.CFLAGS += -DUSE_SYS_TIME
 ap.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
 ifeq ($(ARCH), stm32)
+ifneq ($(SYS_TIME_LED),none)
 ap.CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED)
 endif
+endif
 
 #
 # Telemetry/Datalink
@@ -140,15 +142,16 @@ ap.srcs += $(SRC_FIRMWARE)/commands.c
 #
 ap.srcs += $(SRC_BOARD)/baro_board.c
 ifeq ($(BOARD), booz)
-ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED)
 else ifeq ($(BOARD), lisa_l)
 ap.CFLAGS += -DUSE_I2C2
 else ifeq ($(BOARD), navgo)
-ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED)
 ap.CFLAGS += -DUSE_I2C1
 ap.CFLAGS += -DADS1114_I2C_DEVICE=i2c1
 ap.srcs += peripherals/ads1114.c
 endif
+ifneq ($(BARO_LED),none)
+ap.CFLAGS += -DROTORCRAFT_BARO_LED=$(BARO_LED)
+endif
 
 #
 # Analog Backend
diff --git a/conf/autopilot/setup.makefile b/conf/autopilot/setup.makefile
index f4bea44b93..ad4bda5bc2 100644
--- a/conf/autopilot/setup.makefile
+++ b/conf/autopilot/setup.makefile
@@ -9,15 +9,31 @@ CFG_SHARED=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/shared
 
 SRC_ARCH=arch/$(ARCH)
 SRC_FIRMWARE=firmwares/setup
+SRC_LISA=lisa
 
 SETUP_INC = -I$(SRC_FIRMWARE)
 
 $(TARGET).CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
 
 # a test program to tunnel between both uart
+ifeq ($(ARCH), lpc21)
 tunnel.CFLAGS += -DUSE_LED
 tunnel.srcs += $(SRC_ARCH)/uart_tunnel.c
 tunnel.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
+else ifeq ($(ARCH), stm32)
+tunnel.ARCHDIR = $(ARCH)
+tunnel.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT
+tunnel.srcs   += mcu.c \
+                 $(SRC_ARCH)/mcu_arch.c \
+                 $(SRC_LISA)/tunnel_hw.c          \
+                 $(SRC_ARCH)/stm32_exceptions.c   \
+                 $(SRC_ARCH)/stm32_vector_table.c
+tunnel.CFLAGS += -DUSE_LED
+tunnel.srcs   += $(SRC_ARCH)/led_hw.c
+tunnel.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=$(SYS_TIME_LED)
+tunnel.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
+tunnel.srcs   += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+endif
 
 
 # for the usb_tunnel we need to set PCLK higher with the flag USE_USB_HIGH_PCLK
@@ -40,7 +56,11 @@ usb_tunnel_1.srcs += $(SRC_ARCH)/lpcusb/usbcontrol.c $(SRC_ARCH)/lpcusb/usbstdre
 usb_tunnel_1.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 usb_tunnel_1.srcs += mcu.c $(SRC_ARCH)/mcu_arch.c
 else
-$(error usb_tunnel currently only implemented for the lpc21)
+ifeq ($(TARGET),usb_tunnel_0)
+$(error usb_tunnel_0 currently only implemented for the lpc21)
+else ifeq ($(TARGET),usb_tunnel_1)
+$(error usb_tunnel_1 currently only implemented for the lpc21)
+endif
 endif
 
 
@@ -73,9 +93,13 @@ endif
 
 
 # a test program to setup actuators
+ifeq ($(ARCH), lpc21)
 setup_actuators.CFLAGS += -DFBW -DUSE_LED -DTIME_LED=1
 setup_actuators.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600 -DDOWNLINK_DEVICE=Uart1 -DPPRZ_UART=Uart1
 setup_actuators.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ
 setup_actuators.CFLAGS += -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1
 setup_actuators.CFLAGS += $(SETUP_INC) -Ifirmwares/fixedwing
-setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c pprz_transport.c downlink.c $(SRC_FIRMWARE)/setup_actuators.c mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c firmwares/fixedwing/main.c mcu.c $(SRC_ARCH)/mcu_arch.c
+setup_actuators.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c subsystems/datalink/pprz_transport.c subsystems/datalink/downlink.c $(SRC_FIRMWARE)/setup_actuators.c mcu_periph/uart.c $(SRC_ARCH)/mcu_periph/uart_arch.c firmwares/fixedwing/main.c mcu.c $(SRC_ARCH)/mcu_arch.c
+else ifeq ($(TARGET),setup_actuators)
+$(error setup_actuators currently only implemented for the lpc21)
+endif
diff --git a/conf/autopilot/subsystems/csc_gps.makefile b/conf/autopilot/subsystems/csc_gps.makefile
deleted file mode 100644
index 9f748587d5..0000000000
--- a/conf/autopilot/subsystems/csc_gps.makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-
-ap.srcs += $(SRC_BOOZ)/booz2_gps.c math/pprz_geodetic_int.c math/pprz_geodetic_float.c
-ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DUART0_VIC_SLOT=5
-ap.CFLAGS += -DUSE_GPS -DGPS_LINK=Uart0 -DGPS_LED=3
diff --git a/conf/autopilot/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/autopilot/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
new file mode 100644
index 0000000000..2783f15fc1
--- /dev/null
+++ b/conf/autopilot/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
@@ -0,0 +1,58 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# AHRS_PROPAGATE_FREQUENCY
+# AHRS_H_X
+# AHRS_H_Y
+# AHRS_H_Z
+#
+
+AHRS_CFLAGS  = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
+
+ifneq ($(AHRS_ALIGNER_LED),none)
+  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\"
+AHRS_SRCS   += subsystems/ahrs.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl_rmat.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
+
+ap.CFLAGS += $(AHRS_CFLAGS)
+ap.srcs += $(AHRS_SRCS)
+
+
+
+# Extra stuff for fixedwings
+
+ifdef CPU_LED
+  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
+endif
+
+ifdef AHRS_PROPAGATE_FREQUENCY
+else
+  AHRS_PROPAGATE_FREQUENCY = 60
+endif
+
+ifdef AHRS_CORRECT_FREQUENCY
+else
+  AHRS_CORRECT_FREQUENCY = 60
+endif
+
+ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
+
+
+#
+# Simple simulation of the AHRS result
+#
+ahrssim_CFLAGS  = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
+ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+
+ahrssim_srcs    = $(SRC_SUBSYSTEMS)/ahrs.c
+ahrssim_srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
+
+sim.CFLAGS += $(ahrssim_CFLAGS)
+sim.srcs += $(ahrssim_srcs)
+
+jsbsim.CFLAGS += $(ahrssim_CFLAGS)
+jsbsim.srcs += $(ahrssim_srcs)
diff --git a/conf/autopilot/subsystems/fixedwing/ahrs_float_dcm.makefile b/conf/autopilot/subsystems/fixedwing/ahrs_float_dcm.makefile
index fbed2f257e..0a7e44b0f5 100644
--- a/conf/autopilot/subsystems/fixedwing/ahrs_float_dcm.makefile
+++ b/conf/autopilot/subsystems/fixedwing/ahrs_float_dcm.makefile
@@ -13,7 +13,8 @@ ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
 ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
 
-ifdef AHRS_ALIGNER_LED
+
+ifneq ($(AHRS_ALIGNER_LED),none)
   ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 
diff --git a/conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile b/conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile
index 3393992def..456d93eddc 100644
--- a/conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile
+++ b/conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile
@@ -6,8 +6,8 @@
 AHRS_CFLAGS  = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR -DUSE_AHRS_CMPL
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
 
-ifdef AHRS_ALIGNER_LED
-AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+ifneq ($(AHRS_ALIGNER_LED),none)
+  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\"
 AHRS_SRCS   += subsystems/ahrs.c
diff --git a/conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile b/conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
index 8a89a97d51..0c44fdaab0 100644
--- a/conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
+++ b/conf/autopilot/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
@@ -7,10 +7,10 @@
 #
 
 AHRS_CFLAGS  = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
-AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
+AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
 
-ifdef AHRS_ALIGNER_LED
-AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+ifneq ($(AHRS_ALIGNER_LED),none)
+  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
 AHRS_SRCS   += subsystems/ahrs.c
diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
index 25687b7151..04af065940 100644
--- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile
+++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile
@@ -119,12 +119,11 @@ ns_srcs	   	+= $(SRC_FIRMWARE)/main.c
 # LEDs
 #
 ns_CFLAGS 		+= -DUSE_LED
-ifeq ($(ARCH), stm32)
-  ns_CFLAGS 	+= -DSYS_TIME_LED=1
-else
-  ns_CFLAGS 	+= -DTIME_LED=1
+ifneq ($(SYS_TIME_LED),none)
+  ns_CFLAGS 	+= -DSYS_TIME_LED=$(SYS_TIME_LED)
 endif
 
+
 #
 # Sys-time
 #
@@ -160,6 +159,7 @@ fbw_CFLAGS		+= -DFBW
 fbw_srcs 		+= $(SRC_FIRMWARE)/main_fbw.c
 fbw_srcs 		+= subsystems/electrical.c
 fbw_srcs 		+= $(SRC_FIXEDWING)/commands.c
+fbw_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
 
 ######################################################################
 ##
@@ -169,6 +169,7 @@ fbw_srcs 		+= $(SRC_FIXEDWING)/commands.c
 ap_CFLAGS 		+= -DAP
 ap_srcs 		+= $(SRC_FIRMWARE)/main_ap.c
 ap_srcs 		+= $(SRC_FIXEDWING)/estimator.c
+ap_srcs			+= $(SRC_FIRMWARE)/ap_downlink.c
 
 
 ######################################################################
@@ -178,7 +179,7 @@ ap_srcs 		+= $(SRC_FIXEDWING)/estimator.c
 
 UNAME = $(shell uname -s)
 ifeq ("$(UNAME)","Darwin")
-  sim.CFLAGS += -I/opt/local/include/
+  sim.CFLAGS += -I/opt/paparazzi/include/ -I/opt/local/include/
 endif
 
 sim.CFLAGS              += $(CPPFLAGS)
@@ -189,7 +190,7 @@ sim.CFLAGS 		+= -DSITL
 sim.srcs 		+= $(SRC_ARCH)/sim_ap.c
 
 sim.CFLAGS 		+= -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
-sim.srcs 		+= downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c
+sim.srcs 		+= subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c
 
 sim.srcs 		+= subsystems/settings.c
 sim.srcs 		+= $(SRC_ARCH)/subsystems/settings_arch.c
@@ -217,7 +218,7 @@ jsbsim.CFLAGS 		+= -I$(SIMDIR) -I/usr/include -I$(JSBSIM_INC) -I$(OCAMLLIBDIR) `
 jsbsim.LDFLAGS		+= `pkg-config glib-2.0 --libs` -lglibivy -lm -L/usr/lib -lJSBSim
 
 jsbsim.CFLAGS 		+= -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
-jsbsim.srcs 		+= downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c
+jsbsim.srcs 		+= subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c
 
 jsbsim.srcs 		+= subsystems/settings.c
 jsbsim.srcs 		+= $(SRC_ARCH)/subsystems/settings_arch.c
diff --git a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile
index 1bec3d08d3..31bfa9e121 100644
--- a/conf/autopilot/subsystems/fixedwing/imu_analog.makefile
+++ b/conf/autopilot/subsystems/fixedwing/imu_analog.makefile
@@ -86,6 +86,10 @@ endif
 imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
 imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
 
+else ifeq ($(ARCH), stm32)
+
+$(error Not implemented for the stm32 yet... should be trivial, just do it...)
+
 endif
 
 # Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
diff --git a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
index 933150953b..c9d42eda7b 100644
--- a/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
+++ b/conf/autopilot/subsystems/fixedwing/navigation_extra.makefile
@@ -17,4 +17,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/OSAMNav.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/snav.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/spiral.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/gls.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/border_line.c
 
diff --git a/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile b/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
index 6202acf1b8..bb342a5c3e 100644
--- a/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
+++ b/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
@@ -11,6 +11,9 @@ endif
 
 
 ifeq ($(NORADIO), False)
+ifdef (RADIO_CONTROL_DATALINK_LED)
+  ap.CFLAGS += -D(RADIO_CONTROL_DATALINK_LED=$((RADIO_CONTROL_DATALINK_LED)
+endif
   $(TARGET).CFLAGS += -DRADIO_CONTROL
   $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"radio_control/rc_datalink.h\"
   $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_DATALINK
diff --git a/conf/autopilot/subsystems/fixedwing/telemetry_transparent.makefile b/conf/autopilot/subsystems/fixedwing/telemetry_transparent.makefile
index 72ce361ba5..7f8f2a8a1c 100644
--- a/conf/autopilot/subsystems/fixedwing/telemetry_transparent.makefile
+++ b/conf/autopilot/subsystems/fixedwing/telemetry_transparent.makefile
@@ -6,5 +6,5 @@ ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=$(MODEM_PORT) -DDOWNLINK_AP_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT)
 ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ
-ap.srcs += downlink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 ap.srcs += $(SRC_FIRMWARE)/datalink.c
diff --git a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile
index c9aa7a200b..4baf02135f 100644
--- a/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile
+++ b/conf/autopilot/subsystems/fixedwing/telemetry_transparent_usb.makefile
@@ -6,7 +6,7 @@
 ifeq ($(ARCH), lpc21)
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=UsbS -DDOWNLINK_AP_DEVICE=UsbS -DPPRZ_UART=UsbS
 ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL -DUSE_USB_HIGH_PCLK
-ap.srcs += downlink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 ap.srcs += $(SRC_FIRMWARE)/datalink.c
 ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
diff --git a/conf/autopilot/subsystems/fixedwing/telemetry_xbee_api.makefile b/conf/autopilot/subsystems/fixedwing/telemetry_xbee_api.makefile
index 1332413a75..fb36bddc50 100644
--- a/conf/autopilot/subsystems/fixedwing/telemetry_xbee_api.makefile
+++ b/conf/autopilot/subsystems/fixedwing/telemetry_xbee_api.makefile
@@ -7,5 +7,5 @@ ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=$(MODEM_PORT) -DDOWNLINK_AP_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT)
 ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE
-ap.srcs += downlink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c
 ap.srcs += $(SRC_FIRMWARE)/datalink.c
diff --git a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
index 423f48203e..c1120f52db 100644
--- a/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
+++ b/conf/autopilot/subsystems/lisa_passthrough/ahrs_cmpl.makefile
@@ -2,7 +2,10 @@
 # Complementary filter for attitude estimation
 #
 
-ap.CFLAGS += -DUSE_AHRS_CMPL -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+stm_passthrough.CFLAGS += -DUSE_AHRS_CMPL
+ifneq ($(AHRS_ALIGNER_LED),none)
+  stm_passthrough.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
 stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
 stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
 stm_passthrough.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_int_cmpl_euler.c
diff --git a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
index 5922e9c2ce..ed4da826d6 100644
--- a/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/actuators_mkk.makefile
@@ -6,6 +6,8 @@
 #   ...
 #   <subsystem name="actuators"     type="mkk">
 #     <configure name="MKK_I2C_SCL_TIME" value="50"/> <!-- this is optional, 150 is default, use 50 for 8 motors-->
+#   </subsystem>
+#   <define name="I2C_TRANSACTION_QUEUE_LEN" value="10"/> <!-- default is 8, increase to 10 or more for 8 motors-->
 # </firmware>
 #
 #
diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile
new file mode 100644
index 0000000000..521ae93194
--- /dev/null
+++ b/conf/autopilot/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile
@@ -0,0 +1,23 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# AHRS_PROPAGATE_FREQUENCY
+# AHRS_H_X
+# AHRS_H_Y
+# AHRS_H_Z
+#
+
+AHRS_CFLAGS  = -DUSE_AHRS
+AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
+ifneq ($(AHRS_ALIGNER_LED),none)
+  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
+AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\"
+AHRS_SRCS   += subsystems/ahrs.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_float_cmpl_rmat.c
+AHRS_SRCS   += subsystems/ahrs/ahrs_aligner.c
+
+ap.CFLAGS += $(AHRS_CFLAGS)
+ap.srcs += $(AHRS_SRCS)
+
+sim.CFLAGS += $(AHRS_CFLAGS)
+sim.srcs += $(AHRS_SRCS)
diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile
index a2d817dc86..40ae37da45 100644
--- a/conf/autopilot/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile
@@ -5,8 +5,8 @@
 
 AHRS_CFLAGS  = -DUSE_AHRS -DUSE_AHRS_CMPL
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
-ifdef AHRS_ALIGNER_LED
-AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+ifneq ($(AHRS_ALIGNER_LED),none)
+  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\"
 AHRS_SRCS   += subsystems/ahrs.c
diff --git a/conf/autopilot/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile b/conf/autopilot/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile
index 1eb08fc6ac..2f316c9ed0 100644
--- a/conf/autopilot/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile
@@ -8,8 +8,8 @@
 
 AHRS_CFLAGS  = -DUSE_AHRS
 AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
-ifdef AHRS_ALIGNER_LED
-AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+ifneq ($(AHRS_ALIGNER_LED),none)
+  AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 endif
 AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
 AHRS_SRCS   += subsystems/ahrs.c
diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
index 00b92d675e..58feb0cf6a 100644
--- a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
@@ -78,7 +78,7 @@ sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c
 
 sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
 sim.srcs += $(SRC_FIRMWARE)/telemetry.c \
-            downlink.c \
+            subsystems/datalink/downlink.c \
             $(SRC_ARCH)/ivy_transport.c
 
 sim.srcs   += $(SRC_FIRMWARE)/commands.c
diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile
index 920b031d66..9f49d234d1 100644
--- a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent.makefile
@@ -10,5 +10,5 @@ ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DPPRZ_UART=$(MODEM_PORT)
 ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ
-ap.srcs += downlink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile
index a15920babb..46da240113 100644
--- a/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/telemetry_transparent_usb.makefile
@@ -4,7 +4,7 @@
 ifeq ($(ARCH), lpc21)
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=UsbS -DPPRZ_UART=UsbS
 ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=PPRZ -DUSE_USB_SERIAL
-ap.srcs += downlink.c pprz_transport.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprz_transport.c
 ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
 ap.srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
 ap.srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
diff --git a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile
index d083461928..0f4ff308f8 100644
--- a/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile
+++ b/conf/autopilot/subsystems/rotorcraft/telemetry_xbee_api.makefile
@@ -11,5 +11,5 @@ ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD)
 
 ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=$(MODEM_PORT) -DXBEE_UART=$(MODEM_PORT)
 ap.CFLAGS += -DDOWNLINK_TRANSPORT=XBeeTransport -DDATALINK=XBEE
-ap.srcs += downlink.c xbee.c
+ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/xbee.c
 ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.c
diff --git a/conf/autopilot/subsystems/shared/imu_aspirin.makefile b/conf/autopilot/subsystems/shared/imu_aspirin.makefile
index 51bbaa33e3..1c08dca87b 100644
--- a/conf/autopilot/subsystems/shared/imu_aspirin.makefile
+++ b/conf/autopilot/subsystems/shared/imu_aspirin.makefile
@@ -1,66 +1,3 @@
-#
-# Aspirin IMU
-#
-#
-# required xml:
-#  <section name="IMU" prefix="IMU_">
-#
-#    <define name="GYRO_X_NEUTRAL" value="33924"/>
-#    <define name="GYRO_Y_NEUTRAL" value="33417"/>
-#    <define name="GYRO_Z_NEUTRAL" value="32809"/>
-#
-#    <define name="GYRO_X_SENS" value="1.01" integer="16"/>
-#    <define name="GYRO_Y_SENS" value="1.01" integer="16"/>
-#    <define name="GYRO_Z_SENS" value="1.01" integer="16"/>
-#
-#    <define name="ACCEL_X_NEUTRAL" value="32081"/>
-#    <define name="ACCEL_Y_NEUTRAL" value="33738"/>
-#    <define name="ACCEL_Z_NEUTRAL" value="32441"/>
-#
-#    <define name="ACCEL_X_SENS" value="2.50411474" integer="16"/>
-#    <define name="ACCEL_Y_SENS" value="2.48126183" integer="16"/>
-#    <define name="ACCEL_Z_SENS" value="2.51396167" integer="16"/>
-#
-#    <define name="MAG_X_NEUTRAL" value="2358"/>
-#    <define name="MAG_Y_NEUTRAL" value="2362"/>
-#    <define name="MAG_Z_NEUTRAL" value="2119"/>
-#
-#    <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
-#    <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
-#    <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
-#
-#  </section>
-#
-#
+include $(CFG_SHARED)/imu_aspirin_v1.0.makefile
 
-# imu aspirin
-
-IMU_ASPIRIN_CFLAGS  = -DUSE_IMU
-IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\" -DIMU_OVERRIDE_CHANNELS
-IMU_ASPIRIN_SRCS    = $(SRC_SUBSYSTEMS)/imu.c             \
-                      $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
-                      $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c
-
-# Magnetometer
-IMU_ASPIRIN_SRCS   += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
-
-IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
-
-ifeq ($(ARCH), lpc21)
-#TODO
-else ifeq ($(ARCH), stm32)
-IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ  # Gyro Int on PC14
-IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ    # Mag Int on PB5
-IMU_ASPIRIN_CFLAGS += -DUSE_EXTI2_IRQ      # Accel Int on PD2
-IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
-endif
-
-
-# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
-# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
-ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
-ap.srcs   += $(IMU_ASPIRIN_SRCS)
-
-# sim not done yet
-#sim.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
-#sim.srcs   += $(IMU_ASPIRIN_SRCS)
+$(warning The imu_aspirin subsystem has been split up into different versions, please replace <subsystem name="imu" type="aspirin"/> with <subsystem name="imu" type="aspirin_v1.0"/> (or aspirin_v1.5) in your airframe file.)
diff --git a/conf/autopilot/subsystems/shared/imu_aspirin_common.makefile b/conf/autopilot/subsystems/shared/imu_aspirin_common.makefile
new file mode 100644
index 0000000000..f69e2adb6b
--- /dev/null
+++ b/conf/autopilot/subsystems/shared/imu_aspirin_common.makefile
@@ -0,0 +1,58 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Common part for all Aspirin IMUs
+#
+#
+# required xml:
+#  <section name="IMU" prefix="IMU_">
+#
+#    <define name="GYRO_X_NEUTRAL" value="33924"/>
+#    <define name="GYRO_Y_NEUTRAL" value="33417"/>
+#    <define name="GYRO_Z_NEUTRAL" value="32809"/>
+#
+#    <define name="GYRO_X_SENS" value="1.01" integer="16"/>
+#    <define name="GYRO_Y_SENS" value="1.01" integer="16"/>
+#    <define name="GYRO_Z_SENS" value="1.01" integer="16"/>
+#
+#    <define name="ACCEL_X_NEUTRAL" value="32081"/>
+#    <define name="ACCEL_Y_NEUTRAL" value="33738"/>
+#    <define name="ACCEL_Z_NEUTRAL" value="32441"/>
+#
+#    <define name="ACCEL_X_SENS" value="2.50411474" integer="16"/>
+#    <define name="ACCEL_Y_SENS" value="2.48126183" integer="16"/>
+#    <define name="ACCEL_Z_SENS" value="2.51396167" integer="16"/>
+#
+#    <define name="MAG_X_NEUTRAL" value="2358"/>
+#    <define name="MAG_Y_NEUTRAL" value="2362"/>
+#    <define name="MAG_Z_NEUTRAL" value="2119"/>
+#
+#    <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
+#    <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
+#    <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
+#
+#  </section>
+#
+#
+
+# imu aspirin
+
+IMU_ASPIRIN_CFLAGS  = -DUSE_IMU
+IMU_ASPIRIN_CFLAGS += -DIMU_TYPE_H=\"imu/imu_aspirin.h\"
+IMU_ASPIRIN_SRCS    = $(SRC_SUBSYSTEMS)/imu.c             \
+                      $(SRC_SUBSYSTEMS)/imu/imu_aspirin.c \
+                      $(SRC_ARCH)/subsystems/imu/imu_aspirin_arch.c
+
+# Magnetometer
+IMU_ASPIRIN_SRCS   += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch.c
+
+IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
+
+ifeq ($(ARCH), lpc21)
+#TODO
+else ifeq ($(ARCH), stm32)
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ  # Gyro Int on PC14
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ    # Mag Int on PB5
+IMU_ASPIRIN_CFLAGS += -DUSE_EXTI2_IRQ      # Accel Int on PD2
+IMU_ASPIRIN_CFLAGS += -DUSE_DMA1_C4_IRQ    # SPI2 Rx DMA
+endif
+
diff --git a/conf/autopilot/subsystems/shared/imu_aspirin_v1.0.makefile b/conf/autopilot/subsystems/shared/imu_aspirin_v1.0.makefile
new file mode 100644
index 0000000000..3e2e8a7c26
--- /dev/null
+++ b/conf/autopilot/subsystems/shared/imu_aspirin_v1.0.makefile
@@ -0,0 +1,46 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Aspirin IMU v1.0
+#
+#
+# required xml:
+#  <section name="IMU" prefix="IMU_">
+#
+#    <define name="GYRO_X_NEUTRAL" value="33924"/>
+#    <define name="GYRO_Y_NEUTRAL" value="33417"/>
+#    <define name="GYRO_Z_NEUTRAL" value="32809"/>
+#
+#    <define name="GYRO_X_SENS" value="1.01" integer="16"/>
+#    <define name="GYRO_Y_SENS" value="1.01" integer="16"/>
+#    <define name="GYRO_Z_SENS" value="1.01" integer="16"/>
+#
+#    <define name="ACCEL_X_NEUTRAL" value="32081"/>
+#    <define name="ACCEL_Y_NEUTRAL" value="33738"/>
+#    <define name="ACCEL_Z_NEUTRAL" value="32441"/>
+#
+#    <define name="ACCEL_X_SENS" value="2.50411474" integer="16"/>
+#    <define name="ACCEL_Y_SENS" value="2.48126183" integer="16"/>
+#    <define name="ACCEL_Z_SENS" value="2.51396167" integer="16"/>
+#
+#    <define name="MAG_X_NEUTRAL" value="2358"/>
+#    <define name="MAG_Y_NEUTRAL" value="2362"/>
+#    <define name="MAG_Z_NEUTRAL" value="2119"/>
+#
+#    <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
+#    <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
+#    <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
+#
+#  </section>
+#
+#
+
+# imu aspirin
+
+include $(CFG_SHARED)/imu_aspirin_common.makefile
+
+IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_1_0
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
+ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
+ap.srcs   += $(IMU_ASPIRIN_SRCS)
diff --git a/conf/autopilot/subsystems/shared/imu_aspirin_v1.5.makefile b/conf/autopilot/subsystems/shared/imu_aspirin_v1.5.makefile
new file mode 100644
index 0000000000..2686e94905
--- /dev/null
+++ b/conf/autopilot/subsystems/shared/imu_aspirin_v1.5.makefile
@@ -0,0 +1,46 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Aspirin IMU v1.5
+#
+#
+# required xml:
+#  <section name="IMU" prefix="IMU_">
+#
+#    <define name="GYRO_X_NEUTRAL" value="33924"/>
+#    <define name="GYRO_Y_NEUTRAL" value="33417"/>
+#    <define name="GYRO_Z_NEUTRAL" value="32809"/>
+#
+#    <define name="GYRO_X_SENS" value="1.01" integer="16"/>
+#    <define name="GYRO_Y_SENS" value="1.01" integer="16"/>
+#    <define name="GYRO_Z_SENS" value="1.01" integer="16"/>
+#
+#    <define name="ACCEL_X_NEUTRAL" value="32081"/>
+#    <define name="ACCEL_Y_NEUTRAL" value="33738"/>
+#    <define name="ACCEL_Z_NEUTRAL" value="32441"/>
+#
+#    <define name="ACCEL_X_SENS" value="2.50411474" integer="16"/>
+#    <define name="ACCEL_Y_SENS" value="2.48126183" integer="16"/>
+#    <define name="ACCEL_Z_SENS" value="2.51396167" integer="16"/>
+#
+#    <define name="MAG_X_NEUTRAL" value="2358"/>
+#    <define name="MAG_Y_NEUTRAL" value="2362"/>
+#    <define name="MAG_Z_NEUTRAL" value="2119"/>
+#
+#    <define name="MAG_X_SENS" value="3.4936416" integer="16"/>
+#    <define name="MAG_Y_SENS" value="3.607713" integer="16"/>
+#    <define name="MAG_Z_SENS" value="4.90788848" integer="16"/>
+#
+#  </section>
+#
+#
+
+# imu aspirin
+
+include $(CFG_SHARED)/imu_aspirin_common.makefile
+
+IMU_ASPIRIN_CFLAGS += -DIMU_ASPIRIN_VERSION_1_5
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
+ap.CFLAGS += $(IMU_ASPIRIN_CFLAGS)
+ap.srcs   += $(IMU_ASPIRIN_SRCS)
diff --git a/conf/autopilot/subsystems/shared/imu_b2_v1.0.makefile b/conf/autopilot/subsystems/shared/imu_b2_v1.0.makefile
index b3063437df..6087ccd3b1 100644
--- a/conf/autopilot/subsystems/shared/imu_b2_v1.0.makefile
+++ b/conf/autopilot/subsystems/shared/imu_b2_v1.0.makefile
@@ -39,11 +39,12 @@
 # common Booz2 IMU files
 include $(CFG_SHARED)/imu_b2_common.makefile
 
-# imu Booz2 v1.1
-imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
+# imu Booz2 v1.0
 imu_CFLAGS += -DIMU_B2_VERSION_1_0
 
 # Magnetometer
+ifndef NO_MAG
+imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
 imu_CFLAGS += -DUSE_AMI601
 imu_srcs += peripherals/ami601.c
 
@@ -54,6 +55,8 @@ else ifeq ($(ARCH), stm32)
 imu_CFLAGS += -DUSE_I2C2 -DUSE_EXTI9_5_IRQ
 endif
 
+endif #NO_MAG
+
 # Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
 # see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
 ap.CFLAGS += $(imu_CFLAGS)
diff --git a/conf/autopilot/subsystems/shared/imu_b2_v1.2.makefile b/conf/autopilot/subsystems/shared/imu_b2_v1.2.makefile
index e4775322c3..78d426536f 100644
--- a/conf/autopilot/subsystems/shared/imu_b2_v1.2.makefile
+++ b/conf/autopilot/subsystems/shared/imu_b2_v1.2.makefile
@@ -43,17 +43,17 @@
 include $(CFG_SHARED)/imu_b2_common.makefile
 
 # imu Booz2 v1.2
-imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843
 imu_CFLAGS += -DIMU_B2_VERSION_1_2
 
-# Magnetometer
-imu_srcs += peripherals/hmc5843.c
-imu_srcs += $(SRC_ARCH)/peripherals/hmc5843_arch.c
 
 ifeq ($(ARCH), lpc21)
-#FIXME ms2100 not used on this imu
-#imu_CFLAGS += -DMS2100_DRDY_VIC_SLOT=12
+imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC58XX
+imu_CFLAGS += -DHMC58XX_I2C_DEVICE=i2c1 -DUSE_I2C1
+imu_srcs += peripherals/hmc58xx.c
 else ifeq ($(ARCH), stm32)
+imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843
+imu_srcs += peripherals/hmc5843.c
+imu_srcs += $(SRC_ARCH)/peripherals/hmc5843_arch.c
 imu_CFLAGS += -DUSE_I2C2 -DUSE_EXTI9_5_IRQ
 endif
 
diff --git a/conf/autopilot/subsystems/shared/radio_control_ppm.makefile b/conf/autopilot/subsystems/shared/radio_control_ppm.makefile
index 6b2fdeab88..41b7f90729 100644
--- a/conf/autopilot/subsystems/shared/radio_control_ppm.makefile
+++ b/conf/autopilot/subsystems/shared/radio_control_ppm.makefile
@@ -12,7 +12,7 @@ endif
 
 ifeq ($(NORADIO), False)
   $(TARGET).CFLAGS	+= -DRADIO_CONTROL
-  ifdef RADIO_CONTROL_LED
+  ifneq ($(RADIO_CONTROL_LED),none)
     ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
   endif
   $(TARGET).CFLAGS 	+= -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/ppm.h\"
diff --git a/conf/autopilot/subsystems/shared/radio_control_spektrum.makefile b/conf/autopilot/subsystems/shared/radio_control_spektrum.makefile
index ae53bd7a26..192a21c376 100644
--- a/conf/autopilot/subsystems/shared/radio_control_spektrum.makefile
+++ b/conf/autopilot/subsystems/shared/radio_control_spektrum.makefile
@@ -10,7 +10,7 @@ ap.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/spektrum.h\"
 ifeq ($(ARCH), lpc21)
 ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=$(RADIO_CONTROL_SPEKTRUM_MODEL)
 endif
-ifdef RADIO_CONTROL_LED
+ifneq ($(RADIO_CONTROL_LED),none)
 ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
 endif
 ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_PRIMARY_PORT=$(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT)
diff --git a/conf/boards/booz_1.0.makefile b/conf/boards/booz_1.0.makefile
index 1f56efef5b..900f2f6079 100644
--- a/conf/boards/booz_1.0.makefile
+++ b/conf/boards/booz_1.0.makefile
@@ -18,12 +18,31 @@ endif
 #
 #
 
-SYS_TIME_LED = 1 # not used on rotorcraft, only for tests
-
+#
+# default LED configuration
+#
+ifndef RADIO_CONTROL_LED
 RADIO_CONTROL_LED  = 1
+endif
+
+ifndef BARO_LED
+BARO_LED = 2
+endif
+
+ifndef AHRS_ALIGNER_LED
+AHRS_ALIGNER_LED = 3
+endif
+
+ifndef GPS_LED
+GPS_LED = 4
+endif
+
+ifndef SYS_TIME_LED
+SYS_TIME_LED = 1
+endif
+
 RADIO_CONTROL_LINK = UART0
 
-BARO_LED = 2
 
 ifndef MODEM_PORT
 MODEM_PORT = UART1
@@ -32,12 +51,9 @@ ifndef MODEM_BAUD
 MODEM_BAUD = B57600
 endif
 
-AHRS_ALIGNER_LED = 3
-
 ifndef GPS_PORT
 GPS_PORT=UART0
 endif
 ifndef GPS_BAUD
 GPS_BAUD=B38400
 endif
-GPS_LED=4
diff --git a/conf/boards/classix.makefile b/conf/boards/classix.makefile
index 79002ad107..055ec54e92 100644
--- a/conf/boards/classix.makefile
+++ b/conf/boards/classix.makefile
@@ -24,6 +24,7 @@ LPC21ISP_XTAL = 12000
 ### default settings for classix
 GPS_BAUD = B38400
 GPS_LED = none
+RADIO_CONTROL_LED = none
 
 # All targets on the TINY board run on the same processor achitecture
 $(TARGET).ARCHDIR = $(ARCH)
diff --git a/conf/boards/hb_1.1.makefile b/conf/boards/hb_1.1.makefile
index b8d0c3ae0a..1b60e422ba 100644
--- a/conf/boards/hb_1.1.makefile
+++ b/conf/boards/hb_1.1.makefile
@@ -18,7 +18,28 @@ endif
 #
 #
 
-SYS_TIME_LED = 1 # not used on rotorcraft, only for tests
+#
+# default LED configuration
+#
+ifndef RADIO_CONTROL_LED
+RADIO_CONTROL_LED  = none
+endif
+
+ifndef BARO_LED
+BARO_LED = none
+endif
+
+ifndef AHRS_ALIGNER_LED
+AHRS_ALIGNER_LED = none
+endif
+
+ifndef GPS_LED
+GPS_LED = 2
+endif
+
+ifndef SYS_TIME_LED
+SYS_TIME_LED = 1
+endif
 
 RADIO_CONTROL_LINK = UART0
 
@@ -36,8 +57,6 @@ ifndef GPS_BAUD
 GPS_BAUD = B38400
 endif
 
-GPS_LED     = 2
-
 ifndef ADC_IR1
 ADC_IR1      = 1
 ADC_IR1_CHAN = 0
diff --git a/conf/boards/lisa_l_1.0.makefile b/conf/boards/lisa_l_1.0.makefile
index 88a04653be..d5ebd0ba1f 100644
--- a/conf/boards/lisa_l_1.0.makefile
+++ b/conf/boards/lisa_l_1.0.makefile
@@ -36,11 +36,31 @@ endif
 #
 #
 
-SYS_TIME_LED       = 1
+#
+# default LED configuration
+#
+ifndef RADIO_CONTROL_LED
+RADIO_CONTROL_LED  = 5
+endif
+
+ifndef BARO_LED
+BARO_LED = none
+endif
+
+ifndef AHRS_ALIGNER_LED
+AHRS_ALIGNER_LED = 7
+endif
+
+ifndef GPS_LED
+GPS_LED = 3
+endif
+
+ifndef SYS_TIME_LED
+SYS_TIME_LED = 1
+endif
 
 RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT   = UART3
 RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT = UART5
-RADIO_CONTROL_LED  = 5
 
 ifndef MODEM_PORT
 MODEM_PORT=UART2
@@ -49,15 +69,12 @@ ifndef MODEM_BAUD
 MODEM_BAUD=B57600
 endif
 
-AHRS_ALIGNER_LED = 7
-
 ifndef GPS_PORT
 GPS_PORT=UART1
 endif
 ifndef GPS_BAUD
 GPS_BAUD=B38400
 endif
-GPS_LED = 3
 
 #
 # this is the DRDY pin of a max1168 on a booz IMU
diff --git a/conf/boards/lisa_m_1.0.makefile b/conf/boards/lisa_m_1.0.makefile
index 0a05903f17..c10f49f2cd 100644
--- a/conf/boards/lisa_m_1.0.makefile
+++ b/conf/boards/lisa_m_1.0.makefile
@@ -27,11 +27,33 @@ endif
 #
 #
 
-SYS_TIME_LED       = 1
+
+#
+# default LED configuration
+#
+ifndef RADIO_CONTROL_LED
+RADIO_CONTROL_LED  = none
+endif
+
+ifndef BARO_LED
+BARO_LED = none
+endif
+
+ifndef AHRS_ALIGNER_LED
+AHRS_ALIGNER_LED = none
+endif
+
+ifndef GPS_LED
+GPS_LED = none
+endif
+
+ifndef SYS_TIME_LED
+SYS_TIME_LED = 1
+endif
+
 
 RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT   = UART3
 RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT = UART5
-#RADIO_CONTROL_LED  = 5
 
 ifndef MODEM_PORT
 MODEM_PORT=UART2
@@ -40,7 +62,6 @@ ifndef MODEM_BAUD
 MODEM_BAUD=B57600
 endif
 
-#AHRS_ALIGNER_LED = 7
 
 ifndef GPS_PORT
 GPS_PORT=UART1
@@ -48,7 +69,7 @@ endif
 ifndef GPS_BAUD
 GPS_BAUD=B38400
 endif
-GPS_LED = none
+
 
 #
 # this is the DRDY pin of a max1168 on a booz IMU
diff --git a/conf/boards/navgo_1.0.makefile b/conf/boards/navgo_1.0.makefile
index de88250efd..75b42e53f4 100644
--- a/conf/boards/navgo_1.0.makefile
+++ b/conf/boards/navgo_1.0.makefile
@@ -20,11 +20,29 @@ LPC21ISP_BAUD = 38400
 LPC21ISP_XTAL = 12000
 
 
+#
+# default LED configuration
+#
+ifndef RADIO_CONTROL_LED
 RADIO_CONTROL_LED  = 1
+endif
 
+ifndef BARO_LED
 BARO_LED = 2
+endif
 
+ifndef AHRS_ALIGNER_LED
 AHRS_ALIGNER_LED = 3
+endif
+
+ifndef GPS_LED
+GPS_LED = 4
+endif
+
+ifndef SYS_TIME_LED
+SYS_TIME_LED = none
+endif
+
 
 ### default settings
 ifndef GPS_PORT
@@ -33,7 +51,6 @@ endif
 ifndef GPS_BAUD
 GPS_BAUD	= B38400
 endif
-GPS_LED     = 4
 
 ifndef MODEM_PORT
 MODEM_PORT	= UART1
diff --git a/conf/boards/tiny_2.11.makefile b/conf/boards/tiny_2.11.makefile
index fd92507e9a..53a6d4f355 100644
--- a/conf/boards/tiny_2.11.makefile
+++ b/conf/boards/tiny_2.11.makefile
@@ -19,6 +19,28 @@ endif
 LPC21ISP_BAUD = 38400
 LPC21ISP_XTAL = 12000
 
+#
+# default LED configuration
+#
+ifndef RADIO_CONTROL_LED
+RADIO_CONTROL_LED  = none
+endif
+
+ifndef BARO_LED
+BARO_LED = none
+endif
+
+ifndef AHRS_ALIGNER_LED
+AHRS_ALIGNER_LED = none
+endif
+
+ifndef GPS_LED
+GPS_LED = 2
+endif
+
+ifndef SYS_TIME_LED
+SYS_TIME_LED = none
+endif
 
 ### default settings for tiny_2 and twog
 ifndef GPS_PORT
@@ -27,7 +49,6 @@ endif
 ifndef GPS_BAUD
 GPS_BAUD	= B38400
 endif
-GPS_LED     = 2
 
 ifndef MODEM_PORT
 MODEM_PORT	= UART1
@@ -36,6 +57,7 @@ ifndef MODEM_BAUD
 MODEM_BAUD 	= B57600
 endif
 
+
 ADC_IR_TOP = ADC_0
 ADC_IR1 = ADC_1
 ADC_IR2 = ADC_2
diff --git a/conf/boards/umarim_1.0.makefile b/conf/boards/umarim_1.0.makefile
index 793cc91778..c1eb9b600e 100644
--- a/conf/boards/umarim_1.0.makefile
+++ b/conf/boards/umarim_1.0.makefile
@@ -20,6 +20,30 @@ LPC21ISP_BAUD = 38400
 LPC21ISP_XTAL = 12000
 
 
+#
+# default LED configuration
+#
+ifndef RADIO_CONTROL_LED
+RADIO_CONTROL_LED  = none
+endif
+
+ifndef BARO_LED
+BARO_LED = none
+endif
+
+ifndef AHRS_ALIGNER_LED
+AHRS_ALIGNER_LED = none
+endif
+
+ifndef GPS_LED
+GPS_LED = 2
+endif
+
+ifndef SYS_TIME_LED
+SYS_TIME_LED = none
+endif
+
+
 ### default settings
 ifndef GPS_PORT
 GPS_PORT	= UART0
@@ -27,7 +51,6 @@ endif
 ifndef GPS_BAUD
 GPS_BAUD	= B38400
 endif
-GPS_LED     = 2
 
 ifndef MODEM_PORT
 MODEM_PORT	= UART1
diff --git a/conf/flight_plans/basic.xml b/conf/flight_plans/basic.xml
index f9ead2281c..151f18a087 100644
--- a/conf/flight_plans/basic.xml
+++ b/conf/flight_plans/basic.xml
@@ -3,7 +3,7 @@
 <flight_plan alt="75" ground_alt="0" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
diff --git a/conf/flight_plans/ccc_gl.xml b/conf/flight_plans/ccc_gl.xml
index 1a7656c6f6..d913ac6c92 100644
--- a/conf/flight_plans/ccc_gl.xml
+++ b/conf/flight_plans/ccc_gl.xml
@@ -2,7 +2,7 @@
 
 <flight_plan alt="150" ground_alt="68" lat0="52.26496" lon0="9.99394" max_dist_from_home="1500" name="Hildesheim" qfu="90" security_height="25">
   <header>
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
diff --git a/conf/flight_plans/creidlitz.xml b/conf/flight_plans/creidlitz.xml
index e8f52af09b..c53a001f24 100644
--- a/conf/flight_plans/creidlitz.xml
+++ b/conf/flight_plans/creidlitz.xml
@@ -3,7 +3,7 @@
 <flight_plan alt="420" ground_alt="360" lat0="50.230060" lon0="10.990449" max_dist_from_home="1500" name="Flugplatz Steinruecken" security_height="25"  QFU="210">
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
diff --git a/conf/flight_plans/fp_tp_auto.xml b/conf/flight_plans/fp_tp_auto.xml
index 3f61b05c1b..e676f007c6 100644
--- a/conf/flight_plans/fp_tp_auto.xml
+++ b/conf/flight_plans/fp_tp_auto.xml
@@ -2,7 +2,7 @@
 
 <flight_plan alt="75" ground_alt="0" lat0="43.4622" lon0="1.2729" max_dist_from_home="1500" name="Versatile" qfu="270" security_height="25">
   <header>
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
diff --git a/conf/flight_plans/grosslobke_demo.xml b/conf/flight_plans/grosslobke_demo.xml
index 92cecfe5f7..31be09e05c 100755
--- a/conf/flight_plans/grosslobke_demo.xml
+++ b/conf/flight_plans/grosslobke_demo.xml
@@ -4,7 +4,7 @@
   <header>
 #include "subsystems/navigation/bomb.h"
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="15.0" y="5.0"/>
diff --git a/conf/flight_plans/hsif.xml b/conf/flight_plans/hsif.xml
index 01f2d7cae2..26825eb6e7 100644
--- a/conf/flight_plans/hsif.xml
+++ b/conf/flight_plans/hsif.xml
@@ -3,7 +3,7 @@
 <flight_plan alt="75" ground_alt="0" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
diff --git a/conf/flight_plans/ingolfsskali.xml b/conf/flight_plans/ingolfsskali.xml
index 228ba75842..2666e2b307 100644
--- a/conf/flight_plans/ingolfsskali.xml
+++ b/conf/flight_plans/ingolfsskali.xml
@@ -3,7 +3,7 @@
 <flight_plan alt="1000" ground_alt="840" lat0="65.007419" lon0="-18.895916" max_dist_from_home="4500" name="Ingolfsskali" qfu="0" security_height="160">
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="15.0" y="5.0"/>
diff --git a/conf/flight_plans/joystick.xml b/conf/flight_plans/joystick.xml
index 585a5542e4..c1efecaf3f 100644
--- a/conf/flight_plans/joystick.xml
+++ b/conf/flight_plans/joystick.xml
@@ -3,7 +3,7 @@
 <flight_plan alt="75" ground_alt="0" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "joystick.h"
 </header>
   <waypoints>
diff --git a/conf/flight_plans/kv_svalbard.xml b/conf/flight_plans/kv_svalbard.xml
index 33bb101d5f..153104cc99 100644
--- a/conf/flight_plans/kv_svalbard.xml
+++ b/conf/flight_plans/kv_svalbard.xml
@@ -4,7 +4,7 @@
 
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
diff --git a/conf/flight_plans/nordlys.xml b/conf/flight_plans/nordlys.xml
index 9e279fe850..ae8584808c 100644
--- a/conf/flight_plans/nordlys.xml
+++ b/conf/flight_plans/nordlys.xml
@@ -3,7 +3,7 @@
 <flight_plan alt="150" ground_alt="10" lat0="78.202961" lon0="15.8310" max_dist_from_home="3000" name="Nordlysstasjonen" qfu="270" security_height="60">
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="-0.3" y="-66.4"/>
diff --git a/conf/flight_plans/poles.xml b/conf/flight_plans/poles.xml
index e1476ff1d8..539367ea89 100644
--- a/conf/flight_plans/poles.xml
+++ b/conf/flight_plans/poles.xml
@@ -2,7 +2,7 @@
 
 <flight_plan alt="75" ground_alt="0" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
   <header>
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
diff --git a/conf/flight_plans/snav.xml b/conf/flight_plans/snav.xml
index d22bd1c962..0dd2e92c73 100644
--- a/conf/flight_plans/snav.xml
+++ b/conf/flight_plans/snav.xml
@@ -3,7 +3,7 @@
 <flight_plan alt="75" ground_alt="0" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "subsystems/navigation/snav.h"
 </header>
   <waypoints>
diff --git a/conf/flight_plans/tcas.xml b/conf/flight_plans/tcas.xml
index 9a3c2ce1ba..e2b3045e20 100644
--- a/conf/flight_plans/tcas.xml
+++ b/conf/flight_plans/tcas.xml
@@ -3,7 +3,7 @@
 <flight_plan alt="75" ground_alt="0" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
   <header>
 #include "subsystems/navigation/nav_line.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "tcas.h"
 </header>
   <waypoints>
diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml
index 7544e5dda3..571ab05e04 100644
--- a/conf/flight_plans/versatile.xml
+++ b/conf/flight_plans/versatile.xml
@@ -2,7 +2,7 @@
 
 <flight_plan alt="75" ground_alt="0" lat0="43.4622" lon0="1.2729" max_dist_from_home="1500" name="Versatile" qfu="270" security_height="25">
   <header>
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
diff --git a/conf/messages.xml b/conf/messages.xml
index b83b4ac517..e529ee873a 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1048,12 +1048,7 @@
     <field name="cnt"    type="int32"/>
   </message>
 
-  <message name="FILTER_Q" id="138">
-    <field name="qi"  type="int32"/>
-    <field name="qx"  type="int32"/>
-    <field name="qy"  type="int32"/>
-    <field name="qz"  type="int32"/>
-  </message>
+  <!-- 138 is free -->
 
   <message name="FILTER_COR" id="139">
     <field name="mphi"   type="int32"/>
@@ -1276,7 +1271,7 @@
     <field name="fix"     type="uint8" values="NONE|UKN1|UKN2|3D"/>
   </message>
 
-  <message name="BOOZ2_AHRS_EULER" id="156">
+  <message name="AHRS_EULER_INT" id="156">
     <field name="imu_phi"    type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
     <field name="imu_theta"  type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
     <field name="imu_psi"    type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
@@ -1285,7 +1280,7 @@
     <field name="body_psi"   type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
   </message>
 
-  <message name="BOOZ2_AHRS_QUAT" id="157">
+  <message name="AHRS_QUAT_INT" id="157">
     <field name="imu_qi"  type="int32" alt_unit=""  alt_unit_coef="0.0000305"/>
     <field name="imu_qx"  type="int32" alt_unit=""  alt_unit_coef="0.0000305"/>
     <field name="imu_qy"  type="int32" alt_unit=""  alt_unit_coef="0.0000305"/>
@@ -1296,7 +1291,7 @@
     <field name="body_qz" type="int32" alt_unit=""  alt_unit_coef="0.0000305"/>
   </message>
 
-  <message name="BOOZ2_AHRS_RMAT" id="158">
+  <message name="AHRS_RMAT_INT" id="158">
     <field name="imu_m00"  type="int32" alt_unit=""  alt_unit_coef="0.0000610"/>
     <field name="imu_m01"  type="int32" alt_unit=""  alt_unit_coef="0.0000610"/>
     <field name="imu_m02"  type="int32" alt_unit=""  alt_unit_coef="0.0000610"/>
@@ -1325,7 +1320,7 @@
     <field name="horizontal_mode" type="uint8"/>
   </message>
 
-  <message name="BOOZ2_RADIO_CONTROL" id="160">
+  <message name="ROTORCRAFT_RADIO_CONTROL" id="160">
      <field name="roll"     type="int16" unit="pprz"/>
      <field name="pitch"    type="int16" unit="pprz"/>
      <field name="yaw"      type="int16" unit="pprz"/>
@@ -1335,11 +1330,7 @@
      <field name="status"   type="uint8" values="OK|LOST|REALLY_LOST"/>
   </message>
 
-  <message name="BOOZ_BARO_RAW" id="161">
-      <field name="offset"         type="uint16"/>
-      <field name="value"          type="uint16"/>
-      <field name="value_filtered" type="uint16"/>
-  </message>
+  <!-- 161 is free -->
 
   <message name="VFF" id="162">
       <field name="measure"        type="float"/>
@@ -1474,7 +1465,7 @@
     <field name="data_b"  type="uint32"/>
   </message>
 
-  <message name="BOOZ_AHRS_BIAS" id="178">
+  <message name="AHRS_GYRO_BIAS_INT" id="178">
     <field name="bp"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
     <field name="bq"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
     <field name="br"     type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
@@ -2403,7 +2394,7 @@
 
   <message name="TELEMETRY_STATUS" id="32">
     <field name="ac_id" type="string"/>
-    <field name="time_since_last_bat_msg" type="float" unit="s"/>
+    <field name="time_since_last_msg" type="float" unit="s"/>
   </message>
 
   <message name="TELEMETRY_ERROR" id="33">
diff --git a/conf/modules/baro_board.xml b/conf/modules/baro_board.xml
index 4ace2ff541..2f57658e59 100644
--- a/conf/modules/baro_board.xml
+++ b/conf/modules/baro_board.xml
@@ -6,14 +6,21 @@
   </header>
   <init fun="baro_init()"/>
   <periodic fun="baro_periodic()" freq="60."/>
-  <periodic fun="baro_downlink_raw()" freq="10." autorun="FALSE"/>
   <event fun="BaroEvent(baro_abs,baro_diff)"/>
   <makefile target="ap">
-    <define name="USE_BARO_AS_ALTIMETER"/>
     <file name="baro_board.c" dir="boards/$(BOARD)"/>
-    <define name="ADS1114_I2C_DEVICE" value="i2c1"/>
-    <define name="USE_ADS1114_1"/>
-    <file name="ads1114.c" dir="peripherals"/>
+    <define name="USE_BARO_AS_ALTIMETER"/>
+    <raw>
+ifeq ($(BOARD), navgo)
+ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1
+ap.CFLAGS += -DADS1114_I2C_DEVICE=i2c1
+ap.srcs += peripherals/ads1114.c
+else ifeq ($(BOARD), umarim)
+ap.CFLAGS += -DUSE_I2C1 -DUSE_ADS1114_1
+ap.CFLAGS += -DADS1114_I2C_DEVICE=i2c1
+ap.srcs += peripherals/ads1114.c
+endif
+    </raw>
   </makefile>
 </module>
 
diff --git a/conf/modules/twog_extra_dl.xml b/conf/modules/extra_dl.xml
old mode 100755
new mode 100644
similarity index 90%
rename from conf/modules/twog_extra_dl.xml
rename to conf/modules/extra_dl.xml
index 636238baf9..fa8f94054e
--- a/conf/modules/twog_extra_dl.xml
+++ b/conf/modules/extra_dl.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 
-<module name="extra_dl" dir="core">
+<module name="extra_dl" dir="datalink">
   <header>
     <file name="extra_pprz_dl.h"/>
   </header>
diff --git a/conf/settings/basic.xml b/conf/settings/basic.xml
index aab93f8457..42d515d640 100644
--- a/conf/settings/basic.xml
+++ b/conf/settings/basic.xml
@@ -21,8 +21,8 @@
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot" values="MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink" values="Default|Minimal|Extremal"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink" values="Default|Debug"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink" values="Default|Minimal|Extremal"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink" values="Default|Debug"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
 
diff --git a/conf/settings/basic_infrared.xml b/conf/settings/basic_infrared.xml
index c19cdc299c..37dd032afe 100644
--- a/conf/settings/basic_infrared.xml
+++ b/conf/settings/basic_infrared.xml
@@ -21,14 +21,14 @@
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot" values="MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink" values="Default|Minimal|Extremal"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink" values="Default|Debug"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink" values="Default|Minimal|Extremal"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink" values="Default|Debug"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
 
     <dl_settings NAME="Infrared">
-        <dl_setting MAX="15" MIN="-15" STEP="1" VAR="infrared.roll_neutral" shortname="roll neutral" module="subsystems/sensors/infrared" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch neutral" module="subsystems/sensors/infrared" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.roll_neutral" shortname="roll neutral" module="subsystems/sensors/infrared" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.pitch_neutral" shortname="pitch neutral" module="subsystems/sensors/infrared" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"  alt_unit="deg"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/conf/settings/basic_ins.xml b/conf/settings/basic_ins.xml
index afe5702b71..787d4a4fc7 100644
--- a/conf/settings/basic_ins.xml
+++ b/conf/settings/basic_ins.xml
@@ -21,14 +21,14 @@
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot" values="MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="5" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink" values="Default|Minimal|Extremal|Raw|Scaled|DebugIMU"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink" values="Default|Debug"/>
+      <dl_setting MAX="5" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink" values="Default|Minimal|Extremal|Raw|Scaled|DebugIMU"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink" values="Default|Debug"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
 
     <dl_settings NAME="ins">
-      <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
-      <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/conf/settings/cam.xml b/conf/settings/cam.xml
index e581080f24..85060444bb 100644
--- a/conf/settings/cam.xml
+++ b/conf/settings/cam.xml
@@ -14,8 +14,8 @@
     </dl_settings>
 
     <dl_settings name="angles">
-      <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam_control/cam" VAR="cam_tilt_c"/>
-      <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam_control/cam" VAR="cam_pan_c"/>
+      <dl_setting MAX="30" MIN="-30" STEP="1" module="cam_control/cam" VAR="cam_tilt_c" unit="rad" alt_unit="deg"/>
+      <dl_setting MAX="30" MIN="-30" STEP="1" module="cam_control/cam" VAR="cam_pan_c" unit="rad" alt_unit="deg"/>
     </dl_settings>
 
     <dl_settings name="target">
diff --git a/conf/settings/cam_pitch.xml b/conf/settings/cam_pitch.xml
index fcc99c2ff5..80cace7e60 100644
--- a/conf/settings/cam_pitch.xml
+++ b/conf/settings/cam_pitch.xml
@@ -11,11 +11,11 @@
     </dl_settings>
 
     <dl_settings name="angles">
-      <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam_control/cam" VAR="cam_tilt_c">
+      <dl_setting MAX="30" MIN="-30" STEP="1" module="cam_control/cam" VAR="cam_tilt_c" unit="rad" alt_unit="deg">
         <strip_button name="Look Foreward" icon="lookfore.png" value="-0.5"/>
         <strip_button name="Look Down" icon="lookdown.png" value="0.5"/>
       </dl_setting>
-      <dl_setting MAX="0.5" MIN="-0.5" STEP="0.1" module="cam_control/cam" VAR="cam_pan_c"/>
+      <dl_setting MAX="30" MIN="-30" STEP="1" module="cam_control/cam" VAR="cam_pan_c" unit="rad" alt_unit="deg"/>
     </dl_settings>
 <!--
     <dl_settings name="target">
diff --git a/conf/settings/cam_roll.xml b/conf/settings/cam_roll.xml
index 4a73f03755..3e09e9884f 100644
--- a/conf/settings/cam_roll.xml
+++ b/conf/settings/cam_roll.xml
@@ -3,7 +3,7 @@
 <settings>
   <dl_settings>
     <dl_settings NAME="Cam">
-      <dl_setting MAX="45" MIN="-45" STEP="1" VAR="phi_c" module="cam_control/cam_roll" handler="SetPhiDeg" shortname="phi deg" auto="true">
+      <dl_setting MAX="45" MIN="-45" STEP="1" VAR="cam_roll_phi" module="cam_control/cam_roll" shortname="phi" unit="rad" alt_unit="deg" auto="true">
       </dl_setting>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="cam_roll_mode" module="cam_control/cam_roll" shortname="manual - stablzd">
       </dl_setting>
diff --git a/conf/settings/csc_ap.xml b/conf/settings/csc_ap.xml
deleted file mode 100644
index 3bb0398a29..0000000000
--- a/conf/settings/csc_ap.xml
+++ /dev/null
@@ -1,64 +0,0 @@
-<!DOCTYPE settings SYSTEM "settings.dtd">
-
-<!-- A conf to use for standard operation (no tuning) -->
-
-<settings>
-  <dl_settings>
-    <dl_settings NAME="Misc" >
-      <dl_setting VAR="telemetry_mode_Ap_DefaultChannel" MIN="0" STEP="1" MAX="1" module="csc_telemetry" shortname="telemetry"
-        values="Default|Debug"/>
-    </dl_settings>
-    <dl_settings NAME="imu params">
-      <dl_setting VAR="xsens_setzero" MIN="0" STEP="1" MAX="1" module="csc_xsens" shortname="zero orientation"/>
-      <dl_setting VAR="xsens_psi_offset[0]" MIN="-180" STEP="1" MAX="180" module="csc_xsens" shortname="psi offset"/>
-      <dl_setting VAR="csc_yaw_setpoint_rate" MIN="0" STEP="0.01" MAX="3" module="csc_autopilot" shortname="setpoint rate"/>
-      <dl_setting VAR="csc_yaw_setpoint_range" MIN="0" STEP="0.01" MAX="100" module="csc_autopilot" shortname="setpoint range"/>
-    </dl_settings>
-    <dl_settings NAME="vane sensor">
-      <dl_setting VAR="csc_vane_angle_offset" MIN="-360" STEP="1" MAX="360" module="csc_autopilot" shortname="vane offset"/>
-      <dl_setting VAR="csc_vane_weight" MIN="0" STEP="0.01" MAX="1" module="csc_autopilot" shortname="vane weight" />
-      <dl_setting VAR="csc_vane_filter_constant" MIN="0" STEP="0.01" MAX="1" module="csc_autopilot" shortname="vane filter constant" />
-    </dl_settings>
-    <dl_settings NAME="drag devices">
-      <dl_setting VAR="csc_drag_pitch" MIN="-2" STEP="0.05" MAX="2" module="csc_autopilot" shortname="drag pitch mix"/>
-      <dl_setting VAR="csc_drag_yaw" MIN="-2" STEP="0.05" MAX="2" module="csc_autopilot" shortname="drag yaw mix" />
-    </dl_settings>
-    <dl_settings NAME="flight params">
-      <dl_setting VAR="csc_gains.pitch_kp" MIN="-4000" STEP="10" MAX="4000" module="csc_autopilot" shortname="pitch kp"/>
-      <dl_setting VAR="csc_gains.pitch_kd" MIN="-4000" STEP="10" MAX="4000" module="csc_autopilot" shortname="pitch kd"/>
-      <dl_setting VAR="csc_gains.pitch_ki" MIN="-200" STEP=".1" MAX="200" module="csc_autopilot" shortname="pitch ki" handler="set_pitch_ki"/>
-      <dl_setting VAR="csc_gains.roll_kp" MIN="-8000" STEP="10" MAX="8000" module="csc_autopilot" shortname="roll kp"/>
-      <dl_setting VAR="csc_gains.roll_kd" MIN="-4000" STEP="10" MAX="4000" module="csc_autopilot" shortname="roll kd"/>
-      <dl_setting VAR="csc_gains.roll_ki" MIN="-200" STEP=".1" MAX="200" module="csc_autopilot" shortname="roll ki" handler="set_roll_ki"/>
-      <dl_setting VAR="csc_gains.yaw_kp" MIN="-10000" STEP="10" MAX="10000" module="csc_autopilot" shortname="yaw kp"/>
-      <dl_setting VAR="csc_gains.yaw_kd" MIN="-4000" STEP="10" MAX="4000" module="csc_autopilot" shortname="yaw kd"/>
-      <dl_setting VAR="csc_gains.yaw_ki" MIN="-200" STEP=".1" MAX="200" module="csc_autopilot" shortname="yaw ki" handler="set_yaw_ki"/>
-      <dl_setting VAR="csc_yaw_rudder" MIN="-5" STEP="0.01" MAX="5" module="csc_autopilot" shortname="yaw rudder"/>
-      <dl_setting VAR="csc_yaw_aileron" MIN="-5" STEP="0.01" MAX="5" module="csc_autopilot" shortname="yaw aileron"/>
-      <dl_setting VAR="csc_yaw_deadband" MIN="-30" STEP="0.01" MAX="30" module="csc_autopilot" shortname="yaw deadband"/>
-    </dl_settings>
-    <dl_settings NAME="self-tuning PID">
-      <dl_setting VAR="csc_gamma.pitch_kp" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="pitch kp"/>
-      <dl_setting VAR="csc_gamma.pitch_kd" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="pitch kd"/>
-      <dl_setting VAR="csc_gamma.pitch_ki" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="pitch ki" handler="set_pitch_ki"/>
-      <dl_setting VAR="csc_gamma.roll_kp" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="roll kp"/>
-      <dl_setting VAR="csc_gamma.roll_kd" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="roll kd"/>
-      <dl_setting VAR="csc_gamma.roll_ki" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="roll ki" handler="set_roll_ki"/>
-      <dl_setting VAR="csc_gamma.yaw_kp" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="yaw kp"/>
-      <dl_setting VAR="csc_gamma.yaw_kd" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="yaw kd"/>
-      <dl_setting VAR="csc_gamma.yaw_ki" MIN="-20" STEP="0.01" MAX="20" module="csc_autopilot" shortname="yaw ki" handler="set_yaw_ki"/>
-    </dl_settings>
-    <dl_settings NAME="trims">
-      <dl_setting VAR="csc_trims_set" MIN="0" STEP="1" MAX="1" module="csc_ap_main" shortname="set trims"/>
-      <dl_setting VAR="csc_trims.elevator" MIN="-6000" STEP="10" MAX="6000" module="csc_autopilot" shortname="elevator trim"/>
-      <dl_setting VAR="csc_trims.aileron" MIN="-6000" STEP="10" MAX="6000" module="csc_autopilot" shortname="aileron trim"/>
-      <dl_setting VAR="csc_trims.rudder" MIN="-6000" STEP="10" MAX="6000" module="csc_autopilot" shortname="rudder trim"/>
-    </dl_settings>
-    <dl_settings NAME="GPS">
-      <dl_setting VAR="csc_gps_setzero" MIN="0" STEP="1" MAX="1" module="csc_autopilot" shortname="zero GPS"/>
-      <dl_setting VAR="csc_gps_weight" MIN="-2" STEP="0.01" MAX="2" module="csc_autopilot" shortname="GPS weight"/>
-      <dl_setting VAR="csc_gps_gain" MIN="-1" STEP="0.0001" MAX="1" module="csc_autopilot" shortname="GPS gain"/>
-      <dl_setting VAR="csc_gps_filter_weight" MIN="0" STEP="0.0001" MAX="1" module="csc_autopilot" shortname="GPS low-pass coeff"/>
-    </dl_settings>
-  </dl_settings>
-</settings>
diff --git a/conf/settings/flight_params.xml b/conf/settings/flight_params.xml
index 00b68d0496..62f6d2411b 100644
--- a/conf/settings/flight_params.xml
+++ b/conf/settings/flight_params.xml
@@ -26,8 +26,8 @@
       <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink" values="Default|Minimal|Extremal"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink" values="Default|Debug"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink" values="Default|Minimal|Extremal"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink" values="Default|Debug"/>
 
 
     </dl_settings>
diff --git a/conf/settings/infrared.xml b/conf/settings/infrared.xml
index ee2d434796..4a63671869 100644
--- a/conf/settings/infrared.xml
+++ b/conf/settings/infrared.xml
@@ -3,8 +3,8 @@
 <settings>
   <dl_settings>
     <dl_settings NAME="ir">
-      <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
-      <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
 
       <dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared" param="IR_LATERAL_CORRECTION"/>
       <dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
diff --git a/conf/settings/ins_arduimu_basic.xml b/conf/settings/ins_arduimu_basic.xml
index b3f3818b79..73abc529c7 100644
--- a/conf/settings/ins_arduimu_basic.xml
+++ b/conf/settings/ins_arduimu_basic.xml
@@ -3,8 +3,8 @@
 <settings>
   <dl_settings>
     <dl_settings NAME="ins">
-      <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
-      <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_roll_neutral" shortname="roll_neutral" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="arduimu_calibrate_neutrals" shortname="calibrate" values="FALSE|TRUE"/>
     </dl_settings>
   </dl_settings>
diff --git a/conf/settings/ins_neutrals.xml b/conf/settings/ins_neutrals.xml
index 386a15c7db..bce83b2ed8 100644
--- a/conf/settings/ins_neutrals.xml
+++ b/conf/settings/ins_neutrals.xml
@@ -3,8 +3,8 @@
 <settings>
   <dl_settings>
     <dl_settings NAME="ins">
-      <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
-      <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/conf/settings/ir_i2c.xml b/conf/settings/ir_i2c.xml
index b386b4c6d9..c67299ac4f 100644
--- a/conf/settings/ir_i2c.xml
+++ b/conf/settings/ir_i2c.xml
@@ -3,8 +3,8 @@
 <settings>
   <dl_settings>
     <dl_settings NAME="IR I2C">
-      <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
-      <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+      <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
 
       <dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared" param="IR_LATERAL_CORRECTION"/>
       <dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
diff --git a/conf/settings/lisa.xml b/conf/settings/lisa.xml
index 9f95c86edb..4ab6fa9e0c 100644
--- a/conf/settings/lisa.xml
+++ b/conf/settings/lisa.xml
@@ -24,7 +24,7 @@
       </dl_setting>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle">
       </dl_setting>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
     </dl_settings>
 
diff --git a/conf/settings/mercury_csc.xml b/conf/settings/mercury_csc.xml
deleted file mode 100644
index f46fd98a93..0000000000
--- a/conf/settings/mercury_csc.xml
+++ /dev/null
@@ -1,29 +0,0 @@
-<!DOCTYPE settings SYSTEM "settings.dtd">
-
-<!-- A conf to use for standard operation (no tuning) -->
-
-<settings>
-  <dl_settings>
-       <dl_settings NAME="Misc" >
-      <dl_setting VAR="telemetry_mode_Ap_DefaultChannel" MIN="0" STEP="1" MAX="1" module="csc/csc_telemetry" shortname="telemetry"
-        values="Default|Debug"/>
-    </dl_settings>
-<!--    <dl_settings NAME="imu params">
-      <dl_setting VAR="xsens_setzero" MIN="0" STEP="1" MAX="1" module="csc/mercury_xsens" shortname="zero orientation"/>
-    </dl_settings>-->
-<!--    <dl_settings NAME="flight params">
-      <dl_setting VAR="csc_gains.pitch_kp" MIN="0" STEP="1" MAX="8000" module="csc/csc_ap" shortname="pitch kp"/>
-      <dl_setting VAR="csc_gains.pitch_kd" MIN="0" STEP="1" MAX="3000" module="csc/csc_ap" shortname="ptch kd"/>
-      <dl_setting VAR="csc_gains.roll_kp" MIN="0" STEP="1" MAX="8000" module="csc/csc_ap" shortname="roll kp"/>
-      <dl_setting VAR="csc_gains.roll_kd" MIN="0" STEP="1" MAX="3000" module="csc/csc_ap" shortname="roll kd"/>
-      <dl_setting VAR="csc_gains.yaw_kp" MIN="0" STEP="1" MAX="18000" module="csc/csc_ap" shortname="yaw kp"/>
-      <dl_setting VAR="csc_gains.yaw_kd" MIN="0" STEP="1" MAX="3000" module="csc/csc_ap" shortname="yaw kd"/>
-    </dl_settings>
-    <dl_settings NAME="trims">
-      <dl_setting VAR="csc_trims.elevator" MIN="-6000" STEP="10" MAX="6000" module="csc/csc_ap" shortname="elevator trim"/>
-      <dl_setting VAR="csc_trims.aileron" MIN="-6000" STEP="10" MAX="6000" module="csc/csc_ap" shortname="aileron trim"/>
-      <dl_setting VAR="csc_trims.rudder" MIN="-6000" STEP="10" MAX="6000" module="csc/csc_ap" shortname="rudder trim"/>
-    </dl_settings> -->
-
-  </dl_settings>
-</settings>
diff --git a/conf/settings/photogrammetry_calculator.xml b/conf/settings/photogrammetry_calculator.xml
index f086a5f1cb..9a44f87b94 100644
--- a/conf/settings/photogrammetry_calculator.xml
+++ b/conf/settings/photogrammetry_calculator.xml
@@ -4,7 +4,7 @@
   <dl_settings name="control">
     <dl_settings name="photocalc">
 
-      <dl_setting max="90" min="-90" step="1" module="cartography/photogrammetry_calculator" var="photogrammetry_sweep_angle" shortname="Angle"  param="PHOTOGRAMMETRY_SWEEP_ANGLE" />
+      <dl_setting max="90" min="-90" step="1" module="cartography/photogrammetry_calculator" var="photogrammetry_sweep_angle" shortname="Angle" param="PHOTOGRAMMETRY_SWEEP_ANGLE" unit="rad" alt_unit="deg"/>
 
       <dl_setting max="95" min="0" step="1" VAR="photogrammetry_sidelap" module="cartography/photogrammetry_calculator" shortname="SideLap %" param="PHOTOGRAMMETRY_SIDELAP" handler="UpdateSideLap" />
       <dl_setting max="95" min="0" step="1" var="photogrammetry_overlap" module="cartography/photogrammetry_calculator" shortname="OverLap %" param="PHOTOGRAMMETRY_OVERLAP" handler="UpdateOverLap" />
diff --git a/conf/settings/tuning.xml b/conf/settings/tuning.xml
index 6b9f8bfe70..80375b75d6 100644
--- a/conf/settings/tuning.xml
+++ b/conf/settings/tuning.xml
@@ -17,8 +17,8 @@
       <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink" values="Default|Minimal|Extremal"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink" values="Default|Debug"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink" values="Default|Minimal|Extremal"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink" values="Default|Debug"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
@@ -31,6 +31,11 @@
 
     <dl_settings NAME="control">
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="attitude">
         <dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="1" MIN="0" STEP="0.05" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT"/>
diff --git a/conf/settings/tuningJH.xml b/conf/settings/tuningJH.xml
index 0325681f22..573ed4302d 100644
--- a/conf/settings/tuningJH.xml
+++ b/conf/settings/tuningJH.xml
@@ -23,8 +23,8 @@
 
     <dl_settings NAME="control">
       <dl_settings NAME="infrared">
-        <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral"            shortname="roll_neutral"  param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" module="subsystems/sensors/infrared" />
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral"           shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.roll_neutral"            shortname="roll_neutral"  param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" module="subsystems/sensors/infrared" />
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.pitch_neutral"           shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
 
         <dl_setting MAX="1.5" MIN="0." STEP="0.1"    VAR="infrared.lateral_correction"      shortname="lat_corr"      param="IR_LATERAL_CORRECTION"/>
         <dl_setting MAX="1.5" MIN="0." STEP="0.1"    VAR="infrared.longitudinal_correction" shortname="log_corr"      param="IR_LONGITUDINAL_CORRECTION"/>
diff --git a/conf/settings/tuning_ctl_adaptive.xml b/conf/settings/tuning_ctl_adaptive.xml
index 53980acda5..ff57cd5608 100644
--- a/conf/settings/tuning_ctl_adaptive.xml
+++ b/conf/settings/tuning_ctl_adaptive.xml
@@ -6,6 +6,11 @@
   <dl_settings>
 
     <dl_settings NAME="control">
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="attitude">
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>
@@ -36,15 +41,11 @@
 
         <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_pgain" shortname="pitch_p" param="V_CTL_AUTO_PITCH_PGAIN"/>
         <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_igain" shortname="pitch_i" param="V_CTL_AUTO_PITCH_IGAIN"/>
-        <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_dgain" shortname="pitch_d" param="V_CTL_AUTO_PITCH_DGAIN" module="guidance/guidance_v_n"/>
 
         <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_p" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
         <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_igain" shortname="throttle_i" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
         <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_dgain" shortname="throttle_d" param="V_CTL_AUTO_THROTTLE_DGAIN"/>
 
-        <dl_setting MAX="0" MIN="-1." STEP="0.01" VAR="v_ctl_pitch_dash_trim" shortname="dash trim" param="V_CTL_PITCH_DASH_TRIM"/>
-        <dl_setting MIN="0" MAX="1." STEP="0.01" VAR="v_ctl_pitch_loiter_trim" shortname="loiter trim" param="V_CTL_PITCH_LOITER_TRIM"/>
-
         <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr" param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
         <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz" param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
       </dl_settings>
diff --git a/conf/settings/tuning_ctl_new.xml b/conf/settings/tuning_ctl_new.xml
index 73de84bf6f..2a8e4d9a82 100644
--- a/conf/settings/tuning_ctl_new.xml
+++ b/conf/settings/tuning_ctl_new.xml
@@ -1,11 +1,16 @@
 <!DOCTYPE settings SYSTEM "settings.dtd">
 
-<!-- A conf to use to tune a new A/C -->
+<!-- A conf to use to tune a new A/C without airspeed -->
 
 <settings>
   <dl_settings>
 
     <dl_settings NAME="control">
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="attitude">
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
         <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>
@@ -47,26 +52,10 @@
         <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_igain" shortname="throttle_i" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
         <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_dgain" shortname="throttle_d" param="V_CTL_AUTO_THROTTLE_DGAIN"/>
 
-        <dl_setting MAX="0" MIN="-1." STEP="0.01" VAR="v_ctl_pitch_dash_trim" shortname="dash trim" param="V_CTL_PITCH_DASH_TRIM"/>
-        <dl_setting MIN="0" MAX="1." STEP="0.01" VAR="v_ctl_pitch_loiter_trim" shortname="loiter trim" param="V_CTL_PITCH_LOITER_TRIM"/>
-
         <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr" param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
         <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz" param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
       </dl_settings>
 
-      <dl_settings name="airspeed">
-        <dl_setting max="40" min="5" step="0.1" var="v_ctl_auto_airspeed_setpoint" shortname="as_sp"/>
-        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_pgain" shortname="as_t_pgain"/>
-        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_dgain" shortname="as_t_pgain"/>
-        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_igain" shortname="as_t_igain"/>
-        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_pgain" shortname="as_p_pgain"/>
-        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_dgain" shortname="as_p_pgain"/>
-        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_igain" shortname="as_p_igain"/>
-        <dl_setting max="40" min="5" step="0.1" var="v_ctl_auto_groundspeed_setpoint" shortname="gs_sp"/>
-        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_pgain" shortname="gs_pgain"/>
-        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_igain" shortname="gs_igain"/>
-      </dl_settings>
-
       <dl_settings name="nav">
         <dl_setting MAX="3" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain" param="H_CTL_COURSE_PGAIN"/>
         <dl_setting MAX="2" MIN="0" STEP="0.1" VAR="h_ctl_course_dgain" shortname="course dgain"/>
diff --git a/conf/settings/tuning_ctl_new_airspeed.xml b/conf/settings/tuning_ctl_new_airspeed.xml
new file mode 100644
index 0000000000..2d0a4177ca
--- /dev/null
+++ b/conf/settings/tuning_ctl_new_airspeed.xml
@@ -0,0 +1,90 @@
+<!DOCTYPE settings SYSTEM "settings.dtd">
+
+<!-- A conf to use to tune a new A/C with airspeed-->
+
+<settings>
+  <dl_settings>
+
+    <dl_settings NAME="control">
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
+      <dl_settings NAME="attitude">
+        <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
+        <dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" param="H_CTL_ROLL_IGAIN" handler="SetRollIGain"/>
+        <dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
+        <dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" param="H_CTL_PITCH_IGAIN" handler="SetPitchIGain"/>
+        <dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL"/>
+        <dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle" module="stabilization/stabilization_adaptive"/>
+
+        <dl_setting MAX="60" MIN="0" STEP="1" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
+      </dl_settings>
+
+      <dl_settings NAME="feedforward">
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffa" shortname="roll_Kffa" param="H_CTL_ROLL_KFFA"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffd" shortname="roll_Kffd" param="H_CTL_ROLL_KFFD"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffa" shortname="pitch_Kffa" param="H_CTL_PITCH_KFFA"/>
+        <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffd" shortname="pitch_Kffd" param="H_CTL_PITCH_KFFD"/>
+      </dl_settings>
+
+      <dl_settings name="alt">
+        <dl_setting MAX="0" MIN="-0.2" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN"/>
+      </dl_settings>
+
+      <dl_settings name="climb">
+        <dl_setting MAX="2" MIN="0" STEP="1" var="v_ctl_speed_mode" shortname="speed mode" values="THROTTLE|AIRSPEED|GROUNDSPEED">
+          <strip_button name="TH" value="0" group="speed_mode"/>
+          <strip_button name="AS" value="1" group="speed_mode"/>
+          <strip_button name="GS" value="2" group="speed_mode"/>
+        </dl_setting>
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" module="guidance/guidance_v" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE"/>
+
+        <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_pgain" shortname="pitch_p" param="V_CTL_AUTO_PITCH_PGAIN"/>
+        <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_igain" shortname="pitch_i" param="V_CTL_AUTO_PITCH_IGAIN"/>
+        <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_dgain" shortname="pitch_d" param="V_CTL_AUTO_PITCH_DGAIN" module="guidance/guidance_v_n"/>
+
+        <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_p" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
+        <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_igain" shortname="throttle_i" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
+        <dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_dgain" shortname="throttle_d" param="V_CTL_AUTO_THROTTLE_DGAIN"/>
+
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr" param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz" param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
+      </dl_settings>
+
+      <dl_settings name="airspeed">
+        <dl_setting max="40" min="5" step="0.1" var="v_ctl_auto_airspeed_setpoint" shortname="as_sp"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_pgain" shortname="as_t_pgain"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_dgain" shortname="as_t_pgain"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_throttle_igain" shortname="as_t_igain"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_pgain" shortname="as_p_pgain"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_dgain" shortname="as_p_pgain"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_airspeed_pitch_igain" shortname="as_p_igain"/>
+        <dl_setting max="40" min="5" step="0.1" var="v_ctl_auto_groundspeed_setpoint" shortname="gs_sp"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_pgain" shortname="gs_pgain"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_igain" shortname="gs_igain"/>
+      </dl_settings>
+
+      <dl_settings name="nav">
+        <dl_setting MAX="-0.1" MIN="-3" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain" param="H_CTL_COURSE_PGAIN"/>
+        <dl_setting MAX="2" MIN="0" STEP="0.1" VAR="h_ctl_course_dgain" shortname="course dgain"/>
+        <dl_setting MAX="2" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor"/>
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="nav_glide_pitch_trim" shortname="glide pitch trim" param="NAV_GLIDE_PITCH_TRIM"/>
+        <dl_setting MAX="1" MIN="0.02" STEP="0.01" VAR="h_ctl_roll_slew" shortname="roll slew"/>
+        <dl_setting MAX="500" MIN="-500" STEP="5" VAR="nav_radius"/>
+        <dl_setting MAX="359" MIN="0" STEP="5" VAR="nav_course"/>
+        <dl_setting MAX="2" MIN="1" STEP="1" VAR="nav_mode"/>
+        <dl_setting MAX="5" MIN="-5" STEP="0.5" VAR="nav_climb"/>
+        <dl_setting MAX="20" MIN="-20" STEP="1" VAR="fp_pitch"/>
+        <dl_setting MAX="50" MIN="-50" STEP="5" VAR="nav_shift" module="subsystems/nav" handler="IncreaseShift" shortname="inc. shift"/>
+        <dl_setting MAX="50" MIN="5" STEP="0.5" VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
+        <dl_setting MAX="0." MIN="-0.2" STEP="0.01" VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
+        <dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
+      </dl_settings>
+    </dl_settings>
+  </dl_settings>
+</settings>
diff --git a/conf/settings/tuning_infrared.xml b/conf/settings/tuning_infrared.xml
index 518fd99c0c..94cd218dfd 100644
--- a/conf/settings/tuning_infrared.xml
+++ b/conf/settings/tuning_infrared.xml
@@ -17,8 +17,8 @@
       <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink" values="Default|Minimal|Extremal"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink" values="Default|Debug"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink" values="Default|Minimal|Extremal"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink" values="Default|Debug"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
@@ -31,8 +31,8 @@
 
     <dl_settings NAME="control">
       <dl_settings NAME="ir">
-        <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
 
         <dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared" param="IR_LATERAL_CORRECTION"/>
         <dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
diff --git a/conf/settings/tuning_ins.xml b/conf/settings/tuning_ins.xml
index 23bc414069..da1020dd73 100644
--- a/conf/settings/tuning_ins.xml
+++ b/conf/settings/tuning_ins.xml
@@ -17,8 +17,8 @@
       <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="5" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink" values="Default|Minimal|Extremal|Raw|Scaled|DebugIMU"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"  values="Default|Debug"/>
+      <dl_setting MAX="5" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink" values="Default|Minimal|Extremal|Raw|Scaled|DebugIMU"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink"  values="Default|Debug"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
@@ -31,9 +31,14 @@
 
     <dl_settings NAME="control">
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="ins">
-        <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
       </dl_settings>
 
 
diff --git a/conf/settings/tuning_ins_dcm.xml b/conf/settings/tuning_ins_dcm.xml
index 4a6dc94589..f89fbd498a 100644
--- a/conf/settings/tuning_ins_dcm.xml
+++ b/conf/settings/tuning_ins_dcm.xml
@@ -17,8 +17,8 @@
       <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="5" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink" values="Default|Minimal|Extremal|Raw|Scaled|DebugIMU"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink" values="Default|Debug"/>
+      <dl_setting MAX="5" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink" values="Default|Minimal|Extremal|Raw|Scaled|DebugIMU"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink" values="Default|Debug"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
@@ -31,12 +31,16 @@
 
     <dl_settings NAME="control">
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
       <dl_settings NAME="ins">
         <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_health" shortname="health" module="subsystems/ahrs" />
         <dl_setting MAX="100" MIN="0" STEP="1" VAR="renorm_sqrt_count" shortname="err_norm" module="subsystems/ahrs" />
-        <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun" shortname="ADC overrun" module="subsystems/imu" />
-        <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
       </dl_settings>
 
 
diff --git a/conf/settings/tuning_loiter.xml b/conf/settings/tuning_loiter.xml
index e6acdff1b7..8b5b7966a2 100644
--- a/conf/settings/tuning_loiter.xml
+++ b/conf/settings/tuning_loiter.xml
@@ -17,8 +17,8 @@
       <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps.reset" module="subsystems/gps" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
@@ -31,8 +31,8 @@
 
     <dl_settings NAME="control">
       <dl_settings NAME="infrared">
-        <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral"            shortname="roll_neutral"  param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" module="subsystems/sensors/infrared" />
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral"           shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.roll_neutral"            shortname="roll_neutral"  param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" module="subsystems/sensors/infrared" />
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.pitch_neutral"           shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg"/>
 
         <dl_setting MAX="1.5" MIN="0." STEP="0.1"    VAR="infrared.lateral_correction"      shortname="lat_corr"      param="IR_LATERAL_CORRECTION"/>
         <dl_setting MAX="1.5" MIN="0." STEP="0.1"    VAR="infrared.longitudinal_correction" shortname="log_corr"      param="IR_LONGITUDINAL_CORRECTION"/>
@@ -44,6 +44,10 @@
         <dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down"           shortname="corr_down"     param="IR_CORRECTION_DOWN"/>
       </dl_settings>
 
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_roll_trim" shortname="roll_trim" module="commands" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
 
       <dl_settings NAME="attitude">
         <dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
diff --git a/conf/settings/tuning_pers.xml b/conf/settings/tuning_pers.xml
index 053c62a7a4..18edc644e8 100644
--- a/conf/settings/tuning_pers.xml
+++ b/conf/settings/tuning_pers.xml
@@ -17,8 +17,8 @@
       <dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
       <dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
-      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
+      <dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="firmwares/fixedwing/ap_downlink"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="firmwares/fixedwing/fbw_downlink"/>
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
 
       <dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
@@ -31,8 +31,8 @@
 
     <dl_settings NAME="control">
       <dl_settings NAME="ir">
-        <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad"  persistent="true" />
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"  persistent="true" />
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" persistent="true" />
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" persistent="true" />
 
         <dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="360_lat_corr" module="subsystems/sensors/infrared" param="IR_LATERAL_CORRECTION"/>
         <dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="360_log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
diff --git a/conf/settings/tuning_tp_auto.xml b/conf/settings/tuning_tp_auto.xml
index 25ca72cd39..a1e8f5a52e 100644
--- a/conf/settings/tuning_tp_auto.xml
+++ b/conf/settings/tuning_tp_auto.xml
@@ -23,8 +23,8 @@
 
     <dl_settings NAME="control">
       <dl_settings NAME="ir">
-        <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" module="infrared"/>
-        <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.roll_neutral" shortname="roll_neutral" module="infrared" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="15" MIN="-15" STEP="0.5" VAR="infrared.pitch_neutral" shortname="pitch_neutral" unit="rad" alt_unit="deg"/>
 
       </dl_settings>
 
diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml
deleted file mode 100644
index 6387de0834..0000000000
--- a/conf/telemetry/booz.xml
+++ /dev/null
@@ -1,69 +0,0 @@
-<?xml version="1.0"?>
-<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
-<telemetry>
-
-
-  <process name="Controller">
-    <mode name="default">
-      <message name="ALIVE"             period="5."/>
-      <message name="ROTORCRAFT_STATUS"       period=".5"/>
-<!--      <message name="RC"                period="0.5"/>  -->
-<!--      <message name="BOOZ_FD"           period="0.05"/> -->
-<!--      <message name="PPRZ_DEBUG"        period="0.25"/> -->
-      <message name="ACTUATORS"         period="0.5"/>
-<!--      <message name="BOOZ_HOV_LOOP"     period="0.05"/> -->
-<!--      <message name="BOOZ_VERT_LOOP"    period="0.05"/> -->
-<!--      <message name="BOOZ_CONTROL"      period="0.025"/> -->
-<!--      <message name="BOOZ_CMDS"         period="0.025"/> -->
-<!--      <message name="BOOZ_UF_RATES"     period="0.05"/> -->
-      <message name="DL_VALUE"          period="1."/>
-    </mode>
-    <mode name="debug">
-      <message name="DL_VALUE"          period=".5"/>
-    </mode>
-  </process>
-
-
-
-  <process name="Filter">
-
-    <mode name="default">
-<!--      <message name="IMU_GYRO_LP"           period=".017"/> -->
-      <message name="AHRS_STATE"            period=".5"/>
-      <message name="AHRS_COV"              period=".5"/>
-      <message name="AHRS_MEASURE"          period=".5"/>
-<!--      <message name="IMU_ACCEL_RAW"         period=".02"/> -->
-      <message name="IMU_ACCEL"             period=".5"/>
-<!--      <message name="IMU_GYRO_RAW"          period=".05"/> -->
-      <message name="IMU_GYRO"              period=".5"/>
-<!--      <message name="IMU_MAG_RAW"           period=".05"/>
-      <message name="IMU_MAG"               period=".5"/>
-      <message name="IMU_PRESSURE"          period=".5"/>
-      <message name="INS_STATE"             period=".1"/> -->
-    </mode>
-
-    <mode name="raw_sensors">
-      <message name="IMU_GYRO_RAW"          period=".017"/>
-    </mode>
-
-    <mode name="scaled_sensors">
-      <message name="IMU_GYRO"              period=".017"/>
-    </mode>
-
-    <mode name="simulation">
-<!--      <message name="PPRZ_DEBUG"            period=".1"/>-->
-      <message name="AHRS_STATE"            period=".1"/>
-      <message name="AHRS_COV"              period=".1"/>
-      <message name="IMU_GYRO"              period=".017"/>
-<!--      <message name="IMU_GYRO_LP"           period=".017"/> -->
-      <message name="IMU_GYRO_RAW"          period=".25"/>
-      <message name="IMU_ACCEL"             period=".25"/>
-      <message name="IMU_ACCEL_RAW"         period=".25"/>
-      <message name="IMU_MAG"               period="1."/>
-      <message name="IMU_MAG_RAW"           period="1."/>
-    </mode>
-
-  </process>
-
-
-</telemetry>
diff --git a/conf/telemetry/csc.xml b/conf/telemetry/csc.xml
deleted file mode 100644
index d0e1203e96..0000000000
--- a/conf/telemetry/csc.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<?xml version="1.0"?>
-<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
-<telemetry>
-  <process name="Ap">
-    <mode name="default">
-      <message name="BOOZ2_GPS"      period="0.1"/>
-    </mode>
-  </process>
-</telemetry>
diff --git a/conf/telemetry/csc_ap.xml b/conf/telemetry/csc_ap.xml
deleted file mode 100644
index 22ee700835..0000000000
--- a/conf/telemetry/csc_ap.xml
+++ /dev/null
@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
-<telemetry>
-  <process name="Ap">
-    <mode name="default">
-      <message name="ALIVE"          period="5"/>
-      <message name="ATTITUDE"	     period="0.1"/>
-      <message name="INS3"     period="0.25"/>
-      <message name="GPS_SOL"	     period="1.5"/>
-      <message name="VANE_SENSOR"    period="0.2"/>
-      <message name="SIMPLE_COMMANDS" period="0.2"/>
-      <message name="FBW_STATUS"    period="1.5"/>
-      <message name="DL_VALUE"       period="1.5"/>
-      <message name="CONTROLLER_GAINS" period="1"/>
-      <message name="GPS_ERROR"        period="0.25"/>
-      <message name="GPS"	     period="1.5"/>
-    </mode>
-    <mode name="debug">
-      <message name="ALIVE"          period="5"/>
-      <message name="ATTITUDE"	     period="0.5"/>
-      <message name="COMMANDS"       period="0.5"/>
-      <message name="ACTUATORS"      period="0.5"/>
-      <message name="FBW_STATUS"    period="1.1"/>
-      <message name="RC"	     period="0.5"/>
-      <message name="IMU_ACCEL"      period="0.4"/>
-      <message name="IMU_GYRO"       period="0.2"/>
-      <message name="IMU_MAG"        period="0.4"/>
-      <message name="DL_VALUE"       period="0.5"/>
-      <message name="RMAT_DEBUG"     period="1"/>
-      <message name="GPS_SOL"	     period="1.5"/>
-      <message name="INS3"     period="0.25"/>
-    </mode>
-  </process>
-</telemetry>
diff --git a/conf/telemetry/default_fixedwing_imu.xml b/conf/telemetry/default_fixedwing_imu.xml
index 1b83499095..f7f832e34d 100644
--- a/conf/telemetry/default_fixedwing_imu.xml
+++ b/conf/telemetry/default_fixedwing_imu.xml
@@ -69,13 +69,14 @@
       <message name="IMU_ACCEL_RAW"     period=".05"/>
       <message name="IMU_GYRO_RAW"      period=".05"/>
       <message name="IMU_MAG_RAW"       period=".05"/>
+      <message name="BARO_RAW"          period="0.5"/>
     </mode>
     <mode name="scaled_sensors">
       <message name="DL_VALUE"          period="0.5"/>
       <message name="ALIVE"             period="2.1"/>
-      <message name="IMU_GYRO"   period=".075"/>
-      <message name="IMU_ACCEL"  period=".075"/>
-      <message name="IMU_MAG"    period=".1"/>
+      <message name="IMU_GYRO"          period=".075"/>
+      <message name="IMU_ACCEL"         period=".075"/>
+      <message name="IMU_MAG"           period=".1"/>
     </mode>
     <mode name="debug_imu">
       <message name="ATTITUDE"            period="0.1"/>
diff --git a/conf/telemetry/default_fixedwing_imu_9k6.xml b/conf/telemetry/default_fixedwing_imu_9k6.xml
index df79bb466c..e97b9046a9 100644
--- a/conf/telemetry/default_fixedwing_imu_9k6.xml
+++ b/conf/telemetry/default_fixedwing_imu_9k6.xml
@@ -69,6 +69,7 @@
       <message name="IMU_ACCEL_RAW"     period=".05"/>
       <message name="IMU_GYRO_RAW"      period=".05"/>
       <message name="IMU_MAG_RAW"       period=".05"/>
+      <message name="BARO_RAW"          period="0.5"/>
     </mode>
     <mode name="scaled_sensors">
       <message name="DL_VALUE"          period="0.5"/>
diff --git a/conf/telemetry/mercury_csc.xml b/conf/telemetry/mercury_csc.xml
deleted file mode 100644
index e860073d78..0000000000
--- a/conf/telemetry/mercury_csc.xml
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
-<telemetry>
-  <process name="Ap">
-    <mode name="default">
-      <message name="ALIVE"          period="5"/>
-<!--      <message name="ATTITUDE"	     period="0.04"/> -->
-<!--      <message name="QUAD_STATUS"    period="1.1"/> -->
-      <message name="ACTUATORS" period="0.08" />
-      <message name="DL_VALUE"       period="1.5"/>
-    </mode>
-  </process>
-</telemetry>
diff --git a/conf/telemetry/telemetry.dtd b/conf/telemetry/telemetry.dtd
index 2c7395c368..99e0bb35a1 100644
--- a/conf/telemetry/telemetry.dtd
+++ b/conf/telemetry/telemetry.dtd
@@ -7,6 +7,7 @@
 <!ATTLIST process
   name CDATA #REQUIRED
   channel CDATA "DefaultChannel"
+  device CDATA "DefaultDevice"
 >
 <!ATTLIST mode
   name CDATA #REQUIRED
diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index 36de95539b..5340d21ec2 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -22,7 +22,7 @@
       <message name="BOOZ2_CMD"           period=".05"/>
       <message name="PPM"                 period="0.5"/>
       <message name="RC"                  period="0.5"/>
-      <message name="BOOZ2_RADIO_CONTROL" period="0.5"/>
+      <message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
       <message name="ROTORCRAFT_STATUS"       period="1"/>
     </mode>
 
@@ -49,11 +49,11 @@
       <message name="ROTORCRAFT_STATUS"       period="1.2"/>
       <message name="DL_VALUE"          period="0.5"/>
       <message name="ALIVE"             period="2.1"/>
-<!--      <message name="BOOZ2_ALIGNER"     period=".1"/> -->
+<!--      <message name="FILTER_ALIGNER"     period=".1"/> -->
       <message name="FILTER"      period=".5"/>
-<!--      <message name="BOOZ2_AHRS_QUAT"   period=".25"/> -->
-      <message name="BOOZ2_AHRS_EULER"  period=".1"/>
-<!--      <message name="BOOZ2_AHRS_RMAT"   period=".5"/> -->
+<!--      <message name="AHRS_QUAT_INT"   period=".25"/> -->
+      <message name="AHRS_EULER_INT"  period=".1"/>
+<!--      <message name="AHRS_RMAT_INT"   period=".5"/> -->
     </mode>
 
     <mode name="rate_loop">
@@ -113,7 +113,7 @@
       <message name="ALIVE"             period="2.1"/>
       <!--<message name="BOOZ2_SONAR"       period="0.1"/>-->
       <!--<message name="BOOZ2_TUNE_HOVER"               period=".1"/>-->
-      <!-- <message name="BOOZ2_GPS"               period=".20"/> -->
+      <!-- <message name="GPS_INT"               period=".20"/> -->
       <!--<message name="INS2"              period=".05"/>
       <message name="INS3"              period=".20"/>-->
       <message name="INS_REF"           period="5.1"/>
diff --git a/conf/telemetry/telemetry_jtm.xml b/conf/telemetry/telemetry_jtm.xml
index 2798c506e8..2b1d290955 100644
--- a/conf/telemetry/telemetry_jtm.xml
+++ b/conf/telemetry/telemetry_jtm.xml
@@ -22,7 +22,7 @@
       <message name="BOOZ2_CMD"           period=".05"/>
       <message name="PPM"                 period="0.5"/>
       <message name="RC"                  period="0.5"/>
-      <message name="BOOZ2_RADIO_CONTROL" period="0.5"/>
+      <message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
       <message name="ROTORCRAFT_STATUS"       period="1"/>
     </mode>
 
@@ -49,11 +49,11 @@
       <message name="ROTORCRAFT_STATUS"       period="1.2"/>
       <message name="DL_VALUE"          period="0.5"/>
       <message name="ALIVE"             period="2.1"/>
-<!--      <message name="BOOZ2_ALIGNER"     period=".1"/> -->
+<!--      <message name="FILTER_ALIGNER"     period=".1"/> -->
       <message name="FILTER"      period=".5"/>
-<!--      <message name="BOOZ2_AHRS_QUAT"   period=".25"/> -->
-      <message name="BOOZ2_AHRS_EULER"  period=".1"/>
-<!--      <message name="BOOZ2_AHRS_RMAT"   period=".5"/> -->
+<!--      <message name="AHRS_QUAT_INT"   period=".25"/> -->
+      <message name="AHRS_EULER_INT"  period=".1"/>
+<!--      <message name="AHRS_RMAT_INT"   period=".5"/> -->
     </mode>
 
     <mode name="rate_loop">
@@ -114,7 +114,7 @@
       <message name="ALIVE"             period="2.1"/>
       <!--<message name="BOOZ2_SONAR"       period="0.1"/>-->
       <!--<message name="BOOZ2_TUNE_HOVER"               period=".1"/>-->
-      <!-- <message name="BOOZ2_GPS"               period=".20"/> -->
+      <!-- <message name="GPS_INT"               period=".20"/> -->
       <!--<message name="INS2"              period=".05"/>
       <message name="INS3"              period=".20"/>-->
       <message name="INS_REF"           period="5.1"/>
diff --git a/conf/units.xml b/conf/units.xml
new file mode 100644
index 0000000000..022537d529
--- /dev/null
+++ b/conf/units.xml
@@ -0,0 +1,18 @@
+<!-- Table of default units convertion -->
+<!-- used to convert from unit to alt_unit (messages) or code_unit (airframe) -->
+<units>
+  <unit from="deg" to="rad" coef="0.0174532925"/>
+  <unit from="rad" to="deg" coef="57.2957795131"/>
+  <unit from="deg/s" to="rad/s" coef="0.0174532925"/>
+  <unit from="rad/s" to="deg/s" coef="57.2957795131"/>
+  <unit from="m" to="cm" coef="100."/>
+  <unit from="cm" to="m" coef="0.01"/>
+  <unit from="m/s" to="cm/s" coef="100."/>
+  <unit from="cm/s" to="m/s" coef="0.01"/>
+  <unit from="m" to="mm" coef="1000."/>
+  <unit from="mm" to="m" coef="0.001"/>
+  <unit from="m/s" to="mm/s" coef="1000."/>
+  <unit from="mm/s" to="m/s" coef="0.001"/>
+  <unit from="decideg" to="deg" coef="0.1"/>
+  <unit from="deg" to="decideg" coef="10."/>
+</units>
diff --git a/data/pictures/penguin_logo.svg b/data/pictures/penguin_logo.svg
index 9ae8538987..8c9a98c24a 100644
--- a/data/pictures/penguin_logo.svg
+++ b/data/pictures/penguin_logo.svg
@@ -1,23 +1,24 @@
 <?xml version="1.0" encoding="UTF-8" standalone="no"?>
 <!-- Created with Inkscape (http://www.inkscape.org/) -->
+
 <svg
    xmlns:dc="http://purl.org/dc/elements/1.1/"
-   xmlns:cc="http://web.resource.org/cc/"
+   xmlns:cc="http://creativecommons.org/ns#"
    xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
    xmlns:svg="http://www.w3.org/2000/svg"
    xmlns="http://www.w3.org/2000/svg"
-   xmlns:sodipodi="http://inkscape.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
    xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
    inkscape:export-ydpi="72.000000"
    inkscape:export-xdpi="72.000000"
    inkscape:export-filename="/users/brisset/pprz/savannah/paparazzi3/data/pictures/penguin_logo.png"
-   width="210mm"
-   height="297mm"
+   width="360"
+   height="330"
    id="svg2"
    sodipodi:version="0.32"
-   inkscape:version="0.41.1"
-   sodipodi:docbase="/users/brisset/pprz/savannah/paparazzi3/data/pictures"
-   sodipodi:docname="penguin_logo.svg">
+   inkscape:version="0.48.2 r9819"
+   sodipodi:docname="penguin_logo.svg"
+   version="1.1">
   <defs
      id="defs3" />
   <sodipodi:namedview
@@ -28,25 +29,27 @@
      inkscape:pageopacity="0.0"
      inkscape:pageshadow="2"
      inkscape:zoom="1.7362501"
-     inkscape:cx="264.88757"
-     inkscape:cy="910.40847"
+     inkscape:cx="205.69428"
+     inkscape:cy="184.81534"
      inkscape:document-units="px"
      inkscape:current-layer="layer1"
-     inkscape:window-width="1440"
-     inkscape:window-height="825"
-     inkscape:window-x="-27"
-     inkscape:window-y="154" />
+     inkscape:window-width="1680"
+     inkscape:window-height="1001"
+     inkscape:window-x="0"
+     inkscape:window-y="25"
+     showgrid="false"
+     fit-margin-top="10"
+     fit-margin-left="10"
+     fit-margin-right="10"
+     fit-margin-bottom="10"
+     inkscape:window-maximized="1" />
   <metadata
      id="metadata4">
-    <rdf:RDF
-       id="RDF5">
+    <rdf:RDF>
       <cc:Work
-         rdf:about=""
-         id="Work6">
-        <dc:format
-           id="format7">image/svg+xml</dc:format>
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
         <dc:type
-           id="type9"
            rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
       </cc:Work>
     </rdf:RDF>
@@ -54,29 +57,34 @@
   <g
      inkscape:label="Layer 1"
      inkscape:groupmode="layer"
-     id="layer1">
+     id="layer1"
+     transform="translate(6.1774787,3.2309135)">
     <path
        style="fill:#aaa9ad"
-       d="M 437.14485,243.16035 C 435.21985,242.97738 430.38235,242.31074 426.39485,241.67892 L 419.14485,240.53017 L 419.14485,237.19187 C 419.14485,235.35580 419.83924,232.11811 420.68794,229.99701 L 422.23102,226.14045 L 430.43794,226.45396 C 434.95174,226.62639 441.51457,226.30996 445.02201,225.75078 C 448.52945,225.19159 451.62684,224.96174 451.90509,225.24000 C 452.18335,225.51826 451.00763,227.34428 449.29238,229.29784 C 445.79032,233.28647 444.71581,236.43649 445.36004,240.82589 C 445.83678,244.07412 446.06784,244.00846 437.14485,243.16035 z M 412.39485,239.22952 C 408.58056,236.22341 408.26513,233.45781 411.10225,227.89659 C 413.22052,223.74444 414.71208,222.81309 419.24348,222.81309 L 422.17854,222.81309 L 419.57238,227.95611 C 417.65275,231.74432 417.05750,234.24688 417.31274,237.45611 C 417.71423,242.50417 416.91538,242.79224 412.39485,239.22952 z M 390.14485,235.67916 C 386.56985,235.19464 379.15057,234.01174 373.65755,233.05049 C 363.46386,231.26666 361.90218,230.24602 364.11365,226.81309 C 364.93370,225.54010 368.37036,225.35738 386.82174,225.60577 C 398.77973,225.76674 408.60917,225.99174 408.66495,226.10577 C 408.72073,226.21980 408.58309,228.67559 408.35909,231.56309 L 407.95182,236.81309 L 402.29834,236.68659 C 399.18892,236.61702 393.71985,236.16367 390.14485,235.67916 z "
+       d="m 303.52355,242.5844 c -1.925,-0.18297 -6.7625,-0.84961 -10.75,-1.48143 l -7.25,-1.14875 0,-3.3383 c 0,-1.83607 0.69439,-5.07376 1.54309,-7.19486 l 1.54308,-3.85656 8.20692,0.31351 c 4.5138,0.17243 11.07663,-0.144 14.58407,-0.70318 3.50744,-0.55919 6.60483,-0.78904 6.88308,-0.51078 0.27826,0.27826 -0.89746,2.10428 -2.61271,4.05784 -3.50206,3.98863 -4.57657,7.13865 -3.93234,11.52805 0.47674,3.24823 0.7078,3.18257 -8.21519,2.33446 z m -24.75,-3.93083 c -3.81429,-3.00611 -4.12972,-5.77171 -1.2926,-11.33293 2.11827,-4.15215 3.60983,-5.0835 8.14123,-5.0835 l 2.93506,0 -2.60616,5.14302 c -1.91963,3.78821 -2.51488,6.29077 -2.25964,9.5 0.40149,5.04806 -0.39736,5.33613 -4.91789,1.77341 z m -22.25,-3.55036 c -3.575,-0.48452 -10.99428,-1.66742 -16.4873,-2.62867 -10.19369,-1.78383 -11.75537,-2.80447 -9.5439,-6.2374 0.82005,-1.27299 4.25671,-1.45571 22.70809,-1.20732 11.95799,0.16097 21.78743,0.38597 21.84321,0.5 0.0558,0.11403 -0.0819,2.56982 -0.30586,5.45732 l -0.40727,5.25 -5.65348,-0.1265 c -3.10942,-0.0696 -8.57849,-0.52292 -12.15349,-1.00743 z"
        id="path1468"
-       sodipodi:nodetypes="cccccccccccccccccccccccccccc" />
+       sodipodi:nodetypes="cccccccccccccccccccccccccccc"
+       inkscape:connector-curvature="0" />
     <path
        style="fill:#f3b80d"
-       d="M 256.80701,16.258778 C 252.47819,18.502433 245.65535,12.190020 243.37500,17.312500 C 243.88306,21.560959 251.16252,23.337657 254.54520,26.037661 C 264.11433,31.727853 274.62503,36.325674 281.02181,45.886752 C 286.02120,50.071557 287.85093,60.509513 295.62500,59.718750 C 301.97708,54.374008 310.26224,54.867559 318.03959,54.037169 C 323.40537,54.664365 321.69949,49.164300 318.59156,45.848139 C 307.43158,30.297118 288.75678,22.294486 270.70955,18.136537 C 266.73760,16.774109 264.17692,20.259718 261.16722,16.763167 L 258.90515,16.766216 L 256.80701,16.258778 z M 232.28125,23.312500 C 223.31116,26.367029 223.19976,38.041222 221.64348,45.907300 C 220.62255,54.666112 218.34302,65.434226 225.21875,72.437500 C 234.40952,65.599028 246.12365,60.139827 257.71875,63.406250 C 263.40014,63.543870 260.60897,55.347376 258.85638,52.189302 C 252.47436,41.448479 246.55966,28.216264 234.34375,23.218750 C 233.67610,23.349191 232.93261,23.062034 232.28125,23.312500 z M 217.29990,24.212167 C 203.07822,29.662758 189.17843,43.035238 179.79012,54.730005 C 175.80364,62.380125 171.80702,67.182118 168.58044,78.177259 C 167.55327,84.770838 164.30612,92.508010 168.90625,98.343750 C 175.07570,95.630183 174.82768,87.801195 181.01126,84.887638 C 185.01143,81.988261 190.10440,82.985074 194.37500,81.468750 C 198.56978,71.737355 197.54874,59.267824 204.18750,50.875000 C 207.92491,43.315934 214.49866,37.069563 217.71993,29.356220 C 219.17968,26.484630 219.78061,23.594264 217.29990,24.212167 z M 343.37500,243.53125 C 337.83243,243.89737 334.57686,251.59817 340.34375,254.34375 C 342.76039,254.07508 334.57556,259.29293 339.59375,262.93750 C 345.65414,263.97094 350.78190,267.62887 355.87500,270.53125 C 360.15913,269.90933 360.86979,266.22505 365.15625,269.43750 C 372.67592,269.02116 369.55600,258.90836 365.74298,255.53268 C 359.95366,249.71672 352.12169,242.93985 343.37500,243.53125 z M 184.25000,244.46875 C 177.31321,246.56764 171.20482,251.16362 164.78125,253.87500 C 159.67029,258.44770 152.27302,260.55425 145.98466,259.64643 C 142.44677,259.65985 135.92554,262.29021 138.39515,267.02015 C 137.89537,271.15697 139.66354,273.95704 142.46319,276.25397 C 141.80927,281.06777 145.93865,282.61260 148.94577,284.99780 C 154.42848,283.97819 150.82753,286.06937 152.34375,291.90625 C 156.83613,295.25881 159.54332,292.08699 161.12500,297.43750 C 165.93276,299.36120 164.30382,296.06047 171.75393,294.29663 C 177.03179,291.83966 178.98679,285.75740 185.03227,282.96977 C 187.39857,279.21344 189.28475,277.85761 191.03671,276.02226 C 191.70030,271.20227 193.49688,268.35492 196.50000,264.12500 C 200.45418,255.06935 194.92114,243.72729 184.25000,244.46875 z M 219.29905,279.82595 C 212.77822,279.89551 216.16064,286.09216 218.31369,289.88726 C 221.00311,295.23401 224.67760,303.11732 231.53125,302.34375 C 234.28520,298.34149 238.93204,298.02614 242.86083,296.73344 C 244.83479,292.86794 252.08603,288.03156 247.06250,284.50000 C 236.32431,283.09233 227.62904,283.66436 219.29905,279.82595 z "
+       d="m 123.18571,15.682824 c -4.32882,2.243655 -11.15166,-4.068758 -13.43201,1.053722 0.50806,4.248459 7.78752,6.025157 11.1702,8.725161 9.56913,5.690192 20.07983,10.288013 26.47661,19.849091 4.99939,4.184805 6.82912,14.622761 14.60319,13.831998 6.35208,-5.344742 14.63724,-4.851191 22.41459,-5.681581 5.36578,0.627196 3.6599,-4.872869 0.55197,-8.18903 -11.15998,-15.551021 -29.83478,-23.553653 -47.88201,-27.711602 -3.97195,-1.362428 -6.53263,2.123181 -9.54233,-1.37337 l -2.26207,0.003 -2.09814,-0.507438 z m -24.525763,7.053722 c -8.97009,3.054529 -9.08149,14.728722 -10.63777,22.5948 -1.02093,8.758812 -3.30046,19.526926 3.57527,26.5302 9.190773,-6.838472 20.904903,-12.297673 32.500003,-9.03125 5.68139,0.13762 2.89022,-8.058874 1.13763,-11.216948 -6.38202,-10.740823 -12.29672,-23.973038 -24.51263,-28.970552 -0.66765,0.130441 -1.411143,-0.156716 -2.062503,0.09375 z m -14.98135,0.899667 c -14.22168,5.450591 -28.12147,18.823071 -37.50978,30.517838 -3.98648,7.65012 -7.9831,12.452113 -11.20968,23.447254 -1.02717,6.593579 -4.27432,14.330751 0.32581,20.166491 6.16945,-2.713567 5.92143,-10.542555 12.10501,-13.456112 4.00017,-2.899377 9.09314,-1.902564 13.36374,-3.418888 4.19478,-9.731395 3.17374,-22.200926 9.8125,-30.59375 3.73741,-7.559066 10.31116,-13.805437 13.53243,-21.51878 1.45975,-2.87159 2.06068,-5.761956 -0.42003,-5.144053 z M 209.7537,242.9553 c -5.54257,0.36612 -8.79814,8.06692 -3.03125,10.8125 2.41664,-0.26867 -5.76819,4.94918 -0.75,8.59375 6.06039,1.03344 11.18815,4.69137 16.28125,7.59375 4.28413,-0.62192 4.99479,-4.3062 9.28125,-1.09375 7.51967,-0.41634 4.39975,-10.52914 0.58673,-13.90482 -5.78932,-5.81596 -13.62129,-12.59283 -22.36798,-12.00143 z m -159.125003,0.9375 c -6.93679,2.09889 -13.04518,6.69487 -19.46875,9.40625 -5.11096,4.5727 -12.50823,6.67925 -18.79659,5.77143 -3.5378896,0.0134 -10.0591196,2.64378 -7.5895096,7.37372 -0.49978,4.13682 1.26839,6.93689 4.06804,9.23382 -0.65392,4.8138 3.4754596,6.35863 6.4825796,8.74383 5.48271,-1.01961 1.88176,1.07157 3.39798,6.90845 4.49238,3.35256 7.19957,0.18074 8.78125,5.53125 4.80776,1.9237 3.17882,-1.37703 10.62893,-3.14087 5.27786,-2.45697 7.23286,-8.53923 13.27834,-11.32686 2.3663,-3.75633 4.25248,-5.11216 6.00444,-6.94751 0.66359,-4.81999 2.46017,-7.66734 5.46329,-11.89726 3.95418,-9.05565 -1.57886,-20.39771 -12.25,-19.65625 z m 35.04905,35.3572 c -6.52083,0.0696 -3.13841,6.26621 -0.98536,10.06131 2.68942,5.34675 6.36391,13.23006 13.21756,12.45649 2.753953,-4.00226 7.400793,-4.31761 11.329583,-5.61031 1.97396,-3.8655 9.2252,-8.70188 4.20167,-12.23344 -10.73819,-1.40767 -19.433463,-0.83564 -27.763453,-4.67405 z"
        id="path1466"
-       sodipodi:nodetypes="ccccccccccccccccccccccccccccccccccccccccccccccccccccc" />
+       sodipodi:nodetypes="ccccccccccccccccccccccccccccccccccccccccccccccccccccc"
+       inkscape:connector-curvature="0" />
     <path
        style="fill:#f10c08"
-       d="M 199.77686,114.06689 C 190.44455,115.02176 189.79962,128.69925 195.34156,134.19189 C 199.02779,134.35297 197.81108,124.03730 203.96875,126.37281 C 211.35525,125.79389 222.23373,127.08602 227.53344,132.80811 C 231.59017,132.77878 234.11211,139.35454 238.87405,137.87401 C 243.14940,137.69281 234.59303,130.17785 232.72456,127.86110 C 224.82267,119.89427 216.40031,111.97757 204.14471,114.33039 C 203.42158,114.38912 200.49999,114.00816 199.77686,114.06689 z M 306.50000,123.18750 C 310.32934,126.66679 314.40141,132.26354 309.50000,136.71875 C 301.90337,144.99026 290.34775,146.93009 280.84375,151.87500 C 275.01359,155.56376 283.20366,157.27562 286.92678,156.62784 C 299.94353,157.14641 314.78101,156.70788 323.07225,145.29380 C 329.51610,139.66342 331.27406,128.68580 324.18421,122.78674 C 317.44580,120.34045 316.23782,121.71629 309.15353,121.03783 C 310.17436,122.19408 305.47917,122.03125 306.50000,123.18750 z M 243.40625,149.46875 C 237.74815,150.65099 236.41684,160.01385 241.00000,162.71875 C 233.79055,168.70673 231.17138,178.38001 225.74555,185.88019 C 222.60256,189.41411 220.93842,193.72140 218.81250,197.87500 C 214.45868,205.29761 213.08610,213.95951 211.02501,222.26048 C 209.16745,232.98825 210.63440,244.38121 214.75000,254.31250 C 214.86473,256.99024 216.93556,260.30136 217.37510,263.98948 C 222.53347,272.82397 228.15110,280.63584 237.75000,284.21875 C 244.50922,283.23760 251.63528,284.08103 258.15625,281.90625 C 258.25676,275.78595 249.80129,274.81951 247.06250,269.81250 C 241.10964,262.93197 237.38686,254.60783 236.47165,245.51575 C 234.04233,228.30863 233.24865,210.13395 239.00149,193.58552 C 241.38646,184.10165 247.29524,175.02909 247.56250,165.21875 C 252.28415,167.32221 255.72380,159.86851 256.12500,155.96875 C 255.96218,149.81044 248.09118,148.72781 243.40625,149.46875 z M 150.68750,158.09375 C 148.80081,162.71644 151.97544,166.17869 155.18750,169.65625 C 157.06734,172.36740 148.19782,173.92757 146.67640,177.56199 C 142.03592,182.41332 148.73360,183.98375 153.16633,183.99043 C 165.26846,184.18545 176.21393,186.79924 186.29214,178.55011 C 190.11997,177.69248 192.88451,176.48294 196.53296,173.67469 C 202.50773,170.70369 208.99339,168.08678 215.03935,166.16731 C 220.15585,161.81352 214.51567,160.04265 210.03125,160.62500 C 196.06407,160.13579 182.97412,171.61476 169.20918,167.84324 C 162.88604,165.54755 156.08636,163.02035 152.12597,156.54974 C 149.94244,153.57353 151.47249,157.06917 150.68750,158.09375 z M 334.78125,158.46875 C 322.27109,164.44700 309.41087,173.06017 294.34375,171.37500 C 287.88581,170.89832 281.55542,169.69902 275.37500,167.87500 C 271.23390,172.59325 279.32053,175.80207 282.43750,178.21875 C 294.13899,185.50900 308.82874,185.58719 321.81699,181.60620 C 328.64623,179.63125 337.01505,179.91941 342.43750,175.43750 C 343.90792,170.54500 331.85988,174.71568 335.21065,168.04738 C 335.42021,163.20530 344.56242,156.44383 335.25000,158.40625 L 335.00000,158.40625 L 334.78125,158.46875 z M 261.28125,175.21875 C 255.78104,177.32738 255.78606,182.99154 257.85603,187.62124 C 260.17005,192.52283 257.98895,201.67972 265.67496,201.34859 C 269.51764,202.38147 275.88484,201.71430 273.40625,196.28125 C 270.45214,188.61556 266.80723,181.27242 263.18750,173.90625 C 262.55208,174.34375 261.91667,174.78125 261.28125,175.21875 z "
+       d="m 66.155557,113.49094 c -9.33231,0.95487 -9.97724,14.63236 -4.4353,20.125 3.68623,0.16108 2.46952,-10.15459 8.62719,-7.81908 7.3865,-0.57892 18.26498,0.71321 23.56469,6.4353 4.05673,-0.0293 6.578673,6.54643 11.340613,5.0659 4.27535,-0.1812 -4.28102,-7.69616 -6.149493,-10.01291 -7.90189,-7.96683 -16.32425,-15.88353 -28.57985,-13.53071 -0.72313,0.0587 -3.64472,-0.32223 -4.36785,-0.2635 z m 106.723143,9.12061 c 3.82934,3.47929 7.90141,9.07604 3,13.53125 -7.59663,8.27151 -19.15225,10.21134 -28.65625,15.15625 -5.83016,3.68876 2.35991,5.40062 6.08303,4.75284 13.01675,0.51857 27.85423,0.08 36.14547,-11.33404 6.44385,-5.63038 8.20181,-16.608 1.11196,-22.50706 -6.73841,-2.44629 -7.94639,-1.07045 -15.03068,-1.74891 1.02083,1.15625 -3.67436,0.99342 -2.65353,2.14967 z m -63.09375,26.28125 c -5.6581,1.18224 -6.98941,10.5451 -2.40625,13.25 -7.20945,5.98798 -9.828623,15.66126 -15.254453,23.16144 -3.14299,3.53392 -4.80713,7.84121 -6.93305,11.99481 -4.35382,7.42261 -5.7264,16.08451 -7.78749,24.38548 -1.85756,10.72777 -0.39061,22.12073 3.72499,32.05202 0.11473,2.67774 2.18556,5.98886 2.6251,9.67698 5.15837,8.83449 10.776,16.64636 20.374903,20.22927 6.75922,-0.98115 13.88528,-0.13772 20.40625,-2.3125 0.10051,-6.1203 -8.35496,-7.08674 -11.09375,-12.09375 -5.95286,-6.88053 -9.67564,-15.20467 -10.59085,-24.29675 -2.42932,-17.20712 -3.223003,-35.3818 2.52984,-51.93023 2.38497,-9.48387 8.29375,-18.55643 8.56101,-28.36677 4.72165,2.10346 8.1613,-5.35024 8.5625,-9.25 -0.16282,-6.15831 -8.03382,-7.24094 -12.71875,-6.5 z m -92.718753,8.625 c -1.88669,4.62269 1.28794,8.08494 4.5,11.5625 1.87984,2.71115 -6.98968,4.27132 -8.5111,7.90574 -4.6404796,4.85133 2.0572,6.42176 6.48993,6.42844 12.10213,0.19502 23.0476,2.80881 33.12581,-5.44032 3.82783,-0.85763 6.59237,-2.06717 10.24082,-4.87542 5.97477,-2.971 12.46043,-5.58791 18.50639,-7.50738 5.1165,-4.35379 -0.52368,-6.12466 -5.0081,-5.54231 -13.96718,-0.48921 -27.05713,10.98976 -40.82207,7.21824 -6.32314,-2.29569 -13.12282,-4.82289 -17.08321,-11.2935 -2.18353,-2.97621 -0.65348,0.51943 -1.43847,1.54401 z m 184.093753,0.375 c -12.51016,5.97825 -25.37038,14.59142 -40.4375,12.90625 -6.45794,-0.47668 -12.78833,-1.67598 -18.96875,-3.5 -4.1411,4.71825 3.94553,7.92707 7.0625,10.34375 11.70149,7.29025 26.39124,7.36844 39.37949,3.38745 6.82924,-1.97495 15.19806,-1.68679 20.62051,-6.1687 1.47042,-4.8925 -10.57762,-0.72182 -7.22685,-7.39012 0.20956,-4.84208 9.35177,-11.60355 0.0394,-9.64113 l -0.25,0 -0.21875,0.0625 z m -73.5,16.75 c -5.50021,2.10863 -5.49519,7.77279 -3.42522,12.40249 2.31402,4.90159 0.13292,14.05848 7.81893,13.72735 3.84268,1.03288 10.20988,0.36571 7.73129,-5.06734 -2.95411,-7.66569 -6.59902,-15.00883 -10.21875,-22.375 -0.63542,0.4375 -1.27083,0.875 -1.90625,1.3125 z"
        id="path1464"
-       sodipodi:nodetypes="cccccccccccccccccccccccccccccccccccccccccccccccccccccccccc" />
+       sodipodi:nodetypes="cccccccccccccccccccccccccccccccccccccccccccccccccccccccccc"
+       inkscape:connector-curvature="0" />
     <path
        inkscape:export-ydpi="72.000000"
        inkscape:export-xdpi="72.000000"
        inkscape:export-filename="/users/brisset/pprz/savannah/paparazzi3/data/pictures/penguin_logo.png"
        style="fill:#090903"
-       d="M 305.53492,316.37315 C 293.51334,313.35535 280.84972,303.35231 275.18380,292.39872 C 273.28023,288.71865 271.06735,285.16891 270.26627,284.51039 C 269.33408,283.74409 264.91298,283.31309 257.98461,283.31309 C 247.98930,283.31309 244.90973,284.01817 248.64484,285.45146 C 251.79360,286.65976 249.57428,293.81309 246.05064,293.81309 C 244.93753,293.81309 244.28250,294.22681 244.59501,294.73247 C 245.66205,296.45898 241.93905,299.00988 238.98498,298.57632 C 236.84136,298.26170 235.77491,298.72874 234.62318,300.48649 C 233.52875,302.15680 232.15226,303.74582 229.74340,303.74582 C 226.87079,303.74582 225.81202,302.05778 222.38417,297.56309 C 220.18202,294.67559 217.31480,289.52556 216.01256,286.11857 C 214.17527,281.31176 213.08480,279.78234 211.14485,279.29146 C 209.76985,278.94353 207.06985,277.98102 205.14485,277.15255 C 203.21985,276.32408 200.13500,274.99348 198.28964,274.19567 C 196.44427,273.39786 194.08177,273.01572 193.03963,273.34648 C 190.36669,274.19484 190.57712,271.44863 193.64904,265.39361 C 195.02173,262.68790 196.14485,259.38091 196.14485,258.04474 C 196.14485,253.69447 193.02522,248.66105 189.38677,247.14081 C 185.27804,245.42408 181.68410,246.35828 174.76435,250.94173 C 172.23589,252.61651 169.57068,254.07433 168.84165,254.18133 C 168.11262,254.28833 166.22567,255.39095 164.64843,256.63161 C 163.07120,257.87226 161.16428,258.65079 160.41085,258.36167 C 158.41378,258.84983 158.33779,259.58124 156.29666,260.25488 C 153.55935,261.15827 154.50715,261.02630 152.36256,260.96622 C 150.55915,261.15238 146.97498,261.28822 144.94765,261.86965 C 141.34456,262.90300 139.57710,262.20211 141.11453,260.34961 C 141.78599,259.54056 144.49196,259.08593 148.41631,259.12285 C 153.77377,259.17325 155.67865,258.64164 161.95106,255.34558 L 169.25084,251.50965 L 168.61124,246.39046 C 168.10123,242.30846 168.49927,239.75594 170.57583,233.79218 C 176.11819,217.87484 186.15453,207.92898 202.01886,202.63265 C 206.75074,201.05290 212.27578,199.49598 214.29672,199.17281 C 216.40114,198.83630 218.21694,197.88618 218.54636,196.94917 C 218.86271,196.04933 220.01311,193.96309 221.10279,192.31309 C 222.19247,190.66309 223.72671,188.18809 224.51222,186.81309 C 225.29773,185.43809 227.57331,181.88769 229.56907,178.92332 L 232.90973,172.95759 L 229.56083,171.06221 C 223.69241,167.07452 220.28082,166.56405 211.47419,168.35594 C 206.00446,169.46887 201.54092,171.20937 196.66386,174.13103 C 192.82432,176.43116 185.55734,180.93803 182.39484,182.49047 C 176.98181,185.14768 175.88300,185.31309 163.64484,185.31309 C 143.63831,185.31309 139.34459,182.53747 147.78869,175.06309 C 150.42946,172.72559 153.16491,170.81309 153.86747,170.81309 C 155.82101,170.81309 155.27090,169.09105 153.02826,167.89082 C 151.92826,167.30212 149.67212,163.28679 150.60485,163.08821 C 151.16304,163.18262 151.87313,163.84764 153.82444,165.89436 C 155.69284,167.85412 159.78278,169.74046 159.80779,170.27092 C 159.82517,170.63972 158.90735,171.92902 157.39485,172.26493 C 153.80642,173.06188 145.71269,177.96826 145.40480,179.53323 C 144.88226,182.18926 149.67255,183.29800 162.40019,183.46693 C 174.56306,183.62836 175.42976,183.50339 181.05915,180.77647 C 184.30613,179.20361 187.25529,177.44341 187.61282,176.86491 C 187.97036,176.28641 189.36132,175.81309 190.70387,175.81309 C 192.12332,175.81309 202.19707,168.12207 205.79807,167.52269 C 206.81380,167.35362 208.65735,166.65897 209.89485,165.97901 C 211.13235,165.29905 212.87958,165.44821 213.77758,164.70294 C 215.25880,163.47363 215.20177,163.29866 213.16305,162.81739 C 209.07959,161.85341 198.76681,163.55816 190.64485,166.53974 C 186.24484,168.15499 180.58026,169.58178 178.05688,169.71039 C 171.52184,170.04345 161.45883,166.49023 156.64485,162.14985 L 152.64485,158.54337 L 151.66774,161.76119 C 151.57313,166.36413 149.98939,164.09249 149.98939,158.89082 C 149.98939,156.14082 150.73884,155.10899 151.28884,155.10899 C 151.83884,155.10899 152.76667,155.33310 152.76667,155.86088 C 154.16577,158.53932 161.66940,165.12059 166.14484,166.27441 C 175.70764,168.73979 177.51615,168.68080 186.98930,165.59457 C 191.57874,164.09939 195.62875,162.93456 195.98930,163.00605 C 196.34985,163.07755 197.99485,162.55201 199.64485,161.83818 C 201.29485,161.12434 205.88780,160.48918 209.85141,160.42669 L 217.05797,160.31309 L 218.60141,156.27966 C 219.45030,154.06127 220.14485,151.49929 220.14485,150.58636 C 220.14485,148.67223 223.80684,145.81309 226.25846,145.81309 C 227.17620,145.81309 230.07107,147.58479 232.69151,149.75019 L 237.45595,153.68730 L 239.37297,151.25019 C 240.42734,149.90979 242.15734,148.81309 243.21742,148.81309 C 244.27750,148.81309 245.14484,148.41775 245.14484,147.93456 C 245.14484,146.60652 235.53190,138.71848 227.64484,133.57467 C 219.77107,128.43954 214.97414,126.82679 207.54984,126.81867 C 200.43570,126.81088 198.63278,128.89527 199.57965,136.03315 C 199.96419,138.93192 200.99341,142.09326 201.86682,143.05836 C 202.74022,144.02346 212.88730,152.71034 215.48556,153.36247 C 218.46321,154.10981 221.58447,156.28179 218.88214,156.27518 C 215.14978,156.26605 199.67536,145.19535 197.10066,140.05460 C 196.34547,138.54677 195.27870,136.70519 194.73005,135.96221 C 193.64114,134.48758 193.50702,134.55348 192.04280,127.52749 C 191.30290,123.97716 191.49162,122.05637 192.37270,120.47253 C 192.81355,119.06499 193.31512,118.08140 193.60941,117.43179 C 193.90371,116.83716 195.21283,115.70958 197.12925,114.68179 C 204.54399,110.70522 216.51320,113.18735 224.97011,120.45532 C 233.13699,127.47402 243.01780,138.01923 246.18868,143.10067 C 249.60299,148.57221 252.76605,151.52521 253.94727,150.34399 C 255.60779,148.68348 255.16390,136.70415 253.11240,127.81309 C 251.90680,122.58809 250.12517,114.48809 249.15323,109.81309 C 246.19147,95.567214 241.94580,79.826934 238.85864,71.647194 C 236.73162,66.011434 236.70621,65.988554 234.08099,67.346114 C 232.62939,68.096754 230.00285,70.349714 228.24423,72.352674 L 225.04673,75.994424 L 221.69467,74.403754 C 217.51811,72.421844 211.91627,72.362814 208.24363,74.262004 C 204.71781,76.085284 199.14485,82.357764 199.14485,84.502874 C 199.14485,85.957434 198.96594,85.967904 197.39485,84.605344 C 192.97429,80.771514 183.12189,81.816174 178.58341,86.599944 C 175.33523,90.023664 172.14484,96.090154 172.14484,98.842804 C 172.14484,101.68630 167.77239,101.20244 167.35802,98.313094 C 166.11785,89.665344 166.19546,83.937168 167.19961,83.937168 C 167.80499,83.937168 168.17647,88.275594 168.21513,90.063094 C 168.25379,91.850594 168.63312,94.203054 169.05809,95.290794 C 169.79387,97.174064 169.89602,97.150194 171.19761,94.790794 C 171.94937,93.428054 172.47106,91.840924 172.35691,91.263834 C 172.10328,89.981564 179.32796,82.785014 181.25689,82.398514 C 182.02026,82.245554 183.89846,81.606264 185.43066,80.977884 C 186.96286,80.349494 189.66286,80.064294 191.43066,80.344094 L 193.56220,79.770164 L 194.87130,75.082954 C 194.99585,71.909534 195.79416,66.838094 196.64533,63.813094 C 197.49650,60.788094 198.31433,57.795984 198.46271,57.163974 C 198.61110,56.531954 199.16741,55.406954 199.69896,54.663974 C 200.23050,53.920984 201.51469,51.738090 202.55271,49.813090 C 203.59073,47.888090 207.44480,42.038090 211.11731,36.813090 C 220.10431,24.026974 219.94184,24.349588 216.49673,26.131126 C 214.91919,26.946902 212.73216,27.771571 211.63666,27.963723 C 210.54117,28.155875 209.41985,28.553649 209.14485,28.847665 C 208.86985,29.141682 205.96344,31.140571 202.68616,33.289641 C 190.95291,40.983693 181.36653,51.484165 175.27632,63.313094 C 173.25330,67.242374 171.01395,71.751921 171.00009,70.332551 C 170.95686,65.906291 185.51985,42.824387 188.55553,42.806767 C 189.15465,42.803290 190.41590,41.445478 191.35830,39.789407 C 192.30070,38.133336 193.87570,36.484594 194.85830,36.125535 C 195.84090,35.766477 197.99485,34.417506 199.64485,33.127821 C 201.29485,31.838137 203.11630,30.452224 203.69251,30.048015 C 205.43871,28.823075 215.40959,23.952905 219.25943,22.444516 C 223.53240,20.770343 224.80896,17.355418 223.09885,12.173702 C 221.70439,7.9484475 223.07254,5.8130885 227.17417,5.8130885 C 229.29534,5.8130885 230.96852,7.0193205 234.19833,10.876983 L 238.43807,15.940876 L 244.77379,15.262378 C 251.16402,14.578042 265.48713,15.990127 265.48713,17.326716 C 265.48713,17.724614 254.65734,17.188172 250.39484,16.989630 C 245.28466,16.751604 242.98546,16.965240 243.64484,17.616826 C 244.19484,18.160326 250.71984,21.823347 258.14484,25.756873 C 265.56984,29.690399 272.76984,33.916860 274.14484,35.149009 C 277.69192,38.327576 288.99320,52.264990 289.34809,53.898554 C 289.51130,54.649844 290.62922,56.073504 291.83236,57.062244 C 293.96468,58.814594 294.12870,58.783844 298.33236,55.843714 C 302.35514,53.030094 303.29689,52.817114 312.35138,52.673334 C 321.33912,52.530604 321.98708,52.386834 321.10138,50.731881 C 319.87734,48.444753 319.88359,47.533597 321.11808,48.296551 C 321.65336,48.627374 322.82964,50.453932 323.73202,52.355568 C 324.63441,54.257204 325.77145,55.813094 326.25878,55.813094 C 327.70933,55.813094 327.26339,58.830374 325.72563,59.420464 C 324.94506,59.719994 323.05223,59.143374 321.51934,58.139074 C 317.14691,55.274434 305.94656,55.846824 299.85792,59.246074 L 295.15892,61.869494 L 290.89177,59.925584 C 287.41005,58.339474 285.75470,58.144184 281.89843,58.864614 C 275.70075,60.022464 271.18556,62.371714 268.32236,65.928234 C 267.04500,67.514904 265.60295,68.813094 265.11782,68.813094 C 263.71867,68.813094 259.05265,63.771264 259.61289,62.864794 C 260.53583,61.371434 257.31101,52.623354 253.55197,46.423124 C 244.53551,31.551240 236.67302,23.961742 231.60134,25.234652 C 227.71598,26.209814 225.24377,32.895851 223.42809,47.338954 C 222.10806,57.839434 222.07183,60.609214 223.18867,65.643424 L 224.50462,71.575204 L 229.27826,67.801864 C 234.65262,63.553674 247.05302,59.046484 248.21110,60.920304 C 248.70879,61.725584 249.01587,61.700014 249.30617,60.829104 C 249.61705,59.896464 250.67720,59.879354 253.92722,60.754494 C 259.05671,62.135724 265.44998,66.358079 260.47807,65.551249 L 251.00073,62.528024 L 251.64119,66.420554 C 251.99345,68.561454 253.35698,74.138094 254.67127,78.813094 C 255.98555,83.488094 258.70318,95.188094 260.71045,104.81309 C 265.73215,128.89264 268.65703,141.03309 270.07477,143.68217 C 271.24159,145.86238 271.49384,145.70666 279.08666,138.11913 C 285.33184,131.87829 288.87785,129.31192 296.77148,125.32000 C 305.29222,121.01096 307.60542,120.27840 313.65654,119.97274 C 319.93317,119.65569 321.10572,119.90499 324.84145,122.35084 C 328.18268,124.53841 329.14412,125.84933 329.66398,128.92638 C 330.02110,131.04017 330.00119,134.24191 329.61972,136.04136 C 328.82443,139.79296 324.21415,146.80806 320.03166,149.24314 C 318.32213,151.79253 314.16910,154.14642 312.42566,154.14642 C 308.87038,156.58999 296.55935,158.00360 284.09678,157.31042 L 271.23764,156.59517 L 270.58339,159.20190 C 269.57349,163.22568 270.66510,164.70411 276.67891,167.45732 C 281.66778,169.74130 283.60043,170.01811 295.53175,170.15763 C 304.84689,170.26656 309.27528,169.94156 310.32737,169.07177 C 311.15319,168.38904 331.04708,158.73876 334.11598,158.51745 C 335.67468,158.40505 337.79151,157.20006 339.68085,157.24017 C 344.44765,157.34136 344.83641,159.47250 340.54948,162.00186 C 336.49024,164.39687 334.18723,168.81850 336.32284,170.11674 C 337.04994,170.55875 340.00734,172.25535 342.89484,173.88697 C 350.48527,178.17604 349.82495,179.34068 339.89520,179.17755 C 335.35750,179.10301 331.29666,179.57678 330.74666,179.98149 C 330.19666,180.38621 326.71984,181.06169 323.14484,181.73144 C 319.56984,182.40119 314.75807,183.36855 312.45202,183.88113 C 299.61899,186.73360 284.44293,182.00088 272.28478,171.35477 L 268.90629,168.39646 L 265.92471,171.73343 C 264.28484,173.56876 263.37619,175.35000 263.90549,175.69174 C 264.81049,176.27606 267.18161,181.03914 272.04992,192.05219 C 273.20213,194.65870 274.14484,197.70348 274.14484,198.81837 C 274.14484,201.11739 275.06191,201.59353 292.62301,208.41218 C 299.21100,210.97018 305.63304,213.06309 306.89421,213.06309 C 308.27437,213.06309 311.29249,211.07205 314.47532,208.06186 C 318.62886,204.13361 320.88759,202.81953 325.00333,201.93686 C 334.18942,199.96679 341.35117,200.93009 349.85826,205.27998 C 353.42643,207.10447 361.14484,215.37534 361.14484,217.37442 C 361.14484,218.07829 362.49484,218.95069 364.14484,219.31309 C 365.79484,219.67549 367.14484,220.61125 367.14484,221.39255 C 367.14484,222.96058 372.64462,223.43148 373.56501,221.94225 C 373.86103,221.46329 375.54696,220.78267 377.31153,220.42975 C 380.71040,219.74998 391.49025,209.45088 392.65979,205.76600 C 394.91028,204.80839 397.49453,208.14155 396.99161,217.12133 L 396.64484,223.31309 L 403.99717,223.12547 C 409.22254,222.99212 411.87714,222.46034 413.17340,221.28724 C 414.78839,219.82569 415.66433,219.78283 420.82107,220.91304 C 424.02414,221.61506 426.93990,222.62516 427.30054,223.15770 C 428.38629,224.76102 452.87551,223.21958 455.69791,221.37027 C 459.83716,218.65813 470.72282,220.05619 474.21661,222.74146 C 474.69608,223.20847 480.16019,226.81160 480.10940,232.99735 C 480.05506,239.61566 475.15007,246.51913 468.51182,249.32021 C 465.14675,250.74014 463.60648,250.79593 456.64484,249.75010 C 452.24484,249.08909 448.04660,248.07310 447.31543,247.49233 C 445.23346,245.83864 427.30542,243.81955 420.51466,244.47397 C 414.57890,245.04599 414.29523,244.96456 411.57044,241.90645 C 410.02272,240.16940 408.15045,238.98070 407.40984,239.26489 C 406.66923,239.54909 402.59694,239.29949 398.36031,238.71023 C 391.39012,237.74077 390.56161,237.80608 389.65109,239.39677 C 385.71620,246.27105 378.71193,251.50621 370.48600,253.72123 L 366.33840,254.83806 L 368.24162,258.77727 C 370.60394,263.66670 370.60645,265.65484 368.25417,268.71440 C 366.76844,270.64686 365.81419,270.99696 363.36686,270.50749 C 361.20270,270.07466 360.16453,270.31939 359.76668,271.35618 C 358.96893,273.43507 357.80686,273.19077 352.94497,269.92203 C 347.86086,266.50389 341.68637,263.81309 338.92701,263.81309 C 337.11479,263.81309 332.59268,265.89958 329.29731,268.25620 C 328.55617,268.78622 327.06971,269.35417 325.99407,269.51833 C 324.91843,269.68248 322.82482,270.42840 321.34160,271.17592 C 319.85838,271.92344 317.65804,272.44971 316.45195,272.34540 C 315.24586,272.24109 310.07903,275.43566 304.70797,276.08054 C 301.86951,276.42134 300.73565,281.89882 302.19804,288.20567 C 304.27819,297.17678 310.60180,309.28245 315.09336,312.89196 C 317.32125,314.68234 318.93729,316.35397 318.68456,316.60670 C 317.60322,317.68804 310.25281,317.55750 305.53492,316.37315 z M 237.94155,296.31309 C 240.35000,296.31309 241.74309,295.70964 242.52350,294.32831 C 243.14024,293.23668 244.43234,291.74866 245.39484,291.02160 C 247.45431,289.46591 247.64822,287.47575 245.88307,286.01080 C 245.18910,285.43486 241.47660,284.72483 237.63307,284.43296 C 227.48600,283.66240 221.74536,282.77693 221.15909,281.89189 C 219.99077,280.56294 217.14485,281.07233 217.14485,282.54438 C 217.14485,285.87683 221.89744,294.61832 225.60903,298.11265 L 229.64484,301.91222 L 232.06290,299.11265 C 233.84298,297.05172 235.39398,296.31309 237.94155,296.31309 z M 232.14484,281.81309 C 232.14484,281.26309 231.46984,280.81309 230.64484,280.81309 C 229.81984,280.81309 229.14484,280.36309 229.14484,279.81309 C 229.14484,279.26309 228.55812,278.81309 227.84101,278.81309 C 225.90658,278.81309 219.81215,272.16043 220.59145,270.89950 C 220.96074,270.30197 218.37897,267.11309 217.14485,263.81309 C 215.91073,260.51309 214.67148,257.81309 214.39096,257.81309 C 211.03777,257.81309 201.14485,268.69810 201.14485,272.38755 C 201.14485,273.82997 205.18836,275.46871 210.89910,276.34071 C 213.23894,276.69800 214.74508,278.51999 215.60565,278.18976 C 216.46622,277.85953 217.80960,278.11990 218.59095,278.76835 C 219.37229,279.41681 221.67772,280.15402 223.71412,280.40660 C 225.75052,280.65918 228.14301,281.29028 229.03075,281.80905 C 231.14453,283.04428 232.14484,283.04557 232.14484,281.81309 z M 248.14484,281.38463 C 252.81984,281.09579 256.75734,280.84903 256.89484,280.83628 C 257.03234,280.82352 257.14484,280.43143 257.14484,279.96496 C 257.14484,279.49850 255.27716,278.27463 252.99443,277.24527 C 248.87965,275.38976 241.76666,266.94407 241.14484,264.30663 C 238.65755,261.55593 233.14824,241.79896 233.14621,228.13545 C 233.14417,214.48414 234.71687,205.50953 240.26088,187.53555 C 243.01112,178.61909 245.34761,169.74640 245.45308,167.81845 C 245.62375,164.69854 245.37106,164.31309 243.15501,164.31309 C 241.19357,164.31309 239.98264,165.48027 237.44887,169.81309 C 231.20028,180.49837 226.63305,187.98379 225.64484,189.15918 C 225.09484,189.81336 223.52297,192.59061 222.15180,195.33084 C 220.78062,198.07108 219.15006,200.98809 218.52835,201.81309 C 217.90663,202.63809 216.21601,207.84964 214.77140,213.39431 C 211.29553,226.73541 211.28700,236.22233 214.74002,248.31309 C 216.15370,253.26309 217.65328,258.66309 218.07243,260.31309 C 219.84113,267.27569 224.20951,274.59137 228.29912,277.43960 C 234.60998,281.83483 236.17187,282.61030 237.97926,282.24575 C 238.89533,282.06098 243.46984,281.67348 248.14484,281.38463 z M 265.85386,279.81309 C 267.14053,279.81309 267.28194,278.93404 266.69842,274.56309 C 266.31293,271.67559 265.69318,263.98818 265.32119,257.47995 C 264.74443,247.38926 264.35901,245.43818 262.70371,244.22995 C 259.92681,242.20305 247.81328,242.38438 241.79111,244.54299 L 236.93737,246.28278 L 237.53565,250.64773 C 238.33644,256.49013 243.72052,266.42735 248.63863,271.14017 C 250.83522,273.24506 254.76664,275.88536 257.37513,277.00751 C 260.10387,278.18138 261.84159,279.49474 261.46729,280.10036 C 261.05714,280.76400 261.46188,280.90541 262.56258,280.48303 C 263.52279,280.11456 265.00386,279.81309 265.85386,279.81309 z M 313.39484,271.65115 C 314.90734,271.05676 316.14484,270.27757 316.14484,269.91961 C 316.14484,269.56165 314.74167,267.56649 313.02668,265.48591 C 310.63099,262.57952 309.30046,261.77372 307.28407,262.00807 C 303.94821,262.39577 301.73698,266.67965 302.67299,270.94127 C 303.30757,273.83047 303.54474,273.96348 306.99463,273.36504 C 309.00224,273.01678 311.88234,272.24553 313.39484,271.65115 z M 359.10583,268.46426 C 360.58236,266.07517 351.23898,260.09129 351.23898,259.63296 C 351.23898,259.17462 341.56896,255.14642 340.86510,255.14642 C 340.00912,255.14642 339.71148,256.22674 339.96624,258.40895 C 340.30574,261.31707 340.76317,261.74176 344.17726,262.31857 C 346.28383,262.67447 350.17438,264.50634 352.82292,266.38938 C 357.94959,270.03430 358.24618,269.85520 359.10583,268.46426 z M 366.83463,267.56309 C 367.34870,266.87559 367.69734,265.63809 367.60938,264.81309 C 367.52143,263.98809 367.38093,262.57426 367.29715,261.67124 C 367.09375,259.47866 358.33552,249.81309 356.55217,249.81309 C 355.77814,249.81309 355.14315,249.47559 355.14109,249.06309 C 355.13903,248.65059 353.05876,247.45816 350.51828,246.41324 C 345.08874,244.18003 341.37108,244.97487 339.47708,248.77386 C 337.88800,251.96123 338.41105,252.59952 343.70420,253.93235 C 346.67340,254.68001 350.55663,255.89096 353.31490,258.00041 C 355.14637,259.40107 358.44484,262.40652 360.64484,264.67919 C 365.02408,269.20308 365.45739,269.40496 366.83463,267.56309 z M 377.03558,248.87156 C 379.26385,247.67903 387.14484,239.14862 387.14484,237.92927 C 387.14484,237.41859 385.00734,236.74901 382.39484,236.44131 C 379.78234,236.13361 375.28234,235.59916 372.39484,235.25364 C 368.87836,234.83285 366.05475,235.71385 364.99113,236.37340 C 363.63220,236.37340 362.55764,238.37390 360.85703,242.06309 L 358.43693,247.31309 L 360.98926,249.92813 C 363.52020,252.52126 363.59082,252.53167 369.42130,251.17152 C 372.65515,250.41711 376.08158,249.38213 377.03558,248.87156 z M 468.46524,246.88138 C 472.04624,245.51988 477.01453,239.84091 478.14840,235.81309 C 479.50357,230.99917 478.52247,230.71116 476.72101,227.31309 L 474.30712,224.31309 L 475.40347,228.45407 C 477.46032,236.22289 472.53359,243.97016 463.64484,246.94449 C 461.74357,247.58069 461.75269,247.61865 463.82974,247.71341 C 465.03144,247.76823 467.11741,247.39382 468.46524,246.88138 z M 445.36003,240.82589 C 444.71580,236.43649 445.79031,233.28647 449.29237,229.29784 C 451.00762,227.34428 452.18334,225.51826 451.90508,225.24000 C 451.62683,224.96174 448.52944,225.19159 445.02200,225.75078 C 441.51456,226.30996 434.95173,226.62639 430.43793,226.45396 L 422.23101,226.14045 L 420.68793,229.99701 C 419.83923,232.11811 419.14484,235.35580 419.14484,237.19187 L 419.14484,240.53017 L 426.39484,241.67892 C 433.45606,242.79776 437.96183,243.32638 443.22165,243.65305 C 445.53901,243.79698 445.75431,243.51233 445.36003,240.82589 z M 468.72453,241.92762 C 475.17746,237.99263 475.89535,227.87304 469.90039,225.35235 C 467.10203,224.17573 466.43108,224.26209 463.50586,226.17545 C 460.04381,228.43994 456.14484,234.37727 456.14484,237.38479 C 456.14484,240.09401 460.15375,243.81358 463.06915,243.80935 C 464.48578,243.80730 467.03070,242.96052 468.72453,241.92762 z M 450.02176,240.63856 C 450.76867,239.44257 451.56443,236.40507 451.79011,233.88856 L 452.20044,229.31309 L 449.67264,232.11885 C 447.87040,234.11927 447.14484,236.05674 447.14484,238.86885 C 447.14484,243.30379 448.02079,243.84262 450.02176,240.63856 z M 417.31273,237.45611 C 417.05749,234.24688 417.65274,231.74432 419.57237,227.95611 L 422.17853,222.81309 L 419.24347,222.81309 C 414.71207,222.81309 413.22051,223.74444 411.10224,227.89659 C 408.26512,233.45781 408.58055,236.22341 412.39484,239.22952 C 416.91537,242.79224 417.71422,242.50417 417.31273,237.45611 z M 337.06515,238.22830 C 341.96013,236.26972 344.28067,232.89975 343.96474,228.20852 C 343.52848,221.73076 336.44432,218.53206 332.56915,223.06309 C 327.67388,228.78688 326.74855,234.41680 330.14484,237.81309 C 332.54432,240.21257 332.16314,240.18970 337.06515,238.22830 z M 335.64768,234.82204 C 335.30012,233.72696 335.05949,232.26446 335.49295,231.57204 C 336.23733,229.94689 336.29296,229.56772 336.61841,228.82945 C 338.53297,227.93902 340.07279,226.49339 341.17453,229.39117 C 342.04468,231.67985 341.85762,232.48992 340.00313,234.46394 C 337.30863,237.33209 336.46632,237.40135 335.64768,234.82204 z M 407.73726,232.18491 C 407.96126,229.29741 408.72072,226.21980 408.66494,226.10577 C 408.60916,225.99174 398.77972,225.76674 386.82173,225.60577 C 368.37035,225.35738 364.93369,225.54010 364.11364,226.81309 C 361.90217,230.24602 363.46385,231.26666 373.65754,233.05049 C 387.81848,235.52858 395.79347,236.54105 402.29833,236.68659 L 407.95181,236.81309 L 407.73726,232.18491 z M 363.25615,223.56309 C 363.95758,222.20768 363.75401,221.81309 362.35331,221.81309 C 358.67478,221.81309 356.82069,226.08894 359.38660,228.65485 C 360.37561,229.64386 360.80362,229.43014 361.48943,227.60485 C 361.96302,226.34438 362.75804,224.52559 363.25615,223.56309 z M 393.61538,220.56309 C 394.63025,216.07379 394.81763,213.27248 394.23712,211.26827 C 393.71723,209.47338 392.84738,210.05397 387.12061,216.01827 L 380.59639,222.81309 L 386.85156,222.81309 C 392.52103,222.81309 393.15436,222.60241 393.61538,220.56309 z M 271.70590,197.06309 C 270.99848,194.09639 263.07053,176.81309 262.41712,176.81309 C 262.19928,176.81309 260.98483,177.59472 259.71833,178.55004 L 257.41561,180.28699 L 259.28022,185.70659 C 260.30576,188.68737 261.15173,192.29324 261.16015,193.71964 C 261.18662,198.20177 263.00088,199.59304 269.00325,199.73414 C 272.17106,199.80861 272.32444,199.65703 271.70590,197.06309 z M 259.50498,197.78598 C 260.01654,197.27471 259.07548,192.16860 257.08289,184.64387 L 256.11125,180.97464 L 250.27037,180.94670 L 244.42950,180.91877 L 242.61438,186.91370 C 241.61606,190.21092 240.42701,193.13870 239.97205,193.41988 C 239.51708,193.70106 239.14484,194.92978 239.14484,196.15036 C 239.14484,198.29538 239.47647,198.36865 249.06120,198.34134 C 254.51520,198.32580 259.21490,198.07589 259.50498,197.78598 z M 328.37688,178.70356 C 329.87950,178.08853 333.10610,177.38891 334.94052,177.04941 C 336.10015,177.35266 338.86462,176.56625 340.07038,176.47543 C 341.98631,176.33110 342.43114,175.32661 340.51514,174.66389 C 340.14847,174.63284 338.18234,173.85233 336.39484,173.42718 C 332.48925,172.49825 332.16368,170.16045 335.12972,164.34275 C 336.22140,162.20147 337.95837,160.17331 337.74523,159.96017 C 337.02503,159.23998 327.66462,162.16244 324.89484,164.06264 C 323.38234,165.10029 320.87057,165.58426 319.31314,166.69325 C 313.29376,170.97943 306.54539,172.81389 296.85782,172.79747 C 291.79068,172.78888 285.05734,172.17295 281.89484,171.42875 C 278.73234,170.68455 276.14484,170.44496 276.14484,170.89633 C 276.14484,172.07309 284.43247,177.61044 289.64405,179.91577 C 296.92187,183.13509 304.35550,183.61803 315.64484,181.60496 C 321.14484,180.62422 326.87426,179.31859 328.37688,178.70356 z M 254.14484,156.41976 C 254.14484,153.41400 252.26613,151.86931 247.71145,151.13019 C 242.87018,150.34456 240.14484,152.76139 240.14484,157.84027 C 240.14484,161.40374 240.31027,161.58393 244.14484,162.19710 C 246.34484,162.54889 248.15803,163.39391 248.17415,164.07491 C 248.22880,166.38321 254.14484,158.79812 254.14484,156.41976 z M 307.45170,153.72891 C 321.17774,149.22297 331.03373,135.30027 326.58608,126.69946 C 324.67181,122.99767 321.65426,121.81309 314.13876,121.81309 C 308.33326,121.81309 305.39553,123.12237 309.05424,124.07915 C 311.98826,124.84641 314.42073,129.28051 313.70222,132.55186 C 312.68161,137.19864 304.22648,144.70033 297.64484,146.79855 C 295.44484,147.49990 293.19484,148.39843 292.64484,148.79528 C 292.09484,149.19213 289.28234,150.34738 286.39484,151.36250 C 277.93419,154.33691 279.50271,155.81309 291.12384,155.81309 C 298.53973,155.81309 302.73355,155.27777 307.45170,153.72891 z M 286.21947,148.43041 C 299.84778,143.53933 301.56156,142.67361 306.31683,138.27814 C 308.67612,136.09736 309.95430,136.23094 311.19354,134.50239 C 312.01234,132.15203 311.00870,128.45746 309.89676,127.06309 C 308.48400,125.29148 303.11479,125.61570 301.26087,126.24630 C 300.24703,126.75655 292.40665,129.37530 287.72024,132.99761 C 285.47877,134.73013 281.22974,139.00038 278.27795,142.48706 C 273.81840,147.75473 273.12059,149.07895 274.15040,150.31979 C 275.76841,152.26937 275.33267,152.33758 286.21947,148.43041 z M 232.99784,130.39234 C 219.90583,118.33007 216.42981,116.04234 209.55657,114.96451 C 200.12555,113.48558 193.56890,116.15855 193.56890,122.30663 C 193.56890,123.90970 193.89028,128.02426 194.30609,129.83894 C 193.96476,133.09294 195.72573,134.70897 196.60254,131.94639 C 196.93347,130.90373 198.12880,128.98470 199.25883,127.68187 C 201.10450,125.55398 202.09300,125.32352 208.97914,125.41563 C 217.25858,125.52638 220.87005,126.81350 231.14484,133.31547 C 238.04493,137.68189 239.10119,136.01563 232.99784,130.39234 z M 159.89484,298.12376 C 158.93235,297.87224 158.14485,297.06150 158.14485,296.32212 C 158.14485,295.58275 156.45735,294.15324 154.39485,293.14545 C 152.33235,292.13765 150.60135,290.63809 150.54819,289.81309 C 150.37279,287.09094 148.98838,284.89821 147.05839,284.28565 C 144.12618,283.35500 142.14485,280.91783 142.14485,278.24166 C 142.14485,276.90595 141.67312,275.81309 141.09657,275.81309 C 139.77843,275.81309 137.26999,269.98584 137.68317,267.88356 C 136.45383,263.44432 140.35809,261.46738 140.40017,261.05488 C 140.44225,260.64238 140.86428,261.70398 141.33801,261.70398 C 141.81174,261.70398 138.24714,264.41601 139.37581,267.38524 C 138.84038,270.18616 140.92535,275.06678 142.29556,274.21994 C 142.76267,273.93126 144.97886,275.16588 144.55725,277.36910 C 143.81019,281.27302 146.11819,283.35139 150.75672,282.95176 C 154.49986,282.62927 154.92007,283.29900 152.59002,285.87367 C 152.26128,286.81755 151.23960,287.80906 153.34002,290.04235 C 155.26687,292.09107 156.13683,292.35600 158.64485,291.65783 C 160.29484,291.19851 162.31691,291.07346 163.13832,291.37994 C 164.31611,291.81939 164.10473,292.34212 162.13832,293.85290 C 159.82539,295.62991 162.93467,296.79830 166.07229,296.21155 C 166.56685,296.11906 167.78917,295.32567 168.65419,295.65761 C 170.02239,296.18264 170.14542,295.94252 168.86491,296.75233 C 167.33048,297.72272 162.85564,298.89750 159.89484,298.12376 z M 167.69820,295.89942 C 167.38762,295.39689 168.45361,294.40141 170.06706,293.68723 C 171.68051,292.97305 172.74542,291.97582 172.43353,291.47117 C 172.12164,290.96652 172.49159,290.76480 173.25565,291.02291 C 174.01971,291.28101 175.61144,290.18918 176.79284,288.59662 C 178.24448,286.63976 180.00812,287.46855 180.06590,286.36395 C 180.17433,284.29063 186.67308,280.22848 186.67308,282.23288 C 186.67308,284.06884 169.87148,297.03294 167.66227,297.03294 C 167.26711,297.03294 168.00877,296.40194 167.69820,295.89942 z M 185.58453,282.35558 C 185.58453,281.43892 190.02308,274.20994 191.04142,273.03579 C 193.28809,270.44536 192.73666,274.19492 190.56676,277.29287 C 188.72404,279.92373 185.58453,284.68897 185.58453,282.35558 z M 166.15042,84.063094 C 166.16033,80.953664 170.38583,69.372153 171.41354,69.372153 C 172.32999,69.372153 169.78894,79.328544 168.49014,84.063094 C 167.86760,86.332424 166.14319,86.332424 166.15042,84.063094 z M 169.62653,73.218549 C 169.65447,71.849879 171.18192,69.050463 171.76947,69.638013 C 172.55722,70.425763 171.14692,73.813094 170.03785,73.813094 C 169.54670,73.813094 169.61810,73.631049 169.62653,73.218549 z M 315.57427,44.053346 C 312.62453,40.103322 306.14484,33.356248 306.14484,34.234815 C 306.14484,34.519716 304.52730,33.655121 302.55030,32.313494 C 300.57330,30.971867 297.42330,29.320029 295.55030,28.642743 C 293.67730,27.965456 292.14484,27.025155 292.14484,26.553185 C 292.14484,26.081214 291.76140,25.932037 291.29274,26.221680 C 290.82409,26.511323 288.80616,25.914992 286.80844,24.896501 C 284.81073,23.878009 281.93168,22.809443 280.41054,22.521909 C 278.88941,22.234375 274.49484,21.241180 270.64484,20.314808 C 266.79484,19.388436 262.63234,18.438005 261.39484,18.202740 C 258.74791,17.699524 258.35137,15.809913 260.89484,15.820096 C 263.35481,15.829944 278.03786,19.419815 281.64484,20.893281 C 283.29484,21.567312 285.41103,22.268932 286.34748,22.452438 C 289.88315,23.145285 304.79407,30.650932 308.35343,33.529489 C 313.09862,37.367051 322.40276,48.414051 321.26130,49.119510 C 320.78114,49.416267 317.11046,46.110487 315.57427,44.053346 z "
+       d="m 171.91362,315.7972 c -12.02158,-3.0178 -24.6852,-13.02084 -30.35112,-23.97443 -1.90357,-3.68007 -4.11645,-7.22981 -4.91753,-7.88833 -0.93219,-0.7663 -5.35329,-1.1973 -12.28166,-1.1973 -9.99531,0 -13.07488,0.70508 -9.33977,2.13837 3.14876,1.2083 0.92944,8.36163 -2.5942,8.36163 -1.11311,0 -1.76814,0.41372 -1.45563,0.91938 1.06704,1.72651 -2.65596,4.27741 -5.61003,3.84385 -2.14362,-0.31462 -3.21007,0.15242 -4.3618,1.91017 -1.094433,1.67031 -2.470923,3.25933 -4.879783,3.25933 -2.87261,0 -3.93138,-1.68804 -7.35923,-6.18273 -2.20215,-2.8875 -5.06937,-8.03753 -6.37161,-11.44452 -1.83729,-4.80681 -2.92776,-6.33623 -4.86771,-6.82711 -1.375,-0.34793 -4.075,-1.31044 -6,-2.13891 -1.925,-0.82847 -5.00985,-2.15907 -6.85521,-2.95688 -1.84537,-0.79781 -4.20787,-1.17995 -5.25001,-0.84919 -2.67294,0.84836 -2.46251,-1.89785 0.60941,-7.95287 1.37269,-2.70571 2.49581,-6.0127 2.49581,-7.34887 0,-4.35027 -3.11963,-9.38369 -6.75808,-10.90393 -4.10873,-1.71673 -7.70267,-0.78253 -14.62242,3.80092 -2.52846,1.67478 -5.19367,3.1326 -5.9227,3.2396 -0.72903,0.107 -2.61598,1.20962 -4.19322,2.45028 -1.57723,1.24065 -3.48415,2.01918 -4.23758,1.73006 -1.99707,0.48816 -2.07306,1.21957 -4.11419,1.89321 -2.73731,0.90339 -1.78951,0.77142 -3.9341,0.71134 -1.80341,0.18616 -5.38758,0.322 -7.41491,0.90343 -3.6030896,1.03335 -5.3705496,0.33246 -3.8331196,-1.52004 0.67146,-0.80905 3.3774296,-1.26368 7.3017796,-1.22676 5.35746,0.0504 7.26234,-0.48121 13.53475,-3.77727 l 7.29978,-3.83593 -0.6396,-5.11919 c -0.51001,-4.082 -0.11197,-6.63452 1.96459,-12.59828 5.54236,-15.91734 15.5787,-25.8632 31.44303,-31.15953 4.73188,-1.57975 10.25692,-3.13667 12.27786,-3.45984 2.10442,-0.33651 3.92022,-1.28663 4.24964,-2.22364 0.31635,-0.89984 1.46675,-2.98608 2.55643,-4.63608 1.08968,-1.65 2.62392,-4.125 3.40943,-5.5 0.78551,-1.375 3.06109,-4.9254 5.05685,-7.88977 l 3.34066,-5.96573 -3.3489,-1.89538 c -5.86842,-3.98769 -9.28001,-4.49816 -18.08664,-2.70627 -5.46973,1.11293 -9.93327,2.85343 -14.81033,5.77509 -3.83954,2.30013 -11.10652,6.807 -14.26902,8.35944 -5.41303,2.65721 -6.51184,2.82262 -18.75,2.82262 -20.00653,0 -24.3002496,-2.77562 -15.85615,-10.25 2.64077,-2.3375 5.37622,-4.25 6.07878,-4.25 1.95354,0 1.40343,-1.72204 -0.83921,-2.92227 -1.1,-0.5887 -3.35614,-4.60403 -2.42341,-4.80261 0.55819,0.0944 1.26828,0.75943 3.21959,2.80615 1.8684,1.95976 5.95834,3.8461 5.98335,4.37656 0.01738,0.3688 -0.90044,1.6581 -2.41294,1.99401 -3.58843,0.79695 -11.68216,5.70333 -11.99005,7.2683 -0.52254,2.65603 4.26775,3.76477 16.99539,3.9337 12.16287,0.16143 13.02957,0.0365 18.65896,-2.69046 3.24698,-1.57286 6.19614,-3.33306 6.55367,-3.91156 0.35754,-0.5785 1.7485,-1.05182 3.09105,-1.05182 1.41945,0 11.4932,-7.69102 15.0942,-8.2904 1.01573,-0.16907 2.85928,-0.86372 4.09678,-1.54368 1.2375,-0.67996 2.98473,-0.5308 3.88273,-1.27607 1.48122,-1.22931 1.42419,-1.40428 -0.61453,-1.88555 -4.08346,-0.96398 -14.39624,0.74077 -22.5182,3.72235 -4.40001,1.61525 -10.06459,3.04204 -12.58797,3.17065 -6.53504,0.33306 -16.59805,-3.22016 -21.41203,-7.56054 l -4,-3.60648 -0.97711,3.21782 c -0.09461,4.60294 -1.67835,2.3313 -1.67835,-2.87037 0,-2.75 0.74945,-3.78183 1.29945,-3.78183 0.55,0 1.47783,0.22411 1.47783,0.75189 1.3991,2.67844 8.90273,9.25971 13.37817,10.41353 9.5628,2.46538 11.37131,2.40639 20.84446,-0.67984 4.58944,-1.49518 8.63945,-2.66001 9,-2.58852 0.36055,0.0715 2.00555,-0.45404 3.65555,-1.16787 1.65,-0.71384 6.24295,-1.349 10.20656,-1.41149 l 7.20656,-0.1136 1.54344,-4.03343 c 0.84889,-2.21839 1.54344,-4.78037 1.54344,-5.6933 0,-1.91413 3.66199,-4.77327 6.11361,-4.77327 0.91774,0 3.81261,1.7717 6.43305,3.9371 l 4.764443,3.93711 1.91702,-2.43711 c 1.05437,-1.3404 2.78437,-2.4371 3.84445,-2.4371 1.06008,0 1.92742,-0.39534 1.92742,-0.87853 0,-1.32804 -9.61294,-9.21608 -17.500003,-14.35989 -7.87377,-5.13513 -12.6707,-6.74788 -20.095,-6.756 -7.11414,-0.008 -8.91706,2.0766 -7.97019,9.21448 0.38454,2.89877 1.41376,6.06011 2.28717,7.02521 0.8734,0.9651 11.02048,9.65198 13.61874,10.30411 2.97765,0.74734 6.09891,2.91932 3.39658,2.91271 -3.73236,-0.009 -19.20678,-11.07983 -21.78148,-16.22058 -0.75519,-1.50783 -1.82196,-3.34941 -2.37061,-4.09239 -1.08891,-1.47463 -1.22303,-1.40873 -2.68725,-8.43472 -0.7399,-3.55033 -0.55118,-5.47112 0.3299,-7.05496 0.44085,-1.40754 0.94242,-2.39113 1.23671,-3.04074 0.2943,-0.59463 1.60342,-1.72221 3.51984,-2.75 7.41474,-3.97657 19.38395,-1.49444 27.84086,5.77353 8.16688,7.0187 18.047693,17.56391 21.218573,22.64535 3.41431,5.47154 6.57737,8.42454 7.75859,7.24332 1.66052,-1.66051 1.21663,-13.63984 -0.83487,-22.5309 -1.2056,-5.225 -2.98723,-13.325 -3.95917,-18 -2.96176,-14.24588 -7.20743,-29.98616 -10.29459,-38.1659 -2.12702,-5.63576 -2.15243,-5.65864 -4.77765,-4.30108 -1.451603,0.75064 -4.078143,3.0036 -5.836763,5.00656 l -3.1975,3.64175 -3.35206,-1.59067 c -4.17656,-1.98191 -9.7784,-2.04094 -13.45104,-0.14175 -3.52582,1.82328 -9.09878,8.09576 -9.09878,10.24087 0,1.45456 -0.17891,1.46503 -1.75,0.10247 -4.42056,-3.83383 -14.27296,-2.78917 -18.81144,1.9946 -3.24818,3.42372 -6.43857,9.49021 -6.43857,12.24286 0,2.8435 -4.37245,2.35964 -4.78682,-0.52971 -1.24017,-8.64775 -1.16256,-14.375926 -0.15841,-14.375926 0.60538,0 0.97686,4.338426 1.01552,6.125926 0.03866,1.7875 0.41799,4.13996 0.84296,5.2277 0.73578,1.88327 0.83793,1.8594 2.13952,-0.5 0.75176,-1.36274 1.27345,-2.94987 1.1593,-3.52696 -0.25363,-1.28227 6.97105,-8.47882 8.89998,-8.86532 0.76337,-0.15296 2.64157,-0.79225 4.17377,-1.42063 1.5322,-0.62839 4.2322,-0.91359 6,-0.63379 l 2.13154,-0.57393 1.3091,-4.68721 c 0.12455,-3.17342 0.92286,-8.24486 1.77403,-11.26986 0.85117,-3.025 1.669,-6.01711 1.81738,-6.64912 0.14839,-0.63202 0.7047,-1.75702 1.23625,-2.5 0.53154,-0.74299 1.81573,-2.925884 2.85375,-4.850884 1.03802,-1.925 4.89209,-7.775 8.5646,-13 8.987,-12.786116 8.82453,-12.463502 5.37942,-10.681964 -1.57754,0.815776 -3.76457,1.640445 -4.86007,1.832597 -1.09549,0.192152 -2.21681,0.589926 -2.49181,0.883942 -0.275,0.294017 -3.18141,2.292906 -6.45869,4.441976 -11.73325,7.694052 -21.31963,18.194524 -27.40984,30.023453 -2.02302,3.92928 -4.26237,8.438827 -4.27623,7.019457 -0.04323,-4.42626 14.51976,-27.508164 17.55544,-27.525784 0.59912,-0.0035 1.86037,-1.361289 2.80277,-3.01736 0.9424,-1.656071 2.5174,-3.304813 3.5,-3.663872 0.9826,-0.359058 3.13655,-1.708029 4.78655,-2.997714 1.65,-1.289684 3.47145,-2.675597 4.04766,-3.079806 1.7462,-1.22494 11.71708,-6.09511 15.56692,-7.603499 4.27297,-1.674173 5.54953,-5.089098 3.83942,-10.270814 -1.39446,-4.2252544 -0.02631,-6.3606134 4.07532,-6.3606134 2.12117,0 3.79435,1.206232 7.024163,5.0638944 l 4.23974,5.063893 6.33572,-0.678498 c 6.39023,-0.684336 20.71334,0.727749 20.71334,2.064338 0,0.397898 -10.82979,-0.138544 -15.09229,-0.337086 -5.11018,-0.238026 -7.40938,-0.02439 -6.75,0.627196 0.55,0.5435 7.075,4.206521 14.5,8.140047 7.425,3.933526 14.625,8.159987 16,9.392136 3.54708,3.178567 14.84836,17.115981 15.20325,18.749545 0.16321,0.75129 1.28113,2.17495 2.48427,3.16369 2.13232,1.75235 2.29634,1.7216 6.5,-1.21853 4.02278,-2.81362 4.96453,-3.0266 14.01902,-3.17038 8.98774,-0.14273 9.6357,-0.2865 8.75,-1.941453 -1.22404,-2.287128 -1.21779,-3.198284 0.0167,-2.43533 0.53528,0.330823 1.71156,2.157381 2.61394,4.059017 0.90239,1.901636 2.03943,3.457526 2.52676,3.457526 1.45055,0 1.00461,3.01728 -0.53315,3.60737 -0.78057,0.29953 -2.6734,-0.27709 -4.20629,-1.28139 -4.37243,-2.86464 -15.57278,-2.29225 -21.66142,1.107 l -4.699,2.62342 -4.26715,-1.94391 c -3.48172,-1.58611 -5.13707,-1.7814 -8.99334,-1.06097 -6.19768,1.15785 -10.71287,3.5071 -13.57607,7.06362 -1.27736,1.58667 -2.71941,2.88486 -3.20454,2.88486 -1.39915,0 -6.06517,-5.04183 -5.50493,-5.9483 0.92294,-1.49336 -2.30188,-10.24144 -6.06092,-16.44167 -9.01646,-14.871884 -16.87895,-22.461382 -21.950633,-21.188472 -3.88536,0.975162 -6.35757,7.661199 -8.17325,22.104302 -1.32003,10.50048 -1.35626,13.27026 -0.23942,18.30447 l 1.31595,5.93178 4.77364,-3.77334 c 5.374363,-4.24819 17.774763,-8.75538 18.932843,-6.88156 0.49769,0.80528 0.80477,0.77971 1.09507,-0.0912 0.31088,-0.93264 1.37103,-0.94975 4.62105,-0.07461 5.12949,1.38123 11.52276,5.603585 6.55085,4.796755 l -9.47734,-3.023225 0.64046,3.89253 c 0.35226,2.1409 1.71579,7.71754 3.03008,12.39254 1.31428,4.675 4.03191,16.375 6.03918,26 5.0217,24.07955 7.94658,36.22 9.36432,38.86908 1.16682,2.18021 1.41907,2.02449 9.01189,-5.56304 6.24518,-6.24084 9.79119,-8.80721 17.68482,-12.79913 8.52074,-4.30904 10.83394,-5.0416 16.88506,-5.34726 6.27663,-0.31705 7.44918,-0.0677 11.18491,2.3781 3.34123,2.18757 4.30267,3.49849 4.82253,6.57554 0.35712,2.11379 0.33721,5.31553 -0.0443,7.11498 -0.79529,3.7516 -5.40557,10.7667 -9.58806,13.20178 -1.70953,2.54939 -5.86256,4.90328 -7.606,4.90328 -3.55528,2.44357 -15.86631,3.85718 -28.32888,3.164 l -12.85914,-0.71525 -0.65425,2.60673 c -1.0099,4.02378 0.0817,5.50221 6.09552,8.25542 4.98887,2.28398 6.92152,2.56079 18.85284,2.70031 9.31514,0.10893 13.74353,-0.21607 14.79562,-1.08586 0.82582,-0.68273 20.71971,-10.33301 23.78861,-10.55432 1.5587,-0.1124 3.67553,-1.31739 5.56487,-1.27728 4.7668,0.10119 5.15556,2.23233 0.86863,4.76169 -4.05924,2.39501 -6.36225,6.81664 -4.22664,8.11488 0.7271,0.44201 3.6845,2.13861 6.572,3.77023 7.59043,4.28907 6.93011,5.45371 -2.99964,5.29058 -4.5377,-0.0745 -8.59854,0.39923 -9.14854,0.80394 -0.55,0.40472 -4.02682,1.0802 -7.60182,1.74995 -3.575,0.66975 -8.38677,1.63711 -10.69282,2.14969 -12.83303,2.85247 -28.00909,-1.88025 -40.16724,-12.52636 l -3.37849,-2.95831 -2.98158,3.33697 c -1.63987,1.83533 -2.54852,3.61657 -2.01922,3.95831 0.905,0.58432 3.27612,5.3474 8.14443,16.36045 1.15221,2.60651 2.09492,5.65129 2.09492,6.76618 0,2.29902 0.91707,2.77516 18.47817,9.59381 6.58799,2.558 13.01003,4.65091 14.2712,4.65091 1.38016,0 4.39828,-1.99104 7.58111,-5.00123 4.15354,-3.92825 6.41227,-5.24233 10.52801,-6.125 9.18609,-1.97007 16.34784,-1.00677 24.85493,3.34312 3.56817,1.82449 11.28658,10.09536 11.28658,12.09444 0,0.70387 1.35,1.57627 3,1.93867 1.65,0.3624 3,1.29816 3,2.07946 0,1.56803 5.49978,2.03893 6.42017,0.5497 0.29602,-0.47896 1.98195,-1.15958 3.74652,-1.5125 3.39887,-0.67977 14.17872,-10.97887 15.34826,-14.66375 2.25049,-0.95761 4.83474,2.37555 4.33182,11.35533 l -0.34677,6.19176 7.35233,-0.18762 c 5.22537,-0.13335 7.87997,-0.66513 9.17623,-1.83823 1.61499,-1.46155 2.49093,-1.50441 7.64767,-0.3742 3.20307,0.70202 6.11883,1.71212 6.47947,2.24466 1.08575,1.60332 25.57497,0.0619 28.39737,-1.78743 4.13925,-2.71214 15.02491,-1.31408 18.5187,1.37119 0.47947,0.46701 5.94358,4.07014 5.89279,10.25589 -0.0543,6.61831 -4.95933,13.52178 -11.59758,16.32286 -3.36507,1.41993 -4.90534,1.47572 -11.86698,0.42989 -4.4,-0.66101 -8.59824,-1.677 -9.32941,-2.25777 -2.08197,-1.65369 -20.01001,-3.67278 -26.80077,-3.01836 -5.93576,0.57202 -6.21943,0.49059 -8.94422,-2.56752 -1.54772,-1.73705 -3.41999,-2.92575 -4.1606,-2.64156 -0.74061,0.2842 -4.8129,0.0346 -9.04953,-0.55466 -6.97019,-0.96946 -7.7987,-0.90415 -8.70922,0.68654 -3.93489,6.87428 -10.93916,12.10944 -19.16509,14.32446 l -4.1476,1.11683 1.90322,3.93921 c 2.36232,4.88943 2.36483,6.87757 0.0126,9.93713 -1.48573,1.93246 -2.43998,2.28256 -4.88731,1.79309 -2.16416,-0.43283 -3.20233,-0.1881 -3.60018,0.84869 -0.79775,2.07889 -1.95982,1.83459 -6.82171,-1.43415 -5.08411,-3.41814 -11.2586,-6.10894 -14.01796,-6.10894 -1.81222,0 -6.33433,2.08649 -9.6297,4.44311 -0.74114,0.53002 -2.2276,1.09797 -3.30324,1.26213 -1.07564,0.16415 -3.16925,0.91007 -4.65247,1.65759 -1.48322,0.74752 -3.68356,1.27379 -4.88965,1.16948 -1.20609,-0.10431 -6.37292,3.09026 -11.74398,3.73514 -2.83846,0.3408 -3.97232,5.81828 -2.50993,12.12513 2.08015,8.97111 8.40376,21.07678 12.89532,24.68629 2.22789,1.79038 3.84393,3.46201 3.5912,3.71474 -1.08134,1.08134 -8.43175,0.9508 -13.14964,-0.23355 z m -67.59337,-20.06006 c 2.40845,0 3.80154,-0.60345 4.58195,-1.98478 0.61674,-1.09163 1.90884,-2.57965 2.87134,-3.30671 2.05947,-1.55569 2.25338,-3.54585 0.48823,-5.0108 -0.69397,-0.57594 -4.40647,-1.28597 -8.25,-1.57784 -10.147073,-0.77056 -15.887713,-1.65603 -16.473983,-2.54107 -1.16832,-1.32895 -4.01424,-0.81956 -4.01424,0.65249 0,3.33245 4.75259,12.07394 8.46418,15.56827 l 4.03581,3.79957 2.41806,-2.79957 c 1.780083,-2.06093 3.331083,-2.79956 5.878653,-2.79956 z m -5.796713,-14.5 c 0,-0.55 -0.675,-1 -1.5,-1 -0.825,0 -1.5,-0.45 -1.5,-1 0,-0.55 -0.58672,-1 -1.30383,-1 -1.93443,0 -8.02886,-6.65266 -7.24956,-7.91359 0.36929,-0.59753 -2.21248,-3.78641 -3.4466,-7.08641 -1.23412,-3.3 -2.47337,-6 -2.75389,-6 -3.35319,0 -13.24611,10.88501 -13.24611,14.57446 0,1.44242 4.04351,3.08116 9.75425,3.95316 2.33984,0.35729 3.84598,2.17928 4.70655,1.84905 0.86057,-0.33023 2.20395,-0.0699 2.9853,0.57859 0.78134,0.64846 3.08677,1.38567 5.12317,1.63825 2.0364,0.25258 4.42889,0.88368 5.31663,1.40245 2.11378,1.23523 3.11409,1.23652 3.11409,0.004 z m 16.000003,-0.42846 c 4.675,-0.28884 8.6125,-0.5356 8.75,-0.54835 0.1375,-0.0128 0.25,-0.40485 0.25,-0.87132 0,-0.46646 -1.86768,-1.69033 -4.15041,-2.71969 -4.11478,-1.85551 -11.22777,-10.3012 -11.84959,-12.93864 -2.48729,-2.7507 -7.996603,-22.50767 -7.998633,-36.17118 -0.002,-13.65131 1.570663,-22.62592 7.114673,-40.5999 2.75024,-8.91646 5.08673,-17.78915 5.1922,-19.7171 0.17067,-3.11991 -0.082,-3.50536 -2.29807,-3.50536 -1.96144,0 -3.17237,1.16718 -5.70614,5.5 -6.248593,10.68528 -10.815823,18.1707 -11.804033,19.34609 -0.55,0.65418 -2.12187,3.43143 -3.49304,6.17166 -1.37118,2.74024 -3.00174,5.65725 -3.62345,6.48225 -0.62172,0.825 -2.31234,6.03655 -3.75695,11.58122 -3.47587,13.3411 -3.4844,22.82802 -0.03138,34.91878 1.41368,4.95 2.91326,10.35 3.33241,12 1.7687,6.9626 6.13708,14.27828 10.22669,17.12651 6.310863,4.39523 7.872753,5.1707 9.680143,4.80615 0.91607,-0.18477 5.49058,-0.57227 10.16558,-0.86112 z m 17.70902,-1.57154 c 1.28667,0 1.42808,-0.87905 0.84456,-5.25 -0.38549,-2.8875 -1.00524,-10.57491 -1.37723,-17.08314 -0.57676,-10.09069 -0.96218,-12.04177 -2.61748,-13.25 -2.7769,-2.0269 -14.89043,-1.84557 -20.9126,0.31304 l -4.85374,1.73979 0.59828,4.36495 c 0.80079,5.8424 6.18487,15.77962 11.10298,20.49244 2.19659,2.10489 6.12801,4.74519 8.7365,5.86734 2.72874,1.17387 4.46646,2.48723 4.09216,3.09285 -0.41015,0.66364 -0.005,0.80505 1.09529,0.38267 0.96021,-0.36847 2.44128,-0.66994 3.29128,-0.66994 z m 47.54098,-8.16194 c 1.5125,-0.59439 2.75,-1.37358 2.75,-1.73154 0,-0.35796 -1.40317,-2.35312 -3.11816,-4.4337 -2.39569,-2.90639 -3.72622,-3.71219 -5.74261,-3.47784 -3.33586,0.3877 -5.54709,4.67158 -4.61108,8.9332 0.63458,2.8892 0.87175,3.02221 4.32164,2.42377 2.00761,-0.34826 4.88771,-1.11951 6.40021,-1.71389 z m 45.71099,-3.18689 c 1.47653,-2.38909 -7.86685,-8.37297 -7.86685,-8.8313 0,-0.45834 -9.67002,-4.48654 -10.37388,-4.48654 -0.85598,0 -1.15362,1.08032 -0.89886,3.26253 0.3395,2.90812 0.79693,3.33281 4.21102,3.90962 2.10657,0.3559 5.99712,2.18777 8.64566,4.07081 5.12667,3.64492 5.42326,3.46582 6.28291,2.07488 z m 7.7288,-0.90117 c 0.51407,-0.6875 0.86271,-1.925 0.77475,-2.75 -0.088,-0.825 -0.22845,-2.23883 -0.31223,-3.14185 -0.2034,-2.19258 -8.96163,-11.85815 -10.74498,-11.85815 -0.77403,0 -1.40902,-0.3375 -1.41108,-0.75 -0.002,-0.4125 -2.08233,-1.60493 -4.62281,-2.64985 -5.42954,-2.23321 -9.1472,-1.43837 -11.0412,2.36062 -1.58908,3.18737 -1.06603,3.82566 4.22712,5.15849 2.9692,0.74766 6.85243,1.95861 9.6107,4.06806 1.83147,1.40066 5.12994,4.40611 7.32994,6.67878 4.37924,4.52389 4.81255,4.72577 6.18979,2.8839 z m 10.20095,-18.69153 c 2.22827,-1.19253 10.10926,-9.72294 10.10926,-10.94229 0,-0.51068 -2.1375,-1.18026 -4.75,-1.48796 -2.6125,-0.3077 -7.1125,-0.84215 -10,-1.18767 -3.51648,-0.42079 -6.34009,0.46021 -7.40371,1.11976 -1.35893,0 -2.43349,2.0005 -4.1341,5.68969 l -2.4201,5.25 2.55233,2.61504 c 2.53094,2.59313 2.60156,2.60354 8.43204,1.24339 3.23385,-0.75441 6.66028,-1.78939 7.61428,-2.29996 z m 91.42966,-1.99018 c 3.581,-1.3615 8.54929,-7.04047 9.68316,-11.06829 1.35517,-4.81392 0.37407,-5.10193 -1.42739,-8.5 l -2.41389,-3 1.09635,4.14098 c 2.05685,7.76882 -2.86988,15.51609 -11.75863,18.49042 -1.90127,0.6362 -1.89215,0.67416 0.1849,0.76892 1.2017,0.0548 3.28767,-0.31959 4.6355,-0.83203 z m -23.10521,-6.05549 c -0.64423,-4.3894 0.43028,-7.53942 3.93234,-11.52805 1.71525,-1.95356 2.89097,-3.77958 2.61271,-4.05784 -0.27825,-0.27826 -3.37564,-0.0484 -6.88308,0.51078 -3.50744,0.55918 -10.07027,0.87561 -14.58407,0.70318 l -8.20692,-0.31351 -1.54308,3.85656 c -0.8487,2.1211 -1.54309,5.35879 -1.54309,7.19486 l 0,3.3383 7.25,1.14875 c 7.06122,1.11884 11.56699,1.64746 16.82681,1.97413 2.31736,0.14393 2.53266,-0.14072 2.13838,-2.82716 z m 23.3645,1.10173 c 6.45293,-3.93499 7.17082,-14.05458 1.17586,-16.57527 -2.79836,-1.17662 -3.46931,-1.09026 -6.39453,0.8231 -3.46205,2.26449 -7.36102,8.20182 -7.36102,11.20934 0,2.70922 4.00891,6.42879 6.92431,6.42456 1.41663,-0.002 3.96155,-0.84883 5.65538,-1.88173 z m -18.70277,-1.28906 c 0.74691,-1.19599 1.54267,-4.23349 1.76835,-6.75 l 0.41033,-4.57547 -2.5278,2.80576 c -1.80224,2.00042 -2.5278,3.93789 -2.5278,6.75 0,4.43494 0.87595,4.97377 2.87692,1.76971 z m -32.70903,-3.18245 c -0.25524,-3.20923 0.34001,-5.71179 2.25964,-9.5 l 2.60616,-5.14302 -2.93506,0 c -4.5314,0 -6.02296,0.93135 -8.14123,5.0835 -2.83712,5.56122 -2.52169,8.32682 1.2926,11.33293 4.52053,3.56272 5.31938,3.27465 4.91789,-1.77341 z m -80.24758,0.77219 c 4.89498,-1.95858 7.21552,-5.32855 6.89959,-10.01978 -0.43626,-6.47776 -7.52042,-9.67646 -11.39559,-5.14543 -4.89527,5.72379 -5.8206,11.35371 -2.42431,14.75 2.39948,2.39948 2.0183,2.37661 6.92031,0.41521 z m -1.41747,-3.40626 c -0.34756,-1.09508 -0.58819,-2.55758 -0.15473,-3.25 0.74438,-1.62515 0.80001,-2.00432 1.12546,-2.74259 1.91456,-0.89043 3.45438,-2.33606 4.55612,0.56172 0.87015,2.28868 0.68309,3.09875 -1.1714,5.07277 -2.6945,2.86815 -3.53681,2.93741 -4.35545,0.3581 z m 72.08958,-2.63713 c 0.224,-2.8875 0.98346,-5.96511 0.92768,-6.07914 -0.0558,-0.11403 -9.88522,-0.33903 -21.84321,-0.5 -18.45138,-0.24839 -21.88804,-0.0657 -22.70809,1.20732 -2.21147,3.43293 -0.64979,4.45357 9.5439,6.2374 14.16094,2.47809 22.13593,3.49056 28.64079,3.6361 l 5.65348,0.1265 -0.21455,-4.62818 z m -44.48111,-8.62182 c 0.70143,-1.35541 0.49786,-1.75 -0.90284,-1.75 -3.67853,0 -5.53262,4.27585 -2.96671,6.84176 0.98901,0.98901 1.41702,0.77529 2.10283,-1.05 0.47359,-1.26047 1.26861,-3.07926 1.76672,-4.04176 z m 30.35923,-3 c 1.01487,-4.4893 1.20225,-7.29061 0.62174,-9.29482 -0.51989,-1.79489 -1.38974,-1.2143 -7.11651,4.75 l -6.52422,6.79482 6.25517,0 c 5.66947,0 6.3028,-0.21068 6.76382,-2.25 z m -121.90948,-23.5 c -0.70742,-2.9667 -8.63537,-20.25 -9.28878,-20.25 -0.21784,0 -1.43229,0.78163 -2.69879,1.73695 l -2.30272,1.73695 1.86461,5.4196 c 1.02554,2.98078 1.87151,6.58665 1.87993,8.01305 0.0265,4.48213 1.84073,5.8734 7.8431,6.0145 3.16781,0.0745 3.32119,-0.0771 2.70265,-2.67105 z m -12.20092,0.72289 c 0.51156,-0.51127 -0.4295,-5.61738 -2.42209,-13.14211 l -0.97164,-3.66923 -5.84088,-0.0279 -5.84087,-0.0279 -1.81512,5.99493 c -0.99832,3.29722 -2.18737,6.225 -2.64233,6.50618 -0.45497,0.28118 -0.82721,1.5099 -0.82721,2.73048 0,2.14502 0.33163,2.21829 9.91636,2.19098 5.454,-0.0155 10.1537,-0.26545 10.44378,-0.55536 z m 68.8719,-19.08242 c 1.50262,-0.61503 4.72922,-1.31465 6.56364,-1.65415 1.15963,0.30325 3.9241,-0.48316 5.12986,-0.57398 1.91593,-0.14433 2.36076,-1.14882 0.44476,-1.81154 -0.36667,-0.031 -2.3328,-0.81156 -4.1203,-1.23671 -3.90559,-0.92893 -4.23116,-3.26673 -1.26512,-9.08443 1.09168,-2.14128 2.82865,-4.16944 2.61551,-4.38258 -0.7202,-0.72019 -10.08061,2.20227 -12.85039,4.10247 -1.5125,1.03765 -4.02427,1.52162 -5.5817,2.63061 -6.01938,4.28618 -12.76775,6.12064 -22.45532,6.10422 -5.06714,-0.009 -11.80048,-0.62452 -14.96298,-1.36872 -3.1625,-0.7442 -5.75,-0.98379 -5.75,-0.53242 0,1.17676 8.28763,6.71411 13.49921,9.01944 7.27782,3.21932 14.71145,3.70226 26.00079,1.68919 5.5,-0.98074 11.22942,-2.28637 12.73204,-2.9014 z m -74.23204,-22.2838 c 0,-3.00576 -1.87871,-4.55045 -6.43339,-5.28957 -4.84127,-0.78563 -7.56661,1.6312 -7.56661,6.71008 0,3.56347 0.16543,3.74366 4,4.35683 2.2,0.35179 4.01319,1.19681 4.02931,1.87781 0.0546,2.3083 5.97069,-5.27679 5.97069,-7.65515 z m 53.30686,-2.69085 c 13.72604,-4.50594 23.58203,-18.42864 19.13438,-27.02945 -1.91427,-3.70179 -4.93182,-4.88637 -12.44732,-4.88637 -5.8055,0 -8.74323,1.30928 -5.08452,2.26606 2.93402,0.76726 5.36649,5.20136 4.64798,8.47271 -1.02061,4.64678 -9.47574,12.14847 -16.05738,14.24669 -2.2,0.70135 -4.45,1.59988 -5,1.99673 -0.55,0.39685 -3.3625,1.5521 -6.25,2.56722 -8.46065,2.97441 -6.89213,4.45059 4.729,4.45059 7.41589,0 11.60971,-0.53532 16.32786,-2.08418 z m -21.23223,-5.2985 c 13.62831,-4.89108 15.34209,-5.7568 20.09736,-10.15227 2.35929,-2.18078 3.63747,-2.0472 4.87671,-3.77575 0.8188,-2.35036 -0.18484,-6.04493 -1.29678,-7.4393 -1.41276,-1.77161 -6.78197,-1.44739 -8.63589,-0.81679 -1.01384,0.51025 -8.85422,3.129 -13.54063,6.75131 -2.24147,1.73252 -6.4905,6.00277 -9.44229,9.48945 -4.45955,5.26767 -5.15736,6.59189 -4.12755,7.83273 1.61801,1.94958 1.18227,2.01779 12.06907,-1.88938 z M 99.376537,129.81639 c -13.09201,-12.06227 -16.56803,-14.35 -23.44127,-15.42783 -9.43102,-1.47893 -15.98767,1.19404 -15.98767,7.34212 0,1.60307 0.32138,5.71763 0.73719,7.53231 -0.34133,3.254 1.41964,4.87003 2.29645,2.10745 0.33093,-1.04266 1.52626,-2.96169 2.65629,-4.26452 1.84567,-2.12789 2.83417,-2.35835 9.72031,-2.26624 8.27944,0.11075 11.89091,1.39787 22.1657,7.89984 6.900093,4.36642 7.956353,2.70016 1.853,-2.92313 z m -73.103,167.73142 c -0.96249,-0.25152 -1.74999,-1.06226 -1.74999,-1.80164 0,-0.73937 -1.6875,-2.16888 -3.75,-3.17667 -2.0625,-1.0078 -3.7935,-2.50736 -3.84666,-3.33236 -0.1754,-2.72215 -1.55981,-4.91488 -3.4898,-5.52744 -2.93221,-0.93065 -4.9135396,-3.36782 -4.9135396,-6.04399 0,-1.33571 -0.47173,-2.42857 -1.04828,-2.42857 -1.31814,0 -3.82658,-5.82725 -3.4134,-7.92953 -1.22934,-4.43924 2.67492,-6.41618 2.717,-6.82868 0.04208,-0.4125 0.46411,0.6491 0.93784,0.6491 0.47373,0 -3.09087,2.71203 -1.9622,5.68126 -0.53543,2.80092 1.54954,7.68154 2.91975,6.8347 0.46711,-0.28868 2.6832996,0.94594 2.2616896,3.14916 -0.74706,3.90392 1.56094,5.98229 6.19947,5.58266 3.74314,-0.32249 4.16335,0.34724 1.8333,2.92191 -0.32874,0.94388 -1.35042,1.93539 0.75,4.16868 1.92685,2.04872 2.79681,2.31365 5.30483,1.61548 1.64999,-0.45932 3.67206,-0.58437 4.49347,-0.27789 1.17779,0.43945 0.96641,0.96218 -1,2.47296 -2.31293,1.77701 0.79635,2.9454 3.93397,2.35865 0.49456,-0.0925 1.71688,-0.88588 2.5819,-0.55394 1.3682,0.52503 1.49123,0.28491 0.21072,1.09472 -1.53443,0.97039 -6.00927,2.14517 -8.97007,1.37143 z m 7.80336,-2.22434 c -0.31058,-0.50253 0.75541,-1.49801 2.36886,-2.21219 1.61345,-0.71418 2.67836,-1.71141 2.36647,-2.21606 -0.31189,-0.50465 0.05806,-0.70637 0.82212,-0.44826 0.76406,0.2581 2.35579,-0.83373 3.53719,-2.42629 1.45164,-1.95686 3.21528,-1.12807 3.27306,-2.23267 0.10843,-2.07332 6.60718,-6.13547 6.60718,-4.13107 0,1.83596 -16.8016,14.80006 -19.01081,14.80006 -0.39516,0 0.3465,-0.631 0.03593,-1.13352 z m 17.88633,-13.54384 c 0,-0.91666 4.43855,-8.14564 5.45689,-9.31979 2.24667,-2.59043 1.69524,1.15913 -0.47466,4.25708 -1.84272,2.63086 -4.98223,7.3961 -4.98223,5.06271 z M 32.529117,83.48714 c 0.0099,-3.10943 4.23541,-14.690941 5.26312,-14.690941 0.91645,0 -1.6246,9.956391 -2.9234,14.690941 -0.62254,2.26933 -2.34695,2.26933 -2.33972,0 z m 3.47611,-10.844545 c 0.02794,-1.36867 1.55539,-4.168086 2.14294,-3.580536 0.78775,0.78775 -0.62255,4.175081 -1.73162,4.175081 -0.49115,0 -0.41975,-0.182045 -0.41132,-0.594545 z M 181.95297,43.477392 c -2.94974,-3.950024 -9.42943,-10.697098 -9.42943,-9.818531 0,0.284901 -1.61754,-0.579694 -3.59454,-1.921321 -1.977,-1.341627 -5.127,-2.993465 -7,-3.670751 -1.873,-0.677287 -3.40546,-1.617588 -3.40546,-2.089558 0,-0.471971 -0.38344,-0.621148 -0.8521,-0.331505 -0.46865,0.289643 -2.48658,-0.306688 -4.4843,-1.325179 -1.99771,-1.018492 -4.87676,-2.087058 -6.3979,-2.374592 -1.52113,-0.287534 -5.9157,-1.280729 -9.7657,-2.207101 -3.85,-0.926372 -8.0125,-1.876803 -9.25,-2.112068 -2.64693,-0.503216 -3.04347,-2.392827 -0.5,-2.382644 2.45997,0.0098 17.14302,3.599719 20.75,5.073185 1.65,0.674031 3.76619,1.375651 4.70264,1.559157 3.53567,0.692847 18.44659,8.198494 22.00595,11.077051 4.74519,3.837562 14.04933,14.884562 12.90787,15.590021 -0.48016,0.296757 -4.15084,-3.009023 -5.68703,-5.066164 z"
        id="path1462"
-       sodipodi:nodetypes="ccccccccccccccccccccccccccccccccccccccccccccccccsccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc" />
+       sodipodi:nodetypes="ccccccccccccccccccccccccccccccccccccccccccccccccsccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc"
+       inkscape:connector-curvature="0" />
   </g>
 </svg>
diff --git a/doc/manual/mainpage.dox b/doc/manual/mainpage.dox
new file mode 100644
index 0000000000..45c4e60b25
--- /dev/null
+++ b/doc/manual/mainpage.dox
@@ -0,0 +1,29 @@
+/** @mainpage Paparazzi Developer's Guide
+
+Welcome to the Paparazzi Developer's Guide -- the developer's resource for
+learning about the internal architecture of the Paparazzi project. @par
+
+Developers that want to contribute to Paparazzi should read the following
+sections before starting work:
+
+- The @subpage styleguide provides rules that developers should
+  follow when writing new code for Paparazzi.
+
+@ref primer provide introductory materials for new developers on various
+specific topics.
+
+
+ */
+
+/** @page primer Paparazzi Technical Primers
+
+This pages lists Technical Primers available for Paparazzi Developers.
+They seek to provide information to pull novices up the learning curves
+associated with the fundamental technologies used by Paparazzi.
+
+- TODO
+
+
+Contributions or suggestions for new Technical Primers are welcome.
+
+ */
diff --git a/doc/manual/style.dox b/doc/manual/style.dox
new file mode 100644
index 0000000000..acf471ac6b
--- /dev/null
+++ b/doc/manual/style.dox
@@ -0,0 +1,225 @@
+/** @page styleguide Style Guides
+
+The goals for each of these guides are:
+- to produce correct code that appears clean, consistent, and readable,
+- to allow developers to create patches that conform to a standard, and
+- to eliminate these issues as points of future contention.
+
+Some of these rules may be ignored in the spirit of these stated goals;
+however, such exceptions should be fairly rare.
+
+The following style guides describe a formatting, naming, and other
+conventions that should be followed when writing or changing the Papaprazzi
+code or documentation:
+
+- @subpage stylec
+- @subpage styledoxygen
+
+
+Feedback would be welcome to improve the Paparazzi guidelines.
+ */
+
+/** @page stylec C Style Guide
+
+This page contains guidelines for writing new C source code for the
+OpenOCD project.
+
+@section styleformat Formatting Guide
+
+- remove any trailing white space at the end of lines.
+- use 2 spaces for indentation; do NOT use tabs.
+- use Unix line endings ('\\n'); do NOT use DOS endings ('\\r\\n')
+- limit adjacent empty lines to at most two (2).
+- remove any trailing empty lines at the end of source files
+- do not "comment out" code from the tree; instead, one should either:
+  -# remove it entirely (git can retrieve the old version), or
+  -# use an @c \#if/\#endif block.
+
+@section stylenames Naming Rules
+
+- most identifiers must use lower-case letters (and digits) only.
+  - macros should use upper-case letters (and digits) only.
+
+@section styletypes Type Guidelines
+- use the types from \<inttypes.h\>:
+  - @c int8_t, @c int16_t, @c int32_t, or @c int64_t: signed types of specified size
+  - @c uint8_t, @c uint16_t, @c uint32_t, or @c uint64_t: unsigned types of specified size
+
+@section stylefunc Functions
+
+- static inline functions should be prefered over macros in most cases:
+@code
+/* do NOT define macro-like functions like this... */
+#define CUBE(x) ((x) * (x) * (x))
+/* instead, define the same expression using a C99 inline function */
+static inline int cube(int x) { return x * x * x; }
+@endcode
+- Functions should be declared static unless required by other modules
+  - define static functions before first usage to avoid forward declarations.
+- Functions should have no space between its name and its parameter list:
+@code
+int32_t f(int32_t x1, int32_t x2)
+{
+  ...
+  int32_t y = f(x1, x2 - x1);
+  ...
+}
+@endcode
+
+@section stylecpp Preprocessor directives
+- For conditional compilation use @c #if instead of @c #ifdef.
+Someone might write code like:
+@code
+#ifdef USE_FOO
+  bar();
+#endif
+@endcode
+Someone else might compile the code with turned-off foo info like:
+@verbatim
+ gcc stuff.c -DUSE_FOO=0
+@endverbatim
+So use @c #if not @c #ifdef to turn a feature on or off. This works fine, and does the right thing, even if USE_FOO is not defined at all (!)
+@code
+#if USE_FOO
+  bar();
+#endif
+@endcode
+- If you really need to test whether a symbol is defined or not, test it with the @c defined construct.
+
+ */
+
+/** @page styledoxygen Doxygen Style Guide
+
+The following sections provide guidelines for OpenOCD developers
+who wish to write Doxygen comments in the code or this manual.
+For an introduction to Doxygen documentation,
+see the @ref primerdoxygen.
+
+@section styledoxyblocks Doxygen Block Selection
+
+Several different types of Doxygen comments can be used; often,
+one style will be the most appropriate for a specific context.
+The following guidelines provide developers with heuristics for
+selecting an appropriate form and writing consistent documentation
+comments.
+
+-# use @c /// to for one-line documentation of instances.
+-# for documentation requiring multiple lines, use a "block" style:
+@verbatim
+/**
+ * First sentence is brief description.  Remaining text becomes
+ * the full description block, where "empty" lines start new paragraphs.
+ *
+ * One can make text appear in @a italics, @b bold, @c monospace, or
+ * in blocks such as the one in which this example appears in the Style
+ * Guide.  See the Doxygen Manual for the full list of commands.
+ *
+ * @param foo For a function, describe the parameters (e.g. @a foo).
+ * @returns The value(s) returned, or possible error conditions.
+ */
+@endverbatim
+  -# The block should start on the line following the opening @c /**.
+  -# The end of the block, \f$*/\f$, should also be on its own line.
+  -# Every line in the block should have a @c '*' in-line with its start:
+    - A leading space is required to align the @c '*' with the @c /** line.
+    - A single "empty" line should separate the function documentation
+      from the block of parameter and return value descriptions.
+    - Except to separate paragraphs of documentation, other extra
+      "empty" lines should be removed from the block.
+  -# Only single spaces should be used; do @b not add mid-line indentation.
+-# If the total line length will be less than 80 columns, then
+  - The <code> /**< description </code> form can be used on the same line for a detailed description.
+  - The <code>///< brief </code> form can be used for brief descriptions.
+  - This style should be used sparingly; the best use is for fields:
+@verbatim
+int field;  /**< detailed field description */
+int field2; ///< a brief description
+@endverbatim
+-# Every documented file should contain a block with @c \@file after the license header:
+@verbatim
+/**
+ * @file foo/bar/file.c
+ * Brief description of file.
+ *
+ * More detailed description of file.
+ */
+@endverbatim
+
+@section styledoxyall Doxygen Style Guide
+
+The following guidelines apply to all Doxygen comment blocks:
+
+-# Use the @c '\@cmd' form for all doxygen commands (do @b not use @c '\\cmd').
+-# It is not necessary to use @c \@brief since @c JAVADOC_AUTOBRIEF is enabled.
+  - The first line (until the first dot) is automatically treated as brief description.
+-# Use symbol names such that Doxygen automatically creates links:
+  -# @c function_name() can be used to reference functions
+    (e.g. ahrs_propagate()).
+  -# @c struct_name::member_name should be used to reference structure
+    fields in the documentation (e.g. @c Ahrs::body_rate).
+  -# URLS get converted to markup automatically, without any extra effort.
+  -# new pages can be linked into the heirarchy by using the @c \@subpage
+    command somewhere the page(s) under which they should be linked:
+  -# use @c \@ref in other contexts to create links to pages and sections.
+-# Use good Doxygen mark-up:
+  -# '\@a' (italics) should be used to reference parameters (e.g. <i>foo</i>).
+  -# '\@b' (bold) should be used to emphasizing <b>single</b> words.
+  -# '\@c' (monospace) should be used with <code>file names</code> and
+  <code>code symbols</code>, so they appear visually distinct from
+  surrounding text.
+  -# To mark-up multiple words, the HTML alternatives must be used.
+-# Two spaces should be used when nesting lists; do @b not use '\\t' in lists.
+-# Code examples provided in documentation must conform to the Style Guide.
+
+@section styledoxytext Doxygen Text Inputs
+
+In addition to the guidelines in the preceding sections, the following
+additional style guidelines should be considered when writing
+documentation as part of standalone text files:
+
+-# Text files must contain Doxygen at least one comment block:
+  -# Documentation should begin in the first column (except for nested lists).
+  -# Do NOT use the @c '*' convention that must be used in the source code.
+-# Each file should contain at least one @c \@page block.
+  -# Each new page should be listed as a \@subpage in the \@page block
+  of the page that should serve as its parent.
+  -# Large pages should be structure in parts using meaningful \@section
+  and \@subsection commands.
+-# Include a @c \@file block at the end of each Doxygen @c .dox file to
+  document its contents:
+  - Doxygen creates such pages for files automatically, but no content
+    will appear on them for those that only contain manual pages.
+  - The \@file block should provide useful meta-documentation to assist
+    techincal writers; typically, a list of the pages that it contains.
+  - For example, the @ref styleguide exists in @c doc/manual/style.dox,
+    which contains a reference back to itself.
+-# The \@file and \@page commands should begin on the same line as
+   the start of the Doxygen comment:
+@verbatim
+/** @page pagename Page Title
+
+Documentation for the page.
+
+ */
+/** @file
+
+This file contains the @ref pagename page.
+
+ */
+@endverbatim
+
+For an example, the Doxygen source for this Style Guide can be found in
+@c doc/manual/style.dox, alongside other parts of The Manual.
+
+ */
+
+/** @file
+
+This file contains the @ref styleguide pages.  The @ref styleguide pages
+include the following Style Guides for their respective code and
+documentation languages:
+
+- @ref stylec
+- @ref styledoxygen
+
+ */
diff --git a/sw/airborne/alt_vfilter.h b/sw/airborne/alt_vfilter.h
index 1e779eb602..9df9198c1a 100644
--- a/sw/airborne/alt_vfilter.h
+++ b/sw/airborne/alt_vfilter.h
@@ -18,7 +18,7 @@ extern void alt_vf_update_vz(float vz);
 extern void alt_vf_periodic_task(void);
 
 
-#ifdef USE_BARO_MS5534A
+#if USE_BARO_MS5534A
 
 #include "baro_MS5534A.h"
 
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
deleted file mode 100644
index 9d3c00ee84..0000000000
--- a/sw/airborne/ap_downlink.h
+++ /dev/null
@@ -1,251 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2006- Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file ap_downlink.h
- *  \brief Set of macros defining the periodic telemetry messages of AP process
- *
- * The PeriodicSendAp() macro is generated from the telemetry description
- * (named in conf.xml, usually in conf/telemetry directory). This macro
- * is a sequence of calls to PERIODIC_SEND_message() which have to be defined
- * in the present file.
- *
- */
-
-#ifndef AP_DOWNLINK_H
-#define AP_DOWNLINK_H
-
-#include <inttypes.h>
-
-#include "generated/airframe.h"
-
-#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
-#include "downlink.h"
-
-#include "messages.h"
-#include "generated/periodic.h"
-
-//#include "generated/modules.h"
-
-#if defined DOWNLINK
-#define Downlink(x) x
-#else
-#define Downlink(x) {}
-#endif
-
-
-#define PERIODIC_SEND_ALIVE(_chan)  DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM);
-
-#define PERIODIC_SEND_BAT(_chan) {                      \
-    int16_t amps = (int16_t) (current/10);				\
-    Downlink({ int16_t e = energy;                      \
-        DOWNLINK_SEND_BAT(_chan,                        \
-                          &v_ctl_throttle_slewed,       \
-                          &vsupply,                     \
-                          &amps,                        \
-                          &estimator_flight_time,       \
-                          &kill_throttle,				\
-                          &block_time,					\
-                          &stage_time,					\
-                          &e);                          \
-      });                                               \
-  }
-
-#ifdef MCU_SPI_LINK
-#define PERIODIC_SEND_DEBUG_MCU_LINK(_chan) DOWNLINK_SEND_DEBUG_MCU_LINK(_chan, &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt);
-#else
-#define PERIODIC_SEND_DEBUG_MCU_LINK(_chan) {}
-#endif
-
-
-#define PERIODIC_SEND_DOWNLINK(_chan) { \
-  static uint16_t last; \
-  uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_DefaultChannel_0; \
-  last = downlink_nb_bytes; \
-  DOWNLINK_SEND_DOWNLINK(_chan, &downlink_nb_ovrn, &rate, &downlink_nb_msgs); \
-}
-
-
-#define PERIODIC_SEND_ATTITUDE(_chan) Downlink({ \
-      DOWNLINK_SEND_ATTITUDE(_chan, &estimator_phi, &estimator_psi, &estimator_theta); \
-})
-
-#define PERIODIC_SEND_PPRZ_MODE(_chan) DOWNLINK_SEND_PPRZ_MODE(_chan, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
-#define PERIODIC_SEND_DESIRED(_chan) DOWNLINK_SEND_DESIRED(_chan, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint);
-
-#ifdef USE_INFRARED
-#define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_chan, &mde, &contrast); }
-#elif defined USE_IMU && defined USE_AHRS
-#define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs_timeout_counter > 10) mde = 5; uint16_t val = 0; DOWNLINK_SEND_STATE_FILTER_STATUS(_chan, &mde, &val); }
-#else
-#define PERIODIC_SEND_STATE_FILTER_STATUS(_chan) {}
-#endif
-
-#define PERIODIC_SEND_NAVIGATION_REF(_chan)  DOWNLINK_SEND_NAVIGATION_REF(_chan, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
-
-
-#define DownlinkSendWp(_chan, i) {	     \
-  float x = nav_utm_east0 +  waypoints[i].x; \
-  float y = nav_utm_north0 + waypoints[i].y; \
-  DOWNLINK_SEND_WP_MOVED(_chan, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \
-}
-
-
-#define PERIODIC_SEND_WP_MOVED(_chan) { \
-  static uint8_t i; \
-  i++; if (i >= nb_waypoint) i = 0; \
-  DownlinkSendWp(_chan, i);	    \
-}
-
-#ifdef RADIO_CONTROL_SETTINGS
-#define PERIODIC_SEND_SETTINGS(_chan) if (!RcSettingsOff()) DOWNLINK_SEND_SETTINGS(_chan, &slider_1_val, &slider_2_val);
-#else
-#define PERIODIC_SEND_SETTINGS(_chan) {}
-#endif
-
-#if defined USE_INFRARED || USE_INFRARED_TELEMETRY
-#include "subsystems/sensors/infrared.h"
-#define PERIODIC_SEND_IR_SENSORS(_chan) DOWNLINK_SEND_IR_SENSORS(_chan, &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
-#else
-#define PERIODIC_SEND_IR_SENSORS(_chan) ;
-#endif
-
-#define PERIODIC_SEND_ADC(_chan) {}
-
-
-#define PERIODIC_SEND_CALIBRATION(_chan) DOWNLINK_SEND_CALIBRATION(_chan, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode)
-
-#define PERIODIC_SEND_CIRCLE(_chan) if (nav_in_circle) { DOWNLINK_SEND_CIRCLE(_chan, &nav_circle_x, &nav_circle_y, &nav_circle_radius); }
-
-#define PERIODIC_SEND_SEGMENT(_chan) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_chan, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
-
-#ifdef IMU_TYPE_H
-#  ifdef INS_MODULE_H
-#  include "modules/ins/ins_module.h"
-#    define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
-#    define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
-#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
-#    define PERIODIC_SEND_IMU_GYRO(_chan) { DOWNLINK_SEND_IMU_GYRO(_chan, &ins_p, &ins_q, &ins_r)}
-#    define PERIODIC_SEND_IMU_ACCEL(_chan) { DOWNLINK_SEND_IMU_ACCEL(_chan, &ins_ax, &ins_ay, &ins_az)}
-#    define PERIODIC_SEND_IMU_MAG(_chan) { DOWNLINK_SEND_IMU_MAG(_chan, &ins_mx, &ins_my, &ins_mz)}
-#  else
-#    include "subsystems/imu.h"
-#    define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
-#    define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
-#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) { DOWNLINK_SEND_IMU_MAG_RAW(_chan, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
-#    define PERIODIC_SEND_IMU_ACCEL(_chan) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_chan, &accel_float.x, &accel_float.y, &accel_float.z)}
-#    define PERIODIC_SEND_IMU_GYRO(_chan) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_chan, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
-#    ifdef USE_MAGNETOMETER
-#      define PERIODIC_SEND_IMU_MAG(_chan) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_chan, &mag_float.x, &mag_float.y, &mag_float.z)}
-#    else
-#      define PERIODIC_SEND_IMU_MAG(_chan) {}
-#    endif
-#  endif
-#else
-#    define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
-#    define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
-#    define PERIODIC_SEND_IMU_MAG_RAW(_chan) {}
-#    define PERIODIC_SEND_IMU_ACCEL(_chan) {}
-#    define PERIODIC_SEND_IMU_GYRO(_chan) {}
-#    define PERIODIC_SEND_IMU_MAG(_chan) {}
-#endif
-
-#ifdef IMU_ANALOG
-#define PERIODIC_SEND_IMU(_chan) { int16_t dummy = 42; DOWNLINK_SEND_IMU(_chan, &(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); }
-#else
-#define PERIODIC_SEND_IMU(_chan) {}
-#endif
-
-#define PERIODIC_SEND_ESTIMATOR(_chan) DOWNLINK_SEND_ESTIMATOR(_chan, &estimator_z, &estimator_z_dot)
-
-#define SEND_NAVIGATION(_chan) Downlink({ uint8_t _circle_count = NavCircleCount(); DOWNLINK_SEND_NAVIGATION(_chan, &nav_block, &nav_stage, &estimator_x, &estimator_y, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count);})
-
-#define PERIODIC_SEND_NAVIGATION(_chan) SEND_NAVIGATION(_chan)
-
-
-#if defined CAM || defined MOBILE_CAM
-#define SEND_CAM(_chan) Downlink({ int16_t x = cam_target_x; int16_t y = cam_target_y; int16_t phi = DegOfRad(cam_phi_c); int16_t theta = DegOfRad(cam_theta_c); DOWNLINK_SEND_CAM(_chan, &phi, &theta, &x, &y);})
-#define PERIODIC_SEND_CAM_POINT(_chan) DOWNLINK_SEND_CAM_POINT(_chan, &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon)
-#else
-#define SEND_CAM(_chan) {}
-#define PERIODIC_SEND_CAM_POINT(_chan) {}
-#endif
-
-#define PERIODIC_SEND_DL_VALUE(_chan) PeriodicSendDlValue(_chan) /** generated from the xml settings config in conf/settings */
-
-#define PERIODIC_SEND_SURVEY(_chan) { \
-  if (nav_survey_active) \
-    DOWNLINK_SEND_SURVEY(_chan, &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); \
-  }
-
-#define PERIODIC_SEND_RANGEFINDER(_chan) DOWNLINK_SEND_RANGEFINDER(_chan, &rangemeter, &ctl_grz_z_dot, &ctl_grz_z_dot_sum_err, &ctl_grz_z_dot_setpoint, &ctl_grz_z_sum_err, &ctl_grz_z_setpoint, &flying)
-
-#define PERIODIC_SEND_TUNE_ROLL(_chan) DOWNLINK_SEND_TUNE_ROLL(_chan, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint);
-
-#if defined USE_GPS || defined SITL || defined USE_GPS_XSENS
-#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
-#endif
-
-#define PERIODIC_SEND_GPS(_chan) {                                      \
-    static uint8_t i;                                                   \
-    int16_t climb = -gps.ned_vel.z;                                     \
-    int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));             \
-    DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.hmsl, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
-    if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;                                    \
-    if (i >= gps.nb_channels * 2) i = 0;                                    \
-    if (i < gps.nb_channels && gps.svinfos[i].cno > 0) { \
-      DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
-    }                                                                   \
-    i++;                                                                \
-}
-
-#ifdef USE_BARO_MS5534A
-//#include "baro_MS5534A.h"
-#define PERIODIC_SEND_BARO_MS5534A(_chan) DOWNLINK_SEND_BARO_MS5534A(_chan, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z)
-#else
-#define PERIODIC_SEND_BARO_MS5534A(_chan) {}
-#endif
-
-#ifdef USE_BARO_SCP
-#include "baro_scp.h"
-#define PERIODIC_SEND_SCP_STATUS(_chan) DOWNLINK_SEND_SCP_STATUS(_chan, &baro_scp_pressure, &baro_scp_temperature)
-#else
-#define PERIODIC_SEND_SCP_STATUS(_chan) {}
-#endif
-
-#ifdef MEASURE_AIRSPEED
-#define PERIODIC_SEND_AIRSPEED(_chan) DOWNLINK_SEND_AIRSPEED (_chan, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed)
-#elif defined USE_AIRSPEED
-#define PERIODIC_SEND_AIRSPEED(_chan) DOWNLINK_SEND_AIRSPEED (_chan, &estimator_airspeed, &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint)
-#else
-#define PERIODIC_SEND_AIRSPEED(_chan) {}
-#endif
-
-#define PERIODIC_SEND_ENERGY(_chan) Downlink({ const int16_t e = energy; const float vsup = ((float)vsupply) / 10.0f; const float curs = ((float) current)/1000.0f;  const float power = vsup * curs; DOWNLINK_SEND_ENERGY(_chan, &vsup, &curs, &e, &power); })
-
-
-#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
-#define PERIODIC_SEND_H_CTL_A(_chan) DOWNLINK_SEND_H_CTL_A(_chan, &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle)
-
-
-#endif /* AP_DOWNLINK_H */
diff --git a/sw/airborne/arch/avr/ant_v2x.c b/sw/airborne/arch/avr/ant_v2x.c
index 4dbc56a1ef..83a45e3ebf 100644
--- a/sw/airborne/arch/avr/ant_v2x.c
+++ b/sw/airborne/arch/avr/ant_v2x.c
@@ -12,7 +12,7 @@
 
 #include "ant_spi.h"
 
-//#include "downlink.h"
+//#include "subsystems/datalink/downlink.h"
 
 volatile bool_t ant_v2x_data_available;
 struct  Ant_V2xData ant_v2x_data;
diff --git a/sw/airborne/arch/avr/modem_hw.h b/sw/airborne/arch/avr/modem_hw.h
deleted file mode 100644
index 1fdc482f73..0000000000
--- a/sw/airborne/arch/avr/modem_hw.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file modem_hw.h
- *  \brief AVR CMX469 low level functions
- *
- */
-
-
-#ifndef MODEM_HW_H
-#define MODEM_HW_H
-
-#include <avr/io.h>
-#include "std.h"
-
-#define MODEM_CHECK_RUNNING() { \
-  if (!(EIMSK & _BV(MODEM_CLK_INT))) { \
-    MODEM_LOAD_NEXT_BYTE() \
-    sbi(EIFR, INTF0); \
-    sbi(EIMSK, MODEM_CLK_INT); \
-  } \
-}
-
-
-#define MODEM_TX_PORT   PORTD
-#define MODEM_TX_DDR	DDRD
-#define MODEM_TX_EN     7
-#define MODEM_TX_DATA   6
-
-#ifdef CTL_BRD_V1_2
-#define MODEM_CLK_DDR   DDRD
-#define MODEM_CLK_PORT  PORTD
-#define MODEM_CLK       0
-#define MODEM_CLK_INT   INT0
-#define MODEM_CLK_INT_REG EICRA
-#define MODEM_CLK_INT_CFG _BV(ISC01)
-#define MODEM_CLK_INT_SIG SIG_INTERRUPT0
-
-#define MODEM_OSC_DDR   DDRB
-#define MODEM_OSC_PORT  PORTB
-#define MODEM_OSC       4
-#endif /* CTL_BRD_V1_2 */
-
-#ifdef CTL_BRD_V1_2_1
-#define MODEM_CLK_DDR   DDRE
-#define MODEM_CLK_PORT  PORTE
-#define MODEM_CLK       4
-#define MODEM_CLK_INT   INT4
-#define MODEM_CLK_INT_REG EICRB
-#define MODEM_CLK_INT_CFG _BV(ISC41)
-#define MODEM_CLK_INT_SIG SIG_INTERRUPT4
-#define MODEM_OSC_DDR   DDRB
-#define MODEM_OSC_PORT  PORTB
-#define MODEM_OSC       4
-#endif /* CTL_BRD_V1_2_1 */
-
-static inline void modem_init ( void ) {
-  /* setup TIMER0 to generate a 4MHz clock */
-  MODEM_OSC_DDR |= _BV(MODEM_OSC);
-  OCR0 = 1; /* 4MhZ */
-  TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
-
-  /* setup TX_EN and TX_DATA pin as output */
-  MODEM_TX_DDR |= _BV(MODEM_TX_EN) | _BV(MODEM_TX_DATA);
-  /* data idles hight */
-  sbi(MODEM_TX_PORT, MODEM_TX_DATA);
-  /* enable transmitter */
-  cbi(MODEM_TX_PORT, MODEM_TX_EN);
-  /* set interrupt on failing edge of clock */
-  MODEM_CLK_INT_REG |=  MODEM_CLK_INT_CFG;
-}
-
-
-
-#endif
diff --git a/sw/airborne/arch/avr/modem_hw.c b/sw/airborne/arch/avr/subsystems/datalink/audio_telemetry_hw.c
similarity index 77%
rename from sw/airborne/arch/avr/modem_hw.c
rename to sw/airborne/arch/avr/subsystems/datalink/audio_telemetry_hw.c
index 9e88010795..d547aa58a6 100644
--- a/sw/airborne/arch/avr/modem_hw.c
+++ b/sw/airborne/arch/avr/subsystems/datalink/audio_telemetry_hw.c
@@ -22,7 +22,7 @@
  *
  */
 
-/** \file modem_hw.c
+/** \file audio_telemetry_hw.c
  *  \brief Handling of a CMX 469 on avr mega128 architecture
  */
 
@@ -33,9 +33,9 @@
 #endif
 
 #include <avr/interrupt.h>
-#include "modem.h"
+#include "audio_telemetry.h"
 
-uint8_t modem_nb_ovrn;
+uint8_t audio_telemetry_nb_ovrn;
 
 uint8_t           tx_head;
 volatile uint8_t  tx_tail;
@@ -45,21 +45,21 @@ uint8_t    tx_byte;
 uint8_t    tx_byte_idx;
 
 
-SIGNAL( MODEM_CLK_INT_SIG ) {
+SIGNAL( AUDIO_TELEMETRY_CLK_INT_SIG ) {
   /*  start bit         */
   if (tx_byte_idx == 0)
-    cbi(MODEM_TX_PORT, MODEM_TX_DATA);
+    cbi(AUDIO_TELEMETRY_TX_PORT, AUDIO_TELEMETRY_TX_DATA);
   /* 8 data bits        */
   else if (tx_byte_idx < 9) {
     if (tx_byte & 0x01)
-      sbi(MODEM_TX_PORT, MODEM_TX_DATA);
+      sbi(AUDIO_TELEMETRY_TX_PORT, AUDIO_TELEMETRY_TX_DATA);
     else
-      cbi(MODEM_TX_PORT, MODEM_TX_DATA);
+      cbi(AUDIO_TELEMETRY_TX_PORT, AUDIO_TELEMETRY_TX_DATA);
     tx_byte >>= 1;
   }
   /* stop_bit           */
   else {
-    sbi(MODEM_TX_PORT, MODEM_TX_DATA);
+    sbi(AUDIO_TELEMETRY_TX_PORT, AUDIO_TELEMETRY_TX_DATA);
   }
   tx_byte_idx++;
   /* next byte          */
@@ -67,10 +67,10 @@ SIGNAL( MODEM_CLK_INT_SIG ) {
     /*  if we have nothing left to transmit */
     if( tx_head == tx_tail ) {
       /* disable clock interrupt            */
-      cbi( EIMSK, MODEM_CLK_INT );
+      cbi( EIMSK, AUDIO_TELEMETRY_CLK_INT );
     } else {
       /* else load next byte                  */
-      MODEM_LOAD_NEXT_BYTE();
+      AUDIO_TELEMETRY_LOAD_NEXT_BYTE();
     }
   }
 }
diff --git a/sw/airborne/arch/avr/subsystems/datalink/audio_telemetry_hw.h b/sw/airborne/arch/avr/subsystems/datalink/audio_telemetry_hw.h
new file mode 100644
index 0000000000..13eadafc57
--- /dev/null
+++ b/sw/airborne/arch/avr/subsystems/datalink/audio_telemetry_hw.h
@@ -0,0 +1,96 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file audio_telemetry_hw.h
+ *  \brief AVR CMX469 low level functions
+ *
+ */
+
+
+#ifndef AUDIO_TELEMETRY_HW_H
+#define AUDIO_TELEMETRY_HW_H
+
+#include <avr/io.h>
+#include "std.h"
+
+#define AUDIO_TELEMETRY_CHECK_RUNNING() { \
+  if (!(EIMSK & _BV(AUDIO_TELEMETRY_CLK_INT))) { \
+    AUDIO_TELEMETRY_LOAD_NEXT_BYTE() \
+    sbi(EIFR, INTF0); \
+    sbi(EIMSK, AUDIO_TELEMETRY_CLK_INT); \
+  } \
+}
+
+
+#define AUDIO_TELEMETRY_TX_PORT   PORTD
+#define AUDIO_TELEMETRY_TX_DDR	DDRD
+#define AUDIO_TELEMETRY_TX_EN     7
+#define AUDIO_TELEMETRY_TX_DATA   6
+
+#ifdef CTL_BRD_V1_2
+#define AUDIO_TELEMETRY_CLK_DDR   DDRD
+#define AUDIO_TELEMETRY_CLK_PORT  PORTD
+#define AUDIO_TELEMETRY_CLK       0
+#define AUDIO_TELEMETRY_CLK_INT   INT0
+#define AUDIO_TELEMETRY_CLK_INT_REG EICRA
+#define AUDIO_TELEMETRY_CLK_INT_CFG _BV(ISC01)
+#define AUDIO_TELEMETRY_CLK_INT_SIG SIG_INTERRUPT0
+
+#define AUDIO_TELEMETRY_OSC_DDR   DDRB
+#define AUDIO_TELEMETRY_OSC_PORT  PORTB
+#define AUDIO_TELEMETRY_OSC       4
+#endif /* CTL_BRD_V1_2 */
+
+#ifdef CTL_BRD_V1_2_1
+#define AUDIO_TELEMETRY_CLK_DDR   DDRE
+#define AUDIO_TELEMETRY_CLK_PORT  PORTE
+#define AUDIO_TELEMETRY_CLK       4
+#define AUDIO_TELEMETRY_CLK_INT   INT4
+#define AUDIO_TELEMETRY_CLK_INT_REG EICRB
+#define AUDIO_TELEMETRY_CLK_INT_CFG _BV(ISC41)
+#define AUDIO_TELEMETRY_CLK_INT_SIG SIG_INTERRUPT4
+#define AUDIO_TELEMETRY_OSC_DDR   DDRB
+#define AUDIO_TELEMETRY_OSC_PORT  PORTB
+#define AUDIO_TELEMETRY_OSC       4
+#endif /* CTL_BRD_V1_2_1 */
+
+static inline void audio_telemetry_init ( void ) {
+  /* setup TIMER0 to generate a 4MHz clock */
+  AUDIO_TELEMETRY_OSC_DDR |= _BV(AUDIO_TELEMETRY_OSC);
+  OCR0 = 1; /* 4MhZ */
+  TCCR0 = _BV(WGM01) | _BV(COM00) | _BV(CS00);
+
+  /* setup TX_EN and TX_DATA pin as output */
+  AUDIO_TELEMETRY_TX_DDR |= _BV(AUDIO_TELEMETRY_TX_EN) | _BV(AUDIO_TELEMETRY_TX_DATA);
+  /* data idles hight */
+  sbi(AUDIO_TELEMETRY_TX_PORT, AUDIO_TELEMETRY_TX_DATA);
+  /* enable transmitter */
+  cbi(AUDIO_TELEMETRY_TX_PORT, AUDIO_TELEMETRY_TX_EN);
+  /* set interrupt on failing edge of clock */
+  AUDIO_TELEMETRY_CLK_INT_REG |=  AUDIO_TELEMETRY_CLK_INT_CFG;
+}
+
+
+
+#endif
diff --git a/sw/airborne/arch/lpc21/servos_csc.c b/sw/airborne/arch/lpc21/servos_csc.c
deleted file mode 100644
index 091533d25d..0000000000
--- a/sw/airborne/arch/lpc21/servos_csc.c
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "std.h"
-#include "actuators.h"
-#include "servos_csc.h"
-#include "csc_msg_def.h"
-
-/* 40 Hz */
-#ifndef SERVOS_PERIOD
-#define SERVOS_PERIOD SERVOS_TICS_OF_USEC(25000)
-#endif
-
-void actuators_init ( void )
-{
-
-}
diff --git a/sw/airborne/arch/lpc21/servos_csc.h b/sw/airborne/arch/lpc21/servos_csc.h
deleted file mode 100644
index f0842a2f2c..0000000000
--- a/sw/airborne/arch/lpc21/servos_csc.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef SERVOS_CSC_H
-#define SERVOS_CSC_H
-
-#include "LPC21xx.h"
-#include "generated/airframe.h"
-#include "actuators.h"
-#include "sys_time.h"
-
-#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s)
-#define ChopServo(x,a,b) Chop(x, a, b)
-#define SERVO_COUNT 4
-
-#define Actuator(i) actuators[i]
-
-static inline void ActuatorsCommit(void)
-{
-
-}
-
-#endif /* SERVOS_CSC_H */
diff --git a/sw/airborne/arch/lpc21/modem_hw.c b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c
similarity index 65%
rename from sw/airborne/arch/lpc21/modem_hw.c
rename to sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c
index 3063b132a8..42796a4a85 100644
--- a/sw/airborne/arch/lpc21/modem_hw.c
+++ b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c
@@ -1,8 +1,8 @@
-#include "modem.h"
+#include "audio_telemetry.h"
 #include "types.h"
 #include "armVIC.h"
 
-uint8_t           modem_nb_ovrn;
+uint8_t           audio_telemetry_nb_ovrn;
 uint8_t           tx_head;
 volatile uint8_t  tx_tail;
 uint8_t           tx_buf[ TX_BUF_SIZE ];
@@ -14,7 +14,7 @@ uint8_t tx_bit_idx;
 #define NB_STATE 2
 #define NB_PHASE 2
 
-static const uint16_t modem_sample[NB_STATE][NB_PHASE][SAMPLES_PER_PERIOD] =
+static const uint16_t audio_telemetry_sample[NB_STATE][NB_PHASE][SAMPLES_PER_PERIOD] =
   {
     {{512, 1023, 512, 1},
      {512 ,1 , 512 , 1023}},
@@ -22,8 +22,8 @@ static const uint16_t modem_sample[NB_STATE][NB_PHASE][SAMPLES_PER_PERIOD] =
      {512 ,150 ,1 , 150}}
   };
 
-static uint8_t modem_sample_idx = 0;
-static uint8_t modem_phase = 0;
+static uint8_t audio_telemetry_sample_idx = 0;
+static uint8_t audio_telemetry_phase = 0;
 
 
 static inline uint8_t get_next_bit( void ) {
@@ -61,14 +61,14 @@ static inline uint8_t get_next_bit( void ) {
 
 void TIMER1_ISR ( void ) {
   ISR_ENTRY();
-  static uint8_t modem_bit;
+  static uint8_t audio_telemetry_bit;
 
-  DACR = modem_sample[modem_bit][modem_phase][modem_sample_idx] << 6;
-  modem_sample_idx++;
-  if (modem_sample_idx == SAMPLES_PER_PERIOD) {
-    modem_sample_idx = 0;
-    modem_phase ^= modem_bit;
-    modem_bit =  get_next_bit();
+  DACR = audio_telemetry_sample[audio_telemetry_bit][audio_telemetry_phase][audio_telemetry_sample_idx] << 6;
+  audio_telemetry_sample_idx++;
+  if (audio_telemetry_sample_idx == SAMPLES_PER_PERIOD) {
+    audio_telemetry_sample_idx = 0;
+    audio_telemetry_phase ^= audio_telemetry_bit;
+    audio_telemetry_bit =  get_next_bit();
   }
 
   /* clear interrupt */
diff --git a/sw/airborne/arch/lpc21/modem_hw.h b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.h
similarity index 86%
rename from sw/airborne/arch/lpc21/modem_hw.h
rename to sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.h
index 2c44d3e863..2b408ae3a8 100644
--- a/sw/airborne/arch/lpc21/modem_hw.h
+++ b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.h
@@ -1,5 +1,5 @@
-#ifndef MODEM_HW_H
-#define MODEM_HW_H
+#ifndef AUDIO_TELEMETRY_HW_H
+#define AUDIO_TELEMETRY_HW_H
 
 #include "LPC21xx.h"
 #include BOARD_CONFIG
@@ -28,7 +28,7 @@ void TIMER1_ISR ( void ) __attribute__((naked));
 #define SAMPLES_PER_PERIOD 4
 #define SAMPLE_PERIOD (PCLK/4762/SAMPLES_PER_PERIOD/T1_PCLK_DIV)
 
-static inline void modem_init ( void ) {
+static inline void audio_telemetry_init ( void ) {
     /* turn on DAC pins */
   PINSEL1 &= 1 << 19;
   PINSEL1 |= ~(1 << 18);
@@ -53,7 +53,7 @@ static inline void modem_init ( void ) {
 }
 
 
-#define MODEM_CHECK_RUNNING() {}
+#define AUDIO_TELEMETRY_CHECK_RUNNING() {}
 
 
-#endif /* MODEM_HW_H */
+#endif /* AUDIO_TELEMETRY_HW_H */
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
index 0b98aee11e..6cb17e1fb6 100644
--- a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
@@ -96,6 +96,9 @@ void imu_periodic(void) {
 #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
   RunOnceEvery(10, { ami601_read(); });
 #endif
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
+  RunOnceEvery(5,Hmc58xxPeriodic());
+#endif
 
 }
 
diff --git a/sw/airborne/arch/lpc21/sys_time_hw.h b/sw/airborne/arch/lpc21/sys_time_hw.h
index 9bbf444cce..f4f9637001 100644
--- a/sw/airborne/arch/lpc21/sys_time_hw.h
+++ b/sw/airborne/arch/lpc21/sys_time_hw.h
@@ -144,9 +144,6 @@ static inline bool_t sys_time_periodic( void ) {
     if (cpu_time_ticks > TIME_TICKS_PER_SEC) {
       cpu_time_ticks -= TIME_TICKS_PER_SEC;
       cpu_time_sec++;
-#ifdef TIME_LED
-      LED_TOGGLE(TIME_LED)
-#endif
 #ifdef SYS_TIME_LED
       LED_TOGGLE(SYS_TIME_LED)
 #endif
diff --git a/sw/airborne/arch/lpc21/usb_tunnel.c b/sw/airborne/arch/lpc21/usb_tunnel.c
index c2acef1e61..035cebda84 100644
--- a/sw/airborne/arch/lpc21/usb_tunnel.c
+++ b/sw/airborne/arch/lpc21/usb_tunnel.c
@@ -67,7 +67,7 @@ int main( void ) {
       inc = Uart0Getch();
       VCOM_putchar(inc);
     }
-    if (VCOM_check_available() && uart0_check_free_space(1)) {
+    if (VCOM_check_available() && Uart0CheckFreeSpace(1)) {
       LED_ON(2);
       tx_time = T0TC;
       inc = VCOM_getchar();
@@ -84,7 +84,7 @@ int main( void ) {
       inc = Uart1Getch();
       VCOM_putchar(inc);
     }
-    if (VCOM_check_available() && uart1_check_free_space(1)) {
+    if (VCOM_check_available() && Uart1CheckFreeSpace(1)) {
       LED_ON(2);
       tx_time = T0TC;
       inc = VCOM_getchar();
diff --git a/sw/airborne/arch/sim/baro_MS5534A.h b/sw/airborne/arch/sim/baro_MS5534A.h
index e2330decda..8efed9e752 100644
--- a/sw/airborne/arch/sim/baro_MS5534A.h
+++ b/sw/airborne/arch/sim/baro_MS5534A.h
@@ -32,7 +32,7 @@
 
 #include "std.h"
 
-#ifdef USE_BARO_MS5534A
+#if USE_BARO_MS5534A
 
 extern bool_t spi_message_received;
 extern bool_t baro_MS5534A_available;
diff --git a/sw/airborne/arch/sim/ivy_transport.h b/sw/airborne/arch/sim/ivy_transport.h
index 1926187d9e..8d416e89e9 100644
--- a/sw/airborne/arch/sim/ivy_transport.h
+++ b/sw/airborne/arch/sim/ivy_transport.h
@@ -4,45 +4,45 @@
 extern char ivy_buf[];
 extern char* ivy_p;
 
-#define IvyTransportCheckFreeSpace(_) TRUE
+#define IvyTransportCheckFreeSpace(_dev,_) TRUE
 
-#define IvyTransportSizeOf(x) (x)
+#define IvyTransportSizeOf(_dev, x) (x)
 
-#define IvyTransportHeader(len) ivy_p=ivy_buf;
+#define IvyTransportHeader(_dev,len) ivy_p=ivy_buf;
 
-#define IvyTransportTrailer() { *ivy_p = '\0'; IvySendMsg("%s",ivy_buf); }
+#define IvyTransportTrailer(_dev) { *ivy_p = '\0'; IvySendMsg("%s",ivy_buf); }
 
-#define IvyTransportPutUint8(x) { ivy_p += sprintf(ivy_p, "%u ", x); }
-#define IvyTransportPutNamedUint8(_name, _x) { ivy_p += sprintf(ivy_p, "%s ", _name); }
+#define IvyTransportPutUint8(_dev,x) { ivy_p += sprintf(ivy_p, "%u ", x); }
+#define IvyTransportPutNamedUint8(_dev,_name, _x) { ivy_p += sprintf(ivy_p, "%s ", _name); }
 
 #define Space() ivy_p += sprintf(ivy_p, " ");
 #define Comma() ivy_p += sprintf(ivy_p, ",");
 
-#define IvyTransportPutUintByAddr(x) ivy_p += sprintf(ivy_p, "%u", *x);
-#define IvyTransportPutUint8ByAddr(x) IvyTransportPutUintByAddr(x) Space()
-#define IvyTransportPutUint16ByAddr(x) IvyTransportPutUintByAddr(x) Space()
-#define IvyTransportPutUint32ByAddr(x) IvyTransportPutUintByAddr(x) Space()
+#define IvyTransportPutUintByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%u", *x);
+#define IvyTransportPutUint8ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space()
+#define IvyTransportPutUint16ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space()
+#define IvyTransportPutUint32ByAddr(_dev,x) IvyTransportPutUintByAddr(_dev,x) Space()
 
-#define IvyTransportPutIntByAddr(x) ivy_p += sprintf(ivy_p, "%d", *x);
-#define IvyTransportPutInt8ByAddr(x) IvyTransportPutIntByAddr(x) Space()
-#define IvyTransportPutInt16ByAddr(x) IvyTransportPutIntByAddr(x) Space()
-#define IvyTransportPutInt32ByAddr(x) IvyTransportPutIntByAddr(x) Space()
+#define IvyTransportPutIntByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%d", *x);
+#define IvyTransportPutInt8ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space()
+#define IvyTransportPutInt16ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space()
+#define IvyTransportPutInt32ByAddr(_dev,x) IvyTransportPutIntByAddr(_dev,x) Space()
 
-#define IvyTransportPutOneFloatByAddr(x) ivy_p += sprintf(ivy_p, "%f", *x);
-#define IvyTransportPutFloatByAddr(x) IvyTransportPutOneFloatByAddr(x) Space()
-#define IvyTransportPutDoubleByAddr(x) IvyTransportPutOneFloatByAddr(x) Space()
+#define IvyTransportPutOneFloatByAddr(_dev,x) ivy_p += sprintf(ivy_p, "%f", *x);
+#define IvyTransportPutFloatByAddr(_dev,x) IvyTransportPutOneFloatByAddr(_dev,x) Space()
+#define IvyTransportPutDoubleByAddr(_dev,x) IvyTransportPutOneFloatByAddr(_dev,x) Space()
 
-#define IvyTransportPutArray(_put, _n, _x) { \
+#define IvyTransportPutArray(_dev,_put, _n, _x) { \
   int __i; \
   for(__i = 0; __i < _n; __i++) { \
-    _put(&_x[__i]); \
+    _put(_dev,&_x[__i]); \
     Comma(); \
   } \
 }
 
-#define IvyTransportPutUint8Array(_n, _x) IvyTransportPutArray(IvyTransportPutUintByAddr, _n, _x)
-#define IvyTransportPutInt16Array(_n, _x) IvyTransportPutArray(IvyTransportPutIntByAddr, _n, _x)
-#define IvyTransportPutUint16Array(_n, _x) IvyTransportPutArray(IvyTransportPutUintByAddr, _n, _x)
-#define IvyTransportPutUint32Array(_n, _x) IvyTransportPutArray(IvyTransportPutUintByAddr, _n, _x)
-#define IvyTransportPutFloatArray(_n, _x) IvyTransportPutArray(IvyTransportPutOneFloatByAddr, _n, _x)
-#define IvyTransportPutDoubleArray(_n, _x) IvyTransportPutFloatArray(_n, _x)
+#define IvyTransportPutUint8Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
+#define IvyTransportPutInt16Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutIntByAddr, _n, _x)
+#define IvyTransportPutUint16Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
+#define IvyTransportPutUint32Array(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutUintByAddr, _n, _x)
+#define IvyTransportPutFloatArray(_dev,_n, _x) IvyTransportPutArray(_dev,IvyTransportPutOneFloatByAddr, _n, _x)
+#define IvyTransportPutDoubleArray(_dev,_n, _x) IvyTransportPutFloatArray(_dev,_n, _x)
diff --git a/sw/airborne/arch/sim/jsbsim_hw.c b/sw/airborne/arch/sim/jsbsim_hw.c
index d188b1609a..efa47bf026 100644
--- a/sw/airborne/arch/sim/jsbsim_hw.c
+++ b/sw/airborne/arch/sim/jsbsim_hw.c
@@ -16,7 +16,7 @@
 //uint8_t vertical_mode;
 //uint8_t inflight_calib_mode;
 //bool_t rc_event_1, rc_event_2;
-uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
+uint8_t gps_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
 //float alt_roll_pgain;
 //float roll_rate_pgain;
 //bool_t gpio1_status;
diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h
index 6b291b4060..1202dce646 100644
--- a/sw/airborne/arch/sim/jsbsim_hw.h
+++ b/sw/airborne/arch/sim/jsbsim_hw.h
@@ -45,7 +45,7 @@
 #include "firmwares/fixedwing/main_ap.h"
 #include "ap_downlink.h"
 #include "sim_uart.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 
 
 void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time);
diff --git a/sw/airborne/arch/sim/jsbsim_ir.c b/sw/airborne/arch/sim/jsbsim_ir.c
index 410583b974..f850dd9574 100644
--- a/sw/airborne/arch/sim/jsbsim_ir.c
+++ b/sw/airborne/arch/sim/jsbsim_ir.c
@@ -18,7 +18,7 @@
 
 void set_ir(double roll __attribute__ ((unused)), double pitch __attribute__ ((unused))) {
   // INFRARED_TELEMETRY : Stupid hack to use with modules
-#if defined USE_INFRARED || USE_INFRARED_TELEMETRY
+#if USE_INFRARED || USE_INFRARED_TELEMETRY
   double ir_contrast = 150; //FIXME
   double roll_sensor = roll + JSBSIM_IR_ROLL_NEUTRAL; // ir_roll_neutral;
   double pitch_sensor = pitch + JSBSIM_IR_PITCH_NEUTRAL; // ir_pitch_neutral;
diff --git a/sw/airborne/arch/sim/jsbsim_transport.c b/sw/airborne/arch/sim/jsbsim_transport.c
index 9e8bf97d86..24137fec0d 100644
--- a/sw/airborne/arch/sim/jsbsim_transport.c
+++ b/sw/airborne/arch/sim/jsbsim_transport.c
@@ -9,7 +9,7 @@
 #define MOfCm(_x) (((float)(_x))/100.)
 
 void parse_dl_ping(char* argv[] __attribute__ ((unused))) {
-  DOWNLINK_SEND_PONG(DefaultChannel);
+  DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
 }
 
 void parse_dl_acinfo(char* argv[] __attribute__ ((unused))) {
@@ -30,13 +30,13 @@ void parse_dl_setting(char* argv[]) {
   uint8_t index = atoi(argv[2]);
   float value = atof(argv[3]);
   DlSetting(index, value);
-  DOWNLINK_SEND_DL_VALUE(DefaultChannel,&index, &value);
+  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice,&index, &value);
 }
 
 void parse_dl_get_setting(char* argv[]) {
   uint8_t index = atoi(argv[2]);
   float value = settings_get_value(index);
-  DOWNLINK_SEND_DL_VALUE(DefaultChannel,&index, &value);
+  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice,&index, &value);
 }
 
 void parse_dl_block(char* argv[]) {
@@ -61,6 +61,6 @@ void parse_dl_move_wp(char* argv[]) {
      coordinates */
   utm.east = waypoints[wp_id].x + nav_utm_east0;
   utm.north = waypoints[wp_id].y + nav_utm_north0;
-  DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
+  DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
 }
 
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index c5e1a652db..be8f4bd1c2 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -21,7 +21,7 @@
 #include "firmwares/fixedwing/main_ap.h"
 #include "ap_downlink.h"
 #include "sim_uart.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "generated/flight_plan.h"
 
 #include <caml/mlvalues.h>
@@ -35,7 +35,7 @@ uint8_t vertical_mode;
 uint8_t inflight_calib_mode;
 bool_t rc_event_1, rc_event_2;
 bool_t launch;
-uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
+uint8_t gps_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err;
 float alt_roll_pgain;
 float roll_rate_pgain;
 bool_t gpio1_status;
diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c
index 13ef9b84cd..c329f6b76e 100644
--- a/sw/airborne/arch/sim/sim_gps.c
+++ b/sw/airborne/arch/sim/sim_gps.c
@@ -42,6 +42,7 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
   gps.utm_pos.north = utm_f.north*100;
   gps.utm_pos.zone = nav_utm_zone0;
   x = y = z; /* Just to get rid of the "unused arg" warning */
+  y = x;     /* Just to get rid of the "unused arg" warning */
 #else // GPS_USE_LATLONG
   gps.utm_pos.east = Int_val(x);
   gps.utm_pos.north = Int_val(y);
diff --git a/sw/airborne/arch/sim/sim_ir.c b/sw/airborne/arch/sim/sim_ir.c
index 8b443ecf5f..59684001db 100644
--- a/sw/airborne/arch/sim/sim_ir.c
+++ b/sw/airborne/arch/sim/sim_ir.c
@@ -23,7 +23,7 @@ value set_ir_and_airspeed(
     value air_speed
     ) {
   // INFRARED_TELEMETRY : Stupid hack to use with modules
-#if defined  USE_INFRARED || USE_INFRARED_TELEMETRY
+#if USE_INFRARED || USE_INFRARED_TELEMETRY
   infrared.roll = Int_val(roll);
   infrared.pitch = Int_val(front);
   infrared.top = Int_val(top);
diff --git a/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.c b/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.c
index e680ebc675..deb32021cb 100644
--- a/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.c
+++ b/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.c
@@ -35,7 +35,7 @@ void imu_periodic(void) {
 
 }
 
-#ifdef USE_NPS
+#if USE_NPS
 #include "nps_sensors.h"
 
 void imu_feed_gyro_accel(void) {
diff --git a/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.h b/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.h
index 025acd33bf..2944460181 100644
--- a/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.h
+++ b/sw/airborne/arch/sim/subsystems/imu/imu_b2_arch.h
@@ -31,7 +31,7 @@
 
 extern int imu_overrun;
 
-#ifdef USE_NPS
+#if USE_NPS
 extern void imu_feed_gyro_accel(void);
 extern void imu_feed_mag(void);
 #endif
diff --git a/sw/airborne/arch/sim/subsystems/radio_control/ppm_arch.c b/sw/airborne/arch/sim/subsystems/radio_control/ppm_arch.c
index be462fc82b..164a296e45 100644
--- a/sw/airborne/arch/sim/subsystems/radio_control/ppm_arch.c
+++ b/sw/airborne/arch/sim/subsystems/radio_control/ppm_arch.c
@@ -28,7 +28,7 @@
 #include <inttypes.h>
 #include <caml/mlvalues.h>
 
-#ifdef USE_NPS
+#if USE_NPS
 #include "nps_radio_control.h"
 #endif
 
@@ -55,7 +55,7 @@ value send_ppm(value unit) {
   return unit;
 }
 
-#ifdef USE_NPS
+#if USE_NPS
 #define PPM_OF_NPS(_nps, _neutral, _min, _max)                          \
   ((_nps) >= 0 ? (_neutral) + (_nps) * ((_max)-(_neutral)) : (_neutral) + (_nps) * ((_neutral)- (_min)))
 
@@ -94,7 +94,7 @@ value send_ppm(value unit) {
   return unit;
 }
 
-#ifdef USE_NPS
+#if USE_NPS
 void radio_control_feed(void) {}
 #endif
 
diff --git a/sw/airborne/arch/sim/subsystems/radio_control/ppm_arch.h b/sw/airborne/arch/sim/subsystems/radio_control/ppm_arch.h
index c0f27a3b5e..5a6bcbe639 100644
--- a/sw/airborne/arch/sim/subsystems/radio_control/ppm_arch.h
+++ b/sw/airborne/arch/sim/subsystems/radio_control/ppm_arch.h
@@ -36,7 +36,7 @@
 
 #define PPM_NB_CHANNEL RADIO_CONTROL_NB_CHANNEL
 
-#ifdef USE_NPS
+#if USE_NPS
 extern void radio_control_feed(void);
 #endif
 
diff --git a/sw/airborne/arch/stm32/led_hw.h b/sw/airborne/arch/stm32/led_hw.h
index 397781939f..ca3285f2bc 100644
--- a/sw/airborne/arch/stm32/led_hw.h
+++ b/sw/airborne/arch/stm32/led_hw.h
@@ -60,8 +60,8 @@
     LED_AFIO_REMAP(i);					\
   }
 
-#define LED_ON(i) {LED_GPIO(i)->BSRR = LED_GPIO_PIN(i);}
-#define LED_OFF(i) { LED_GPIO(i)->BRR  = LED_GPIO_PIN(i);}
+#define LED_ON(i) {LED_GPIO(i)->BRR = LED_GPIO_PIN(i);}
+#define LED_OFF(i) { LED_GPIO(i)->BSRR  = LED_GPIO_PIN(i);}
 #define LED_TOGGLE(i) {	LED_GPIO(i)->ODR ^= LED_GPIO_PIN(i);}
 
 #define LED_PERIODIC() {}
diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c
index 22ea358a60..3293155c25 100644
--- a/sw/airborne/arch/stm32/mcu_arch.c
+++ b/sw/airborne/arch/stm32/mcu_arch.c
@@ -31,7 +31,7 @@
 #include <stm32/rcc.h>
 #include <stm32/flash.h>
 #include <stm32/misc.h>
-#ifdef USE_OPENCM3
+#if USE_OPENCM3
 #	if defined(STM32F1) || defined(STM32F2) || defined(STM32F4)
 #		include <libopencm3/stm32/f1/rcc.h>
 #	else
@@ -41,11 +41,11 @@
 
 
 void mcu_arch_init(void) {
-#ifdef USE_OPENCM3
+#if USE_OPENCM3
   rcc_clock_setup_in_hse_12mhz_out_72mhz();
   NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);
   return;
-#endif
+#else // !USE_OPENCM3
 #ifdef HSE_TYPE_EXT_CLK
 #warning Info: Using external clock
   /* Setup the microcontroller system.
@@ -97,6 +97,6 @@ void mcu_arch_init(void) {
                          RCC_APB2Periph_GPIOE | RCC_APB2Periph_AFIO, ENABLE);
 #endif
 
-
+#endif // USE_OPENCM3
 }
 
diff --git a/sw/airborne/arch/stm32/pprz_baudrate.h b/sw/airborne/arch/stm32/pprz_baudrate.h
index bb4a7369a2..183e1ee708 100644
--- a/sw/airborne/arch/stm32/pprz_baudrate.h
+++ b/sw/airborne/arch/stm32/pprz_baudrate.h
@@ -3,7 +3,7 @@
 
 #include BOARD_CONFIG
 
-#ifdef USE_OPENCM3
+#if USE_OPENCM3
 void usart_set_baudrate(void *usart, uint32_t baud);
 #define pprz_usart_set_baudrate(x, y) usart_set_baudrate(x, y)
 #else
diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c
index 38e1bcd7a8..812f4c31c0 100644
--- a/sw/airborne/arch/stm32/subsystems/settings_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c
@@ -35,9 +35,14 @@
 
 #include "subsystems/settings.h"
 
-#include <libopencm3/stm32/dbgmcu.h>
+#if defined(STM32F1) || defined(STM32F2) || defined(STM32F4)
+#include <libopencm3/stm32/f1/flash.h>
+#else
 #include <libopencm3/stm32/flash.h>
+#endif
+
 #include <libopencm3/stm32/crc.h>
+#include <libopencm3/stm32/dbgmcu.h>
 
 struct FlashInfo {
     uint32_t addr;
diff --git a/sw/airborne/arch/stm32/sys_time_hw.h b/sw/airborne/arch/stm32/sys_time_hw.h
index 9d88a69d8c..68146d1318 100644
--- a/sw/airborne/arch/stm32/sys_time_hw.h
+++ b/sw/airborne/arch/stm32/sys_time_hw.h
@@ -50,6 +50,12 @@ extern uint32_t cpu_time_ticks;
 #define SYS_TICS_OF_SEC(s)        (uint32_t)((s) * AHB_CLK + 0.5)
 #define SIGNED_SYS_TICS_OF_SEC(s)  (int32_t)((s) * AHB_CLK + 0.5)
 
+/* Generic timer macros */
+//FIXME: dirty temporary hack!!!
+#define SysTimeTimerStart(_t) { _t = SYS_TICS_OF_SEC(cpu_time_sec) + cpu_time_ticks; }
+#define SysTimeTimer(_t) ((uint32_t)(SYS_TICS_OF_SEC(cpu_time_sec) + cpu_time_ticks - _t))
+#define SysTimeTimerStop(_t) { _t = (SYS_TICS_OF_SEC(cpu_time_sec) + cpu_time_ticks - _t); }
+
 static inline bool_t sys_time_periodic( void ) {
   if (sys_time_period_elapsed) {
     sys_time_period_elapsed = FALSE;
diff --git a/sw/airborne/boards/booz/test_baro.c b/sw/airborne/boards/booz/test_baro.c
index f00cbe54e9..73d5127258 100644
--- a/sw/airborne/boards/booz/test_baro.c
+++ b/sw/airborne/boards/booz/test_baro.c
@@ -32,7 +32,7 @@
 #include "mcu.h"
 
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "subsystems/sensors/baro.h"
 
 static inline void main_init( void );
@@ -68,7 +68,7 @@ static inline void main_init( void ) {
 static inline void main_periodic_task( void ) {
 
   RunOnceEvery(2, {baro_periodic();});
-  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
   LED_PERIODIC();
 
 }
@@ -85,5 +85,5 @@ static inline void main_on_baro_diff(void) {
 }
 
 static inline void main_on_baro_abs(void) {
-  RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);});
+  RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential);});
 }
diff --git a/sw/airborne/boards/booz_1.0.h b/sw/airborne/boards/booz_1.0.h
index db7e8069b6..2d6d2a5b8a 100644
--- a/sw/airborne/boards/booz_1.0.h
+++ b/sw/airborne/boards/booz_1.0.h
@@ -91,11 +91,14 @@
 #endif
 
 /* battery */
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank0(2)
 #ifndef USE_AD0
 #define USE_AD0
 #endif
 #define USE_AD0_2
+#endif
 
 #define DefaultVoltageOfAdc(adc) (0.0183*adc)
 
diff --git a/sw/airborne/boards/hb_1.1.h b/sw/airborne/boards/hb_1.1.h
index 317efcffc8..4599681f72 100644
--- a/sw/airborne/boards/hb_1.1.h
+++ b/sw/airborne/boards/hb_1.1.h
@@ -114,11 +114,14 @@
 #define USE_AD1_4
 #endif
 
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank1(6)
 #ifndef USE_AD1
 #define USE_AD1
 #endif
 #define USE_AD1_6
+#endif
 
 
 #define DefaultVoltageOfAdc(adc) (0.032362123*adc)
diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c
index 11829b5acf..0333505ba8 100644
--- a/sw/airborne/boards/lisa_l/baro_board.c
+++ b/sw/airborne/boards/lisa_l/baro_board.c
@@ -5,7 +5,6 @@ struct Baro baro;
 struct BaroBoard baro_board;
 struct i2c_transaction baro_trans;
 
-
 static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb);
 static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr);
 static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr);
diff --git a/sw/airborne/boards/lisa_l/test_baro.c b/sw/airborne/boards/lisa_l/test_baro.c
index 8a24041f53..43b97d3c7a 100644
--- a/sw/airborne/boards/lisa_l/test_baro.c
+++ b/sw/airborne/boards/lisa_l/test_baro.c
@@ -33,7 +33,7 @@
 #include "sys_time.h"
 #include "mcu_periph/uart.h"
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/sensors/baro.h"
 //#include "my_debug_servo.h"
@@ -75,10 +75,10 @@ static inline void main_periodic_task( void ) {
 
   RunOnceEvery(2, {baro_periodic();});
   LED_PERIODIC();
-  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
   RunOnceEvery(256,
     {
-      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
 			       &i2c2_errors.ack_fail_cnt,
 			       &i2c2_errors.miss_start_stop_cnt,
 			       &i2c2_errors.arb_lost_cnt,
@@ -104,5 +104,5 @@ static inline void main_on_baro_diff(void) {
 }
 
 static inline void main_on_baro_abs(void) {
-  RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);});
+  RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential);});
 }
diff --git a/sw/airborne/boards/lisa_l_1.0.h b/sw/airborne/boards/lisa_l_1.0.h
index 0923a56a40..5cf7ef8e27 100644
--- a/sw/airborne/boards/lisa_l_1.0.h
+++ b/sw/airborne/boards/lisa_l_1.0.h
@@ -24,7 +24,10 @@
 
 
 /* PA0 - ADC0 */
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY 2
+#endif
 #define DefaultVoltageOfAdc(adc) (0.0059*adc)
 /* Onboard ADCs */
 #define BOARD_ADC_CHANNEL_1 ADC_Channel_8
diff --git a/sw/airborne/boards/lisa_m/test_baro.c b/sw/airborne/boards/lisa_m/test_baro.c
index 8a24041f53..43b97d3c7a 100644
--- a/sw/airborne/boards/lisa_m/test_baro.c
+++ b/sw/airborne/boards/lisa_m/test_baro.c
@@ -33,7 +33,7 @@
 #include "sys_time.h"
 #include "mcu_periph/uart.h"
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/sensors/baro.h"
 //#include "my_debug_servo.h"
@@ -75,10 +75,10 @@ static inline void main_periodic_task( void ) {
 
   RunOnceEvery(2, {baro_periodic();});
   LED_PERIODIC();
-  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
   RunOnceEvery(256,
     {
-      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
 			       &i2c2_errors.ack_fail_cnt,
 			       &i2c2_errors.miss_start_stop_cnt,
 			       &i2c2_errors.arb_lost_cnt,
@@ -104,5 +104,5 @@ static inline void main_on_baro_diff(void) {
 }
 
 static inline void main_on_baro_abs(void) {
-  RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);});
+  RunOnceEvery(5,{DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential);});
 }
diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h
index 0fa3a7f7d4..65c50d754d 100644
--- a/sw/airborne/boards/lisa_m_1.0.h
+++ b/sw/airborne/boards/lisa_m_1.0.h
@@ -15,17 +15,25 @@
 #define LED_2_BANK
 #define LED_2_GPIO GPIOC
 #define LED_2_GPIO_CLK RCC_APB2Periph_GPIOC
-#define LED_2_GPIO_PIN GPIO_Pin_13
+#define LED_2_GPIO_PIN GPIO_Pin_5
 #define LED_2_AFIO_REMAP ((void)0)
 
+#define LED_3_BANK
+#define LED_3_GPIO GPIOC
+#define LED_3_GPIO_CLK RCC_APB2Periph_GPIOC
+#define LED_3_GPIO_PIN GPIO_Pin_2
+#define LED_3_AFIO_REMAP ((void)0)
+
 
 /* configuration for aspirin - and more generaly IMUs */
 #define IMU_ACC_DRDY_RCC_GPIO         RCC_APB2Periph_GPIOB
 #define IMU_ACC_DRDY_GPIO             GPIOB
 #define IMU_ACC_DRDY_GPIO_PORTSOURCE  GPIO_PortSourceGPIOB
 
-
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY 2
+#endif
 #define DefaultVoltageOfAdc(adc) (0.00485*adc)
 
 /* Onboard ADCs */
@@ -48,11 +56,12 @@
 
 #define BOARD_HAS_BARO
 
-#define USE_OPENCM3
+#define USE_OPENCM3 1
 
-#define HSE_TYPE_EXT_CLK
-#define STM32_RCC_MODE RCC_HSE_ON
-#define STM32_PLL_MULT RCC_PLLMul_6
+// not needed with USE_OPENCM3:
+//#define HSE_TYPE_EXT_CLK
+//#define STM32_RCC_MODE RCC_HSE_ON
+//#define STM32_PLL_MULT RCC_PLLMul_6
 
 #define PWM_5AND6_TIMER TIM5
 #define PWM_5AND6_RCC RCC_APB1Periph_TIM5
diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c
index 87ac9fdbe7..8940bae2f7 100644
--- a/sw/airborne/boards/navgo/baro_board.c
+++ b/sw/airborne/boards/navgo/baro_board.c
@@ -35,7 +35,7 @@ struct Baro baro;
 #define OFFSET_NBSAMPLES_AVRG 300
 uint16_t offset_cnt;
 
-#ifdef USE_BARO_AS_ALTIMETER
+#if USE_BARO_AS_ALTIMETER
 /* Weight for offset IIR filter */
 #define OFFSET_FILTER 7
 
@@ -52,7 +52,7 @@ void baro_init( void ) {
   LED_OFF(ROTORCRAFT_BARO_LED);
 #endif
   offset_cnt = OFFSET_NBSAMPLES_AVRG;
-#ifdef USE_BARO_AS_ALTIMETER
+#if USE_BARO_AS_ALTIMETER
   baro_alt = 0.;
   baro_alt_offset = 0.;
 #endif
@@ -61,7 +61,7 @@ void baro_init( void ) {
 void baro_periodic( void ) {
 
   if (baro.status == BS_UNINITIALIZED) {
-#ifdef USE_BARO_AS_ALTIMETER
+#if USE_BARO_AS_ALTIMETER
     // IIR filter to compute an initial offset
     baro_alt_offset = (OFFSET_FILTER * baro_alt_offset + (float)baro.absolute) / (OFFSET_FILTER + 1);
 #endif
diff --git a/sw/airborne/boards/navgo/baro_board.h b/sw/airborne/boards/navgo/baro_board.h
index de705d46d0..64a58b1ff6 100644
--- a/sw/airborne/boards/navgo/baro_board.h
+++ b/sw/airborne/boards/navgo/baro_board.h
@@ -33,7 +33,7 @@
 #include "std.h"
 #include "peripherals/ads1114.h"
 
-#ifdef USE_BARO_AS_ALTIMETER
+#if USE_BARO_AS_ALTIMETER
 extern float baro_alt;
 extern float baro_alt_offset;
 #define BaroAltHandler() { baro_alt = BARO_SENS*(baro_alt_offset - (float)baro.absolute); }
diff --git a/sw/airborne/boards/navgo/imu_navgo.c b/sw/airborne/boards/navgo/imu_navgo.c
index b872219e9e..78be53e4a5 100644
--- a/sw/airborne/boards/navgo/imu_navgo.c
+++ b/sw/airborne/boards/navgo/imu_navgo.c
@@ -29,7 +29,7 @@
 // Downlink
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -91,9 +91,9 @@ void imu_periodic( void )
 
 void imu_navgo_downlink_raw( void )
 {
-  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
-  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
-  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
+  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
 }
 
 
diff --git a/sw/airborne/boards/navgo_1.0.h b/sw/airborne/boards/navgo_1.0.h
index da87c56b1c..17a6fbf118 100644
--- a/sw/airborne/boards/navgo_1.0.h
+++ b/sw/airborne/boards/navgo_1.0.h
@@ -43,11 +43,14 @@
 /* ADC */
 
 /* battery */
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank1(3)
 #ifndef USE_AD1
 #define USE_AD1
 #endif
 #define USE_AD1_3
+#endif
 
 #define DefaultVoltageOfAdc(adc) (0.01837*adc)
 
diff --git a/sw/airborne/boards/tiny_0.99.h b/sw/airborne/boards/tiny_0.99.h
index a2891e70f7..f618356e65 100644
--- a/sw/airborne/boards/tiny_0.99.h
+++ b/sw/airborne/boards/tiny_0.99.h
@@ -116,11 +116,14 @@
 #define USE_AD1_2
 #endif
 
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank1(6)
 #ifndef USE_AD1
 #define USE_AD1
 #endif
 #define USE_AD1_6
+#endif
 
 #define DefaultVoltageOfAdc(adc) (0.01787109375*adc)
 
diff --git a/sw/airborne/boards/tiny_1.1.h b/sw/airborne/boards/tiny_1.1.h
index bec08225e0..d24360cb36 100644
--- a/sw/airborne/boards/tiny_1.1.h
+++ b/sw/airborne/boards/tiny_1.1.h
@@ -124,11 +124,14 @@
 #define USE_AD1_5
 #endif
 
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank1(6)
 #ifndef USE_AD1
 #define USE_AD1
 #endif
 #define USE_AD1_6
+#endif
 
 #define DefaultVoltageOfAdc(adc) (0.01787109375*adc)
 
diff --git a/sw/airborne/boards/tiny_2.0.h b/sw/airborne/boards/tiny_2.0.h
index 23bc02f777..1e2c48ead0 100644
--- a/sw/airborne/boards/tiny_2.0.h
+++ b/sw/airborne/boards/tiny_2.0.h
@@ -122,11 +122,14 @@
 #define USE_AD1_3
 #endif
 
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank1(5)
 #ifndef USE_AD1
 #define USE_AD1
 #endif
 #define USE_AD1_5
+#endif
 
 
 #ifndef VoltageOfAdc
diff --git a/sw/airborne/boards/tiny_2.1.h b/sw/airborne/boards/tiny_2.1.h
index 772834da7c..d80dad8efa 100644
--- a/sw/airborne/boards/tiny_2.1.h
+++ b/sw/airborne/boards/tiny_2.1.h
@@ -148,11 +148,14 @@
 #define USE_AD1_3
 #endif
 
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank1(5)
 #ifndef USE_AD1
 #define USE_AD1
 #endif
 #define USE_AD1_5
+#endif
 
 
 #define DefaultVoltageOfAdc(adc) (0.01787109375*adc)
diff --git a/sw/airborne/boards/tiny_sim.h b/sw/airborne/boards/tiny_sim.h
index ff3845ca86..0c6fc3da1c 100644
--- a/sw/airborne/boards/tiny_sim.h
+++ b/sw/airborne/boards/tiny_sim.h
@@ -122,11 +122,14 @@
 #endif
 
 /* #define ADC_3 AdcBank1(7) Used for VSUPPLY */
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank1(7)
 #ifndef USE_AD1
 #define USE_AD1
 #endif
 #define USE_AD1_7
+#endif
 
 
 #define DefaultVoltageOfAdc(adc) (0.01787109375*adc)
diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c
index a63fa457f1..4bc9d9767f 100644
--- a/sw/airborne/boards/umarim/baro_board.c
+++ b/sw/airborne/boards/umarim/baro_board.c
@@ -27,19 +27,11 @@
 
 #include "subsystems/sensors/baro.h"
 
-// Downlink
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "downlink.h"
-
-#ifndef DOWNLINK_DEVICE
-#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
-#endif
 
 /* Common Baro struct */
 struct Baro baro;
 
-#ifdef USE_BARO_AS_ALTIMETER
+#if USE_BARO_AS_ALTIMETER
 /* Number of values to compute an offset at startup */
 #define OFFSET_NBSAMPLES_AVRG 100
 
@@ -56,7 +48,7 @@ void baro_init( void ) {
   baro.status = BS_UNINITIALIZED;
   baro.absolute     = 0;
   baro.differential = 0; /* not handled on this board, use extra module (ex: airspeed_ads1114) */
-#ifdef USE_BARO_AS_ALTIMETER
+#if USE_BARO_AS_ALTIMETER
   baro_alt = 0.;
   baro_alt_offset = 0.;
   offset_cnt = OFFSET_NBSAMPLES_AVRG;
@@ -65,7 +57,7 @@ void baro_init( void ) {
 
 void baro_periodic( void ) {
 
-#ifdef USE_BARO_AS_ALTIMETER
+#if USE_BARO_AS_ALTIMETER
   if (baro.status == BS_UNINITIALIZED && BARO_ABS_ADS.data_available) {
     // IIR filter to compute an initial offset
     baro_alt_offset = (OFFSET_FILTER * baro_alt_offset + (float)baro.absolute) / (OFFSET_FILTER + 1);
@@ -78,8 +70,3 @@ void baro_periodic( void ) {
   ads1114_read(&BARO_ABS_ADS);
 }
 
-void baro_downlink_raw( void )
-{
-  DOWNLINK_SEND_BARO_RAW(DefaultChannel,&baro.absolute,&baro.differential);
-}
-
diff --git a/sw/airborne/boards/umarim/baro_board.h b/sw/airborne/boards/umarim/baro_board.h
index e74b52c77d..62f09ca49c 100644
--- a/sw/airborne/boards/umarim/baro_board.h
+++ b/sw/airborne/boards/umarim/baro_board.h
@@ -40,14 +40,12 @@
  * */
 #define DIFF_FILTER_GAIN 5
 
-#ifdef USE_BARO_AS_ALTIMETER
+#if USE_BARO_AS_ALTIMETER
 extern float baro_alt;
 extern float baro_alt_offset;
 #define BaroAltHandler() { baro_alt = BARO_SENS*(baro_alt_offset - (float)baro.absolute); }
 #endif
 
-extern void baro_downlink_raw( void );
-
 #define BARO_ABS_ADS ads1114_1
 
 #define BaroAbs(_ads, _handler) {           \
diff --git a/sw/airborne/boards/umarim/imu_umarim.c b/sw/airborne/boards/umarim/imu_umarim.c
index 7f53a5e118..0ff9f8bc30 100644
--- a/sw/airborne/boards/umarim/imu_umarim.c
+++ b/sw/airborne/boards/umarim/imu_umarim.c
@@ -29,7 +29,7 @@
 // Downlink
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -80,8 +80,8 @@ void imu_periodic( void )
 
 void imu_umarim_downlink_raw( void )
 {
-  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
-  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
 }
 
 
diff --git a/sw/airborne/boards/umarim/imu_umarim.h b/sw/airborne/boards/umarim/imu_umarim.h
index a4cf9ccfcc..432d723060 100644
--- a/sw/airborne/boards/umarim/imu_umarim.h
+++ b/sw/airborne/boards/umarim/imu_umarim.h
@@ -40,6 +40,57 @@
 #define IMU_ACCEL_Z_SIGN  1
 #endif
 
+/** default gyro sensitivy and neutral from the datasheet
+ * ITG3200 has 14.375 LSB/(deg/s)
+ * sens = 1/14.375 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/14.375 * pi/180 * 4096 = 4.973126
+ */
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+#define IMU_GYRO_P_SENS 4.973
+#define IMU_GYRO_P_SENS_NUM 4973
+#define IMU_GYRO_P_SENS_DEN 1000
+#define IMU_GYRO_Q_SENS 4.973
+#define IMU_GYRO_Q_SENS_NUM 4973
+#define IMU_GYRO_Q_SENS_DEN 1000
+#define IMU_GYRO_R_SENS 4.973
+#define IMU_GYRO_R_SENS_NUM 4973
+#define IMU_GYRO_R_SENS_DEN 1000
+#endif
+#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
+#define IMU_GYRO_P_NEUTRAL 0
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#endif
+
+
+/** default accel sensitivy from the ADXL345 datasheet
+ * sensitivity of x & y axes depends on supply voltage:
+ *   - 256 LSB/g @ 2.5V
+ *   - 265 LSB/g @ 3.3V
+ * z sensitivity stays at 256 LSB/g
+ * fixed point sens: 9.81 [m/s^2] / 256 [LSB/g] * 2^INT32_ACCEL_FRAC
+ * x/y sens = 9.81 / 265 * 1024 = 37.91
+ * z sens   = 9.81 / 256 * 1024 = 39.24
+ *
+ * what about the offset at 3.3V?
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+#define IMU_ACCEL_X_SENS 37.91
+#define IMU_ACCEL_X_SENS_NUM 3791
+#define IMU_ACCEL_X_SENS_DEN 100
+#define IMU_ACCEL_Y_SENS 37.91
+#define IMU_ACCEL_Y_SENS_NUM 3791
+#define IMU_ACCEL_Y_SENS_DEN 100
+#define IMU_ACCEL_Z_SENS 39.24
+#define IMU_ACCEL_Z_SENS_NUM 39.24
+#define IMU_ACCEL_Z_SENS_DEN 100
+#endif
+#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
+#define IMU_ACCEL_X_NEUTRAL 0
+#define IMU_ACCEL_Y_NEUTRAL 0
+#define IMU_ACCEL_Z_NEUTRAL 0
+#endif
+
 extern volatile bool_t gyr_valid;
 extern volatile bool_t acc_valid;
 
diff --git a/sw/airborne/boards/umarim_1.0.h b/sw/airborne/boards/umarim_1.0.h
index f4c87cbb96..eb47a0545c 100644
--- a/sw/airborne/boards/umarim_1.0.h
+++ b/sw/airborne/boards/umarim_1.0.h
@@ -78,11 +78,14 @@
 
 
 /* battery */
+/* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/
+#ifndef ADC_CHANNEL_VSUPPLY
 #define ADC_CHANNEL_VSUPPLY AdcBank0(2)
 #ifndef USE_AD0
 #define USE_AD0
 #endif
 #define USE_AD0_2
+#endif
 
 #define DefaultVoltageOfAdc(adc) (0.0247*adc)
 
diff --git a/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c b/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c
index c19f2ff874..516e9aad0e 100644
--- a/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c
+++ b/sw/airborne/booz/test/booz2_test_buss_bldc_hexa.c
@@ -26,8 +26,8 @@
 #include "mcu.h"
 #include "sys_time.h"
 #include "interrupt_hw.h"
-#include "downlink.h"
-#include "datalink.h"
+#include "subsystems/datalink/downlink.h"
+#include "subsystems/datalink/datalink.h"
 
 #include "booz2_test_buss_bldc_hexa.h"
 
@@ -68,7 +68,7 @@ static inline void main_periodic_task( void ) {
   i2c0_buf[0] = thrust;
   i2c0_transmit(motor_addr[motor], 1, &i2c_done);
 
-  RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
 
 }
 
diff --git a/sw/airborne/booz/test/booz2_test_crista.c b/sw/airborne/booz/test/booz2_test_crista.c
index 1231b7e6ab..5d85c36be1 100644
--- a/sw/airborne/booz/test/booz2_test_crista.c
+++ b/sw/airborne/booz/test/booz2_test_crista.c
@@ -29,7 +29,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/imu.h"
 
diff --git a/sw/airborne/booz/test/booz2_test_gps.c b/sw/airborne/booz/test/booz2_test_gps.c
index fa6f92a64d..aa633866d9 100644
--- a/sw/airborne/booz/test/booz2_test_gps.c
+++ b/sw/airborne/booz/test/booz2_test_gps.c
@@ -26,7 +26,7 @@
 #include "std.h"
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "subsystems/gps.h"
 #include "interrupt_hw.h"
 
@@ -56,7 +56,7 @@ static inline void main_init( void ) {
 
 static inline void main_periodic_task( void ) {
 
-  RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(128, { DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
   RunOnceEvery(128, { LED_PERIODIC();});
 }
 
@@ -67,7 +67,7 @@ static inline void main_event_task( void ) {
 
 static void on_gps_sol(void) {
 
-  DOWNLINK_SEND_GPS_INT( DefaultChannel,
+  DOWNLINK_SEND_GPS_INT( DefaultChannel, DefaultDevice,
 			   &gps.ecef_pos.x,
 			   &gps.ecef_pos.y,
 			   &gps.ecef_pos.z,
diff --git a/sw/airborne/booz/test/booz2_test_max1168.c b/sw/airborne/booz/test/booz2_test_max1168.c
index dc57a9a454..7565673cb9 100644
--- a/sw/airborne/booz/test/booz2_test_max1168.c
+++ b/sw/airborne/booz/test/booz2_test_max1168.c
@@ -28,7 +28,7 @@
 #include "sys_time.h"
 #include "interrupt_hw.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "armVIC.h"
 #include "LPC21xx.h"
@@ -73,9 +73,9 @@ static inline void main_periodic_task( void ) {
 static inline void main_event_task( void ) {
   if (max1168_status == STA_MAX1168_DATA_AVAILABLE) {
     RunOnceEvery(10, {
-	DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, &max1168_values[0], &max1168_values[1], &max1168_values[2]);
-	DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, &max1168_values[3], &max1168_values[4], &max1168_values[6]);
-	DOWNLINK_SEND_BOOT(DefaultChannel, &max1168_values[7]); });
+	DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &max1168_values[0], &max1168_values[1], &max1168_values[2]);
+	DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &max1168_values[3], &max1168_values[4], &max1168_values[6]);
+	DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &max1168_values[7]); });
     max1168_status = STA_MAX1168_IDLE;
   }
 }
diff --git a/sw/airborne/booz/test/booz2_test_mc.c b/sw/airborne/booz/test/booz2_test_mc.c
index ba755f46ce..9b1fb96277 100644
--- a/sw/airborne/booz/test/booz2_test_mc.c
+++ b/sw/airborne/booz/test/booz2_test_mc.c
@@ -29,7 +29,7 @@
 #include "mcu_periph/uart.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "commands.h"
 #include "mcu_periph/i2c.h"
diff --git a/sw/airborne/booz/test/booz2_test_micromag.c b/sw/airborne/booz/test/booz2_test_micromag.c
index 9ba50b7fee..bbc1aaae51 100644
--- a/sw/airborne/booz/test/booz2_test_micromag.c
+++ b/sw/airborne/booz/test/booz2_test_micromag.c
@@ -29,7 +29,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "interrupt_hw.h"
 #include "armVIC.h"
diff --git a/sw/airborne/booz/test/booz2_test_modem.c b/sw/airborne/booz/test/booz2_test_modem.c
index df06203773..e4fa59f95f 100644
--- a/sw/airborne/booz/test/booz2_test_modem.c
+++ b/sw/airborne/booz/test/booz2_test_modem.c
@@ -30,7 +30,7 @@
 #include "mcu_periph/uart.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
diff --git a/sw/airborne/booz/test/booz2_test_radio_control.c b/sw/airborne/booz/test/booz2_test_radio_control.c
index 9daf3a9afc..a0da36ee80 100644
--- a/sw/airborne/booz/test/booz2_test_radio_control.c
+++ b/sw/airborne/booz/test/booz2_test_radio_control.c
@@ -28,7 +28,7 @@
 #include "interrupt_hw.h"
 #include "mcu_periph/uart.h"
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/radio_control.h"
 
@@ -62,14 +62,14 @@ static inline void main_periodic_task( void ) {
   RunOnceEvery(51, {
     /*LED_TOGGLE(2);*/
     uint32_t blaaa= cpu_time_sec;
-    DOWNLINK_SEND_TIME(DefaultChannel, &blaaa);
+    DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &blaaa);
   });
 
   RunOnceEvery(10, {radio_control_periodic_task();});
 
   int16_t foo = 0;//RC_PPM_SIGNED_TICS_OF_USEC(2050-1500);
   RunOnceEvery(10,
-    {DOWNLINK_SEND_BOOZ2_RADIO_CONTROL(DefaultChannel,	\
+    {DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(DefaultChannel, DefaultDevice,	\
 				       &radio_control.values[RADIO_ROLL], \
 				       &radio_control.values[RADIO_PITCH], \
 				       &radio_control.values[RADIO_YAW], \
@@ -79,7 +79,7 @@ static inline void main_periodic_task( void ) {
 				       &radio_control.status);});
 #ifdef RADIO_CONTROL_TYPE_PPM
   RunOnceEvery(10,
-	       {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel,&blaa, 8, booz_radio_control_ppm_pulses);});
+	       {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice,&blaa, 8, booz_radio_control_ppm_pulses);});
 #endif
 
   LED_PERIODIC();
diff --git a/sw/airborne/booz/test/booz2_test_rc.c b/sw/airborne/booz/test/booz2_test_rc.c
index 5806e65250..65a9f2a0f4 100644
--- a/sw/airborne/booz/test/booz2_test_rc.c
+++ b/sw/airborne/booz/test/booz2_test_rc.c
@@ -29,7 +29,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "interrupt_hw.h"
 
diff --git a/sw/airborne/booz/test/booz2_test_usb.c b/sw/airborne/booz/test/booz2_test_usb.c
index 9e35042c62..8fe1805101 100644
--- a/sw/airborne/booz/test/booz2_test_usb.c
+++ b/sw/airborne/booz/test/booz2_test_usb.c
@@ -31,7 +31,7 @@
 #include "mcu_periph/usb_serial.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "interrupt_hw.h"
 
diff --git a/sw/airborne/booz/test/booz_test_imu.c b/sw/airborne/booz/test/booz_test_imu.c
index 19146b63cb..a22e5a29ec 100644
--- a/sw/airborne/booz/test/booz_test_imu.c
+++ b/sw/airborne/booz/test/booz_test_imu.c
@@ -32,7 +32,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/imu.h"
 
@@ -78,11 +78,11 @@ static inline void led_toggle ( void ) {
 static inline void main_periodic_task( void ) {
   RunOnceEvery(100, {
       led_toggle();
-      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
     });
 #ifdef USE_I2C2
   RunOnceEvery(111, {
-      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
                    &i2c2_errors.ack_fail_cnt,
                    &i2c2_errors.miss_start_stop_cnt,
                    &i2c2_errors.arb_lost_cnt,
@@ -112,13 +112,13 @@ static inline void on_accel_event(void) {
   cnt++;
   if (cnt > 15) cnt = 0;
   if (cnt == 0) {
-    DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
+    DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
                 &imu.accel_unscaled.x,
                 &imu.accel_unscaled.y,
                 &imu.accel_unscaled.z);
   }
   else if (cnt == 7) {
-    DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
+    DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice,
                   &imu.accel.x,
                   &imu.accel.y,
                   &imu.accel.z);
@@ -134,13 +134,13 @@ static inline void on_gyro_accel_event(void) {
   if (cnt > 15) cnt = 0;
 
   if (cnt == 0) {
-    DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
+    DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
                    &imu.gyro_unscaled.p,
                    &imu.gyro_unscaled.q,
                    &imu.gyro_unscaled.r);
   }
   else if (cnt == 7) {
-    DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel,
+    DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice,
                  &imu.gyro.p,
                  &imu.gyro.q,
                  &imu.gyro.r);
@@ -155,13 +155,13 @@ static inline void on_mag_event(void) {
   if (cnt > 10) cnt = 0;
 
   if (cnt == 0) {
-    DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel,
+    DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice,
                 &imu.mag.x,
                 &imu.mag.y,
                 &imu.mag.z);
   }
   else if (cnt == 5) {
-    DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+    DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
                   &imu.mag_unscaled.x,
                   &imu.mag_unscaled.y,
                   &imu.mag_unscaled.z);
diff --git a/sw/airborne/booz/test/booz_test_telemetry.c b/sw/airborne/booz/test/booz_test_telemetry.c
index f21b005ff9..0485b08dc8 100644
--- a/sw/airborne/booz/test/booz_test_telemetry.c
+++ b/sw/airborne/booz/test/booz_test_telemetry.c
@@ -28,7 +28,7 @@
 #include "interrupt_hw.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
@@ -52,7 +52,7 @@ static inline void main_init( void ) {
 
 static inline void main_periodic_task( void ) {
   LED_TOGGLE(2);
-  DOWNLINK_SEND_TIME(DefaultChannel, &cpu_time_sec);
+  DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &cpu_time_sec);
 }
 
 static inline void main_event_task( void ) {
diff --git a/sw/airborne/commands.c b/sw/airborne/commands.c
index c73b50081f..3dd61cfdd3 100644
--- a/sw/airborne/commands.c
+++ b/sw/airborne/commands.c
@@ -28,5 +28,16 @@
 
 #include "commands.h"
 
+#ifndef COMMAND_ROLL_TRIM
+#define COMMAND_ROLL_TRIM 0
+#endif
+
+#ifndef COMMAND_PITCH_TRIM
+#define COMMAND_PITCH_TRIM 0
+#endif
+
+pprz_t command_roll_trim = COMMAND_ROLL_TRIM;
+pprz_t command_pitch_trim = COMMAND_PITCH_TRIM;
+
 pprz_t commands[COMMANDS_NB];
 const pprz_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
diff --git a/sw/airborne/commands.h b/sw/airborne/commands.h
index a824de01c9..4716959233 100644
--- a/sw/airborne/commands.h
+++ b/sw/airborne/commands.h
@@ -32,6 +32,8 @@
 #include "paparazzi.h"
 #include "generated/airframe.h"
 
+extern pprz_t command_roll_trim;
+extern pprz_t command_pitch_trim;
 
 extern pprz_t commands[COMMANDS_NB];
 extern const pprz_t commands_failsafe[COMMANDS_NB];
diff --git a/sw/airborne/csc/arm7/buss_twi_blmc_hw.c b/sw/airborne/csc/arm7/buss_twi_blmc_hw.c
deleted file mode 100644
index 11c5fd4d9d..0000000000
--- a/sw/airborne/csc/arm7/buss_twi_blmc_hw.c
+++ /dev/null
@@ -1,93 +0,0 @@
-#include "buss_twi_blmc_hw.h"
-#include <inttypes.h>
-
-// From sys_time_hw.h
-#define T0_PCLK_DIV 1
-
-// Number of PCLK cycles to wait between sending commands
-#define TICS_OF_BLMC_IDX (I2C_CYCLES_PER_MSG * (I2C0_SCLL + I2C0_SCLH) / T0_PCLK_DIV)
-
-// Number of I2C cycles to wait before timeout and send next command (including some margin)
-#define I2C_CYCLES_PER_MSG 48
-
-uint8_t twi_blmc_nb_err;
-
-uint8_t motor_power[BUSS_TWI_BLMC_NB];
-static volatile bool_t  buss_twi_blmc_status;
-static volatile bool_t  buss_twi_blmc_i2c_done;
-static volatile uint8_t buss_twi_blmc_idx;
-static uint32_t commit_start_time;
-
-const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = BUSS_BLMC_ADDR;
-
-void motors_init ( void ) {
-  uint8_t i;
-  for (i=0; i<BUSS_TWI_BLMC_NB;i++)
-    motor_power[i] = 0;
-  buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
-  twi_blmc_nb_err = 0;
-  buss_twi_blmc_i2c_done = TRUE;
-}
-
-void motors_set_motor(uint8_t id, int16_t value)
-{
-  // insert range checks
-  if(value < 0)
-    motor_power[id] = 0;
-  else if(value > 255)
-    motor_power[id] = 255;
-  else
-    motor_power[id] = value;
-}
-
-static void buss_twi_blmc_send_next()
-{
-  i2c0_buf[0] = motor_power[buss_twi_blmc_idx];
-  i2c0_transmit(buss_twi_blmc_addr[buss_twi_blmc_idx], 1, &buss_twi_blmc_i2c_done);
-}
-
-void motors_commit_next( void )
-{
-    buss_twi_blmc_idx++;
-    if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB_SEND)
-      buss_twi_blmc_send_next();
-    else
-      buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
-}
-
-void motors_commit(int force)
-{
-
-  if (force || buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) {
-    I2c0SendStop();
-    buss_twi_blmc_idx = 0;
-    buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY;
-    buss_twi_blmc_send_next();
-
-    commit_start_time = T0TC;
-
-  }
-}
-
-
-void motors_callback_nop()
-{
-  // do nothing...
-  // We don't use this to kick the next command automatically anymore
-}
-
-void motors_event( void )
-{
-  // if busy sending, check progress
-  if (buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_BUSY) {
-    // check timer 0 counter to see if it is time for next message
-    if ((T0TC - commit_start_time) > TICS_OF_BLMC_IDX) {
-      // abort any previous activity
-      I2c0SendStop();
-      // advance index and start transmit on next message
-      motors_commit_next();
-      // record current time
-      commit_start_time = T0TC;
-    }
-  }
-}
diff --git a/sw/airborne/csc/arm7/buss_twi_blmc_hw.h b/sw/airborne/csc/arm7/buss_twi_blmc_hw.h
deleted file mode 100644
index 1b7c3d8282..0000000000
--- a/sw/airborne/csc/arm7/buss_twi_blmc_hw.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef BUSS_TWI_BLMC_HW_H
-#define BUSS_TWI_BLMC_HW_H
-
-#include <string.h>
-#include "std.h"
-#include "i2c.h"
-
-#include "generated/airframe.h"
-
-#define BUSS_TWI_BLMC_STATUS_IDLE 0
-#define BUSS_TWI_BLMC_STATUS_BUSY 1
-
-extern uint8_t motor_power[BUSS_TWI_BLMC_NB];
-
-void motors_init ( void );
-void motors_set_motor(uint8_t id, int16_t value);
-void motors_commit(int force);
-void motors_commit_next( void );
-void motors_callback_nop( void );
-void motors_event( void );
-
-#endif /* BUSS_TWI_BLMC_HW_H */
diff --git a/sw/airborne/csc/arm7/props_csc.c b/sw/airborne/csc/arm7/props_csc.c
deleted file mode 100644
index 38263555c7..0000000000
--- a/sw/airborne/csc/arm7/props_csc.c
+++ /dev/null
@@ -1,31 +0,0 @@
-#include "props_csc.h"
-#include "generated/airframe.h"
-#include "csc_ap_link.h"
-#include "csc_msg_def.h"
-
-uint8_t csc_prop_speeds[PROPS_NB];
-
-void props_init()
-{
-  for(uint8_t i = 0; i < PROPS_NB; i++)
-    csc_prop_speeds[i] = 0;
-
-}
-
-void props_set(uint8_t i,uint8_t speed)
-{
-  if(i > PROPS_NB) return;
-  csc_prop_speeds[i] = speed;
-}
-
-void props_commit()
-{
-  struct CscPropCmd cmd;
-  uint8_t i;
-
-  for(i = 0; i < PROPS_NB; i++)
-    cmd.speeds[i] = csc_prop_speeds[i];
-
-  csc_ap_send_msg(CSC_PROP_CMD_ID, (uint8_t *)&cmd, sizeof(struct CscPropCmd));
-
-}
diff --git a/sw/airborne/csc/arm7/props_csc.h b/sw/airborne/csc/arm7/props_csc.h
deleted file mode 100644
index 3fafdef242..0000000000
--- a/sw/airborne/csc/arm7/props_csc.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef PROPS_CSC_H
-#define PROPS_CSC_H
-
-#include "generated/airframe.h"
-#include "firmwares/rotorcraft/actuators.h"
-#include "sys_time.h"
-#include "csc_ap_link.h"
-#include "csc_msg_def.h"
-
-extern uint8_t csc_prop_speeds[PROPS_NB];
-
-void props_set(uint8_t i, uint8_t speed);
-void props_commit();
-
-#endif /* PROPS_CSC_H */
diff --git a/sw/airborne/csc/csc_adc.c b/sw/airborne/csc/csc_adc.c
deleted file mode 100644
index d81fa19d8a..0000000000
--- a/sw/airborne/csc/csc_adc.c
+++ /dev/null
@@ -1,51 +0,0 @@
-#include "csc_adc.h"
-#include "csc_ap_link.h"
-#include <stdio.h>
-#include "mcu_periph/uart.h"
-#include "print.h"
-
-#include "LPC21xx.h"
-#include "led.h"
-#include "mcu_periph/adc.h"
-#include ACTUATORS
-#include "csc_servos.h"
-#include "sys_time.h"
-
-#define ADC_NB_CSC 4
-
-struct adc_buf adc_bufs[ADC_NB_CSC];
-
-uint8_t vsupply;
-uint16_t adc_slider;
-uint16_t adc_values[ADC_NB_CSC];
-
-#define ADC_VDIV 5.7
-#define ADC_VOLT 3.28
-#define ADC_FACTOR 1024.0 * ADC_VOLT * ADC_VDIV
-
-#ifndef ADC_AV_NB
-#define ADC_AV_NB 8
-#endif
-
-#define ADC_VSUPPLY 0
-#define ADC_SLIDER 3
-
-
-
-void csc_adc_init(void)
-{
-  adc_init();
-  for (int i = 0; i < ADC_NB_CSC; i++) {
-    adc_buf_channel(i, &adc_bufs[i], ADC_AV_NB);
-  }
-}
-
-void csc_adc_periodic(void)
-{
-  vsupply = adc_bufs[ADC_VSUPPLY].sum / adc_bufs[ADC_VSUPPLY].av_nb_sample / ADC_FACTOR * 10;
-  adc_slider = adc_bufs[ADC_SLIDER].sum / adc_bufs[ADC_SLIDER].av_nb_sample;
-  for (int i = 0; i < ADC_NB_CSC; i++) {
-    adc_values[i] = adc_bufs[i].sum / adc_bufs[i].av_nb_sample;
-  }
-}
-
diff --git a/sw/airborne/csc/csc_adc.h b/sw/airborne/csc/csc_adc.h
deleted file mode 100644
index 903ce28b0d..0000000000
--- a/sw/airborne/csc/csc_adc.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef CSC_ADC_H
-#define CSC_ADC_H
-
-#include <inttypes.h>
-
-void csc_adc_init(void);
-void csc_adc_periodic(void);
-
-extern uint8_t vsupply;
-extern uint16_t adc_slider;
-extern uint16_t adc_values[];
-
-#endif /* CSC_ADC_H */
-
diff --git a/sw/airborne/csc/csc_airspeed.c b/sw/airborne/csc/csc_airspeed.c
deleted file mode 100644
index 3a67f5a28f..0000000000
--- a/sw/airborne/csc/csc_airspeed.c
+++ /dev/null
@@ -1,15 +0,0 @@
-#include "csc_airspeed.h"
-#include "csc_ap_link.h"
-#include "csc_adc.h"
-
-float estimator_airspeed;
-
-#define AIRSPEED_ADC 3
-
-void csc_airspeed_periodic(void)
-{
-  float adc_airspeed;
-
-  adc_airspeed = adc_values[AIRSPEED_ADC];
-  csc_ap_link_send_airspeed(estimator_airspeed, adc_airspeed);
-}
diff --git a/sw/airborne/csc/csc_airspeed.h b/sw/airborne/csc/csc_airspeed.h
deleted file mode 100644
index 5020ecd4c1..0000000000
--- a/sw/airborne/csc/csc_airspeed.h
+++ /dev/null
@@ -1 +0,0 @@
-void csc_airspeed_periodic(void);
diff --git a/sw/airborne/csc/csc_ap_link.c b/sw/airborne/csc/csc_ap_link.c
deleted file mode 100644
index b5365483d7..0000000000
--- a/sw/airborne/csc/csc_ap_link.c
+++ /dev/null
@@ -1,233 +0,0 @@
-#include <string.h>
-#include "csc_ap_link.h"
-#include "csc_can.h"
-#include "led.h"
-
-
-int32_t csc_ap_link_error_cnt;
-
-static void (* servo_msg_cb)(struct CscServoCmd *);
-static void (* motor_msg_cb)(struct CscMotorMsg *);
-static void (* rc_msg_cb)(struct CscRCMsg *);
-static void (* prop_msg_cb)(struct CscPropCmd *, int idx);
-static void (* gpsfix_cb)(struct CscGPSFixMsg *);
-static void (* gpspos_cb)(struct CscGPSPosMsg *);
-static void (* gpsacc_cb)(struct CscGPSAccMsg *);
-
-void csc_ap_link_send_adc(float adc1, float adc2)
-{
-  struct CscADCMsg msg;
-
-  msg.ADCVolts1 = adc1;
-  msg.ADCVolts2 = adc2;
-
-  csc_ap_send_msg(CSC_BOARD_ADCVOLTS_ID, (const uint8_t *) &msg, sizeof(msg));
-}
-
-void csc_ap_link_send_status(uint32_t loops, uint32_t msgs)
-{
-
-  struct CscStatusMsg msg;
-
-  msg.loop_count = loops;
-  msg.msg_count = msgs;
-
-  csc_ap_send_msg(CSC_BOARD_STATUS_ID, (const uint8_t *) &msg, sizeof(msg));
-}
-
-void csc_ap_link_send_vane(float *vane_angles)
-{
-
-  struct CscVaneMsg msg;
-
-  msg.vane_angle1 = vane_angles[0];
-  msg.vane_angle2 = vane_angles[1];
-
-  csc_ap_send_msg(CSC_VANE_MSG_ID, (const uint8_t *) &msg, sizeof(msg));
-}
-
-
-void csc_ap_link_send_pressure(float pressure1, float pressure2)
-{
-
-  struct CscPressureMsg msg;
-
-  msg.pressure1 = pressure1;
-  msg.pressure2 = pressure2;
-
-  csc_ap_send_msg(CSC_PRESSURE_MSG_ID, (const uint8_t *) &msg, sizeof(msg));
-}
-
-void csc_ap_link_send_airspeed(float airspeed, uint8_t sensor_id)
-{
-
-  struct CscAirspeedMsg msg;
-
-  msg.airspeed = airspeed;
-  msg.sensor_addr = sensor_id;
-
-  csc_ap_send_msg(CSC_AIRSPEED_MSG_ID, (const uint8_t *) &msg, sizeof(msg));
-}
-
-
-void csc_ap_link_send_baro(uint32_t pressure, uint16_t temperature, uint8_t status, uint8_t sensor_addr)
-{
-
-  struct CscBaroMsg msg;
-
-  msg.baro_pressure = pressure;
-  msg.baro_temperature = temperature;
-  msg.baro_status = status;
-  msg.baro_sensor_addr = sensor_addr;
-
-  csc_ap_send_msg(CSC_BARO_MSG_ID, (const uint8_t *) &msg, sizeof(msg));
-}
-
-
-void csc_ap_link_send_bat(uint16_t volts, uint16_t amps, uint8_t msgctr)
-{
-
-  struct CscBatMsg msg;
-
-  msg.volts = volts;
-  msg.amps = amps;
-  msg.msgctr = msgctr;
-
-  csc_ap_send_msg(CSC_BAT_MSG_ID, (const uint8_t *) &msg, sizeof(msg));
-}
-
-
-// Generic function for sending can messages
-void can_write_csc(uint8_t board_id, uint8_t msg_id, const uint8_t *buf, uint8_t len)
-{
-  struct CscCanMsg out_msg;
-
-  if (len > 8) return;
-
-  // set frame length
-  out_msg.frame = (len << 16);
-  // build msg id using our board ID and requested msg id
-  out_msg.id = ((board_id & CSC_BOARD_MASK) << 7) | (msg_id & CSC_MSGID_MASK);
-  // copy msg payload in host order
-  memcpy((char *)&out_msg.dat_a, buf, len);
-  // send via CAN
-  csc_can1_send(&out_msg);
-}
-
-void csc_ap_send_msg(uint8_t msg_id, const uint8_t *buf, uint8_t len)
-{
-  can_write_csc(CSC_BOARD_ID, msg_id, buf, len);
-}
-
-void csc_ap_link_set_servo_cmd_cb(void (* cb)(struct CscServoCmd *cmd))
-{
-  servo_msg_cb = cb;
-}
-
-void csc_ap_link_set_motor_cmd_cb(void (* cb)(struct CscMotorMsg *msg))
-{
-  motor_msg_cb = cb;
-}
-
-void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd, int idx))
-{
-  prop_msg_cb = cb;
-}
-
-void csc_ap_link_set_rc_cmd_cb(void (* cb)(struct CscRCMsg *msg))
-{
-  rc_msg_cb = cb;
-}
-
-void csc_ap_link_set_gpsfix_cb(void (* cb)(struct CscGPSFixMsg *msg))
-{
-  gpsfix_cb = cb;
-}
-
-void csc_ap_link_set_gpspos_cb(void (* cb)(struct CscGPSPosMsg *msg))
-{
-  gpspos_cb = cb;
-}
-
-void csc_ap_link_set_gpsacc_cb(void (* cb)(struct CscGPSAccMsg *msg))
-{
-  gpsacc_cb = cb;
-}
-
-static void on_ap_link_msg( struct CscCanMsg *msg)
-{
-  uint32_t msg_id = MSGID_OF_CANMSG_ID(msg->id);
-  uint32_t len = CAN_MSG_LENGTH_OF_FRAME(msg->frame);
-  switch (msg_id) {
-    case CSC_SERVO_CMD_ID:
-      if (len != sizeof(struct CscServoCmd)) {
-	LED_ON(ERROR_LED);
-	csc_ap_link_error_cnt++;
-      } else {
-	// cast can data buffer pointer directly to csc_servo_cmd pointer
-	servo_msg_cb((struct CscServoCmd *) &msg->dat_a);
-      }
-      break;
-    case CSC_MOTOR_CMD_ID:
-      if (len != sizeof(struct CscMotorMsg)) {
-	LED_ON(ERROR_LED);
-	csc_ap_link_error_cnt++;
-      } else {
-	// cast can data buffer pointer directly to csc_motor_msg pointer
-	//motor_msg_cb((struct CscMotorMsg *) &msg->dat_a);
-	LED_TOGGLE(ERROR_LED);
-      }
-      break;
-    case CSC_RC_ID:
-      if (len != sizeof(struct CscRCMsg)) {
-	LED_ON(ERROR_LED);
-	csc_ap_link_error_cnt++;
-      } else {
-	// cast can data buffer pointer directly to csc_motor_msg pointer
-	rc_msg_cb((struct CscRCMsg *) &msg->dat_a);
-      }
-      break;
-    case CSC_PROP_CMD_ID:
-      if (len != sizeof(struct CscPropCmd)){
-	LED_ON(ERROR_LED);
-	csc_ap_link_error_cnt++;
-      } else
-	prop_msg_cb((struct CscPropCmd *) &msg->dat_a, 0);
-      break;
-    case CSC_GPS_FIX_ID:
-      if (len != sizeof(struct CscGPSFixMsg)){
-	LED_ON(ERROR_LED);
-	csc_ap_link_error_cnt++;
-      } else
-	gpsfix_cb((struct CscGPSFixMsg *) &msg->dat_a);
-      break;
-    case CSC_GPS_POS_ID:
-      if (len != sizeof(struct CscGPSPosMsg)){
-	LED_ON(ERROR_LED);
-	csc_ap_link_error_cnt++;
-      } else
-	gpspos_cb((struct CscGPSPosMsg *) &msg->dat_a);
-      break;
-    case CSC_GPS_ACC_ID:
-      if (len != sizeof(struct CscGPSAccMsg)){
-	LED_ON(ERROR_LED);
-	csc_ap_link_error_cnt++;
-      } else
-	gpsacc_cb((struct CscGPSAccMsg *) &msg->dat_a);
-      break;
-    case CSC_PROP2_CMD_ID:
-      if (len != sizeof(struct CscPropCmd)){
-	LED_ON(ERROR_LED);
-	csc_ap_link_error_cnt++;
-      } else
-	prop_msg_cb((struct CscPropCmd *) &msg->dat_a, 1);
-      break;
-  }
-}
-
-void csc_ap_link_init(void)
-{
-  csc_can1_init(on_ap_link_msg);
-  csc_ap_link_error_cnt = 0;
-}
-
diff --git a/sw/airborne/csc/csc_ap_link.h b/sw/airborne/csc/csc_ap_link.h
deleted file mode 100644
index a7d82a4989..0000000000
--- a/sw/airborne/csc/csc_ap_link.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef CSC_AP_LINK_H
-#define CSC_AP_LINK_H
-
-#include "std.h"
-#include "string.h"
-#include "csc_can.h"
-#include "csc_msg_def.h"
-
-
-extern int32_t csc_ap_link_error_cnt;
-
-void csc_ap_link_init(void);
-void csc_ap_send_msg(uint8_t msg_id, const uint8_t *buf, uint8_t len);
-void can_write_csc(uint8_t board_id, uint8_t msg_id, const uint8_t *buf, uint8_t len);
-void csc_ap_link_send_status(uint32_t loops, uint32_t msgs);
-void csc_ap_link_send_adc(float adc1, float adc2);
-void csc_ap_link_send_vane(float *vane_angle);
-void csc_ap_link_send_pressure(float pressure1, float pressure2);
-void csc_ap_link_send_airspeed(float airspeed, uint8_t sensor_id);
-void csc_ap_link_send_baro(uint32_t pressure, uint16_t temp, uint8_t status, uint8_t sensor_addr);
-void csc_ap_link_set_servo_cmd_cb(void (* cb)(struct CscServoCmd *cmd));
-void csc_ap_link_set_motor_cmd_cb(void (* cb)(struct CscMotorMsg *msg));
-void csc_ap_link_set_prop_cmd_cb(void (* cb)(struct CscPropCmd *cmd, int idx));
-void csc_ap_link_set_rc_cmd_cb(void (* cb)(struct CscRCMsg *msg));
-void csc_ap_link_set_gpsfix_cb(void (* cb)(struct CscGPSFixMsg *msg));
-void csc_ap_link_set_gpspos_cb(void (* cb)(struct CscGPSPosMsg *msg));
-void csc_ap_link_set_gpsacc_cb(void (* cb)(struct CscGPSAccMsg *msg));
-void csc_ap_link_send_bat(uint16_t volts, uint16_t amps, uint8_t msgctr);
-
-#endif /* CSC_AP_LINK_H */
-
diff --git a/sw/airborne/csc/csc_ap_main.c b/sw/airborne/csc/csc_ap_main.c
deleted file mode 100644
index 012d151e38..0000000000
--- a/sw/airborne/csc/csc_ap_main.c
+++ /dev/null
@@ -1,221 +0,0 @@
-/*
- * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $
- *
- * Copyright (C) 2008  Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include <inttypes.h>
-
-#include "csc_main.h"
-
-#include "std.h"
-
-#include "mcu.h"
-#include "sys_time.h"
-#include "led.h"
-#include "interrupt_hw.h"
-#include "mcu_periph/uart.h"
-#include "downlink.h"
-#include "generated/periodic.h"
-#include "generated/airframe.h"
-#include "commands.h"
-#include "subsystems/radio_control.h"
-#include "booz/booz2_gps.h"
-
-//#include "ap_downlink.h"
-
-#include "csc_servos.h"
-#include "csc_telemetry.h"
-#include "csc_adc.h"
-#include "csc_xsens.h"
-#include "csc_autopilot.h"
-#include "csc_can.h"
-#include "pwm_input.h"
-#include "csc_ap_link.h"
-#include "led.h"
-
-#include "pprz_transport.h"
-
-#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
-
-#define PPRZ_MODE_MANUAL 0
-#define PPRZ_MODE_AUTO1 1
-
-#define TRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
-#define PPRZ_MODE_OF_RC(mode) ((mode) < TRESHOLD_MANUAL_PPRZ ? PPRZ_MODE_MANUAL : PPRZ_MODE_AUTO1)
-
-extern uint8_t vsupply;
-
-uint8_t pprz_mode = PPRZ_MODE_AUTO1;
-static uint16_t cpu_time = 0;
-uint8_t csc_trims_set = 0;
-
-struct Booz_gps_state booz_gps_state;
-struct NedCoor_i booz_ins_gps_pos_cm_ned;
-struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
-
-static void nop(struct CscCanMsg *msg)
-{
-
-}
-
-#define RADIO_SCALE 20
-#define ROLL_OFFSET 1544
-#define PITCH_OFFSET 2551
-#define YAW_OFFSET 3585
-#define MODE_OFFSET 5632
-
-static void on_rc_cmd(struct CscRCMsg *msg)
-{
-  int aux2_flag;
-  static int last_aux2_flag = -1;
-
-  rc_values[RADIO_ROLL]  = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET);
-  rc_values[RADIO_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET);
-  rc_values[RADIO_YAW]   =  CSC_RC_SCALE*((msg->left_stick_horizontal_and_aux2 & ~(3 << 13)) - CSC_RC_OFFSET);
-  uint8_t mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13;
-  rc_values[RADIO_MODE]  =  mode ? -7000 : ( (mode == 1) ? 0 : 7000);
-  aux2_flag = (msg->left_stick_horizontal_and_aux2 >> 13) & 0x1;
-  rc_values[RADIO_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000);
-  rc_values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET);
-
-  time_since_last_ppm = 0;
-  rc_status = RC_OK;
-  pprz_mode = PPRZ_MODE_OF_RC(rc_values[RADIO_MODE]);
-  if (pprz_mode == PPRZ_MODE_MANUAL) {
-    csc_ap_clear_ierrors();
-    SetCommandsFromRC(commands, rc_values);
-  }
-}
-
-static void on_gpsacc_cmd( struct CscGPSAccMsg *msg )
-{
-  booz_gps_state.pacc = msg->pacc;
-  booz_gps_state.sacc = msg->sacc;
-}
-
-static void on_gpsfix_cmd( struct CscGPSFixMsg *msg )
-{
-  booz_gps_state.fix = msg->gps_fix;
-  booz_gps_state.num_sv = msg->num_sv;
-  booz_ins_gps_speed_cm_s_ned.x = msg->vx;
-  booz_ins_gps_speed_cm_s_ned.y = msg->vy;
-  booz_ins_gps_speed_cm_s_ned.z = msg->vz;
-}
-
-static void on_gpspos_cmd( struct CscGPSPosMsg *msg )
-{
-  switch (msg->axis)  {
-    case CSC_GPS_AXIS_IDX_X:
-      booz_ins_gps_pos_cm_ned.x = msg->val;
-      break;
-    case CSC_GPS_AXIS_IDX_Y:
-      booz_ins_gps_pos_cm_ned.y = msg->val;
-      break;
-    case CSC_GPS_AXIS_IDX_Z:
-      booz_ins_gps_pos_cm_ned.z = msg->val;
-      break;
-    default:
-      // Invalid msg
-      break;
-  }
-}
-
-static void csc_main_init( void ) {
-
-  mcu_init();
-  sys_time_init();
-  led_init();
-
-  Uart0Init();
-  Uart1Init();
-
-  xsens_init();
-
-  csc_adc_init();
-  ppm_init();
-
-#ifdef USE_PWM_INPUT
-  pwm_input_init();
-#endif
-
-  csc_ap_link_init();
-  csc_ap_link_set_servo_cmd_cb(&nop);
-  csc_ap_link_set_motor_cmd_cb(&nop);
-  csc_ap_link_set_rc_cmd_cb(on_rc_cmd);
-  csc_ap_link_set_gpsfix_cb(on_gpsfix_cmd);
-  csc_ap_link_set_gpspos_cb(on_gpspos_cmd);
-  csc_ap_link_set_gpsacc_cb(on_gpsacc_cmd);
-  actuators_init();
-
-  csc_ap_init();
-  mcu_int_enable();
-
-}
-
-
-static void csc_main_periodic( void )
-{
-  static uint32_t csc_loops = 0;
-
-  PeriodicSendAp(DefaultChannel);
-  radio_control_periodic_task();
-
-  if (rc_status == RC_REALLY_LOST) {
-      pprz_mode = PPRZ_MODE_AUTO1;
-  }
-  cpu_time++;
-
-  if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
-    csc_adc_periodic();
-  }
-  xsens_periodic_task();
-  if (pprz_mode == PPRZ_MODE_AUTO1)
-    csc_ap_periodic(cpu_time, &booz_ins_gps_pos_cm_ned, &booz_ins_gps_speed_cm_s_ned);
-
-  if (csc_trims_set) {
-    csc_trims_set = 0;
-    csc_ap_set_trims();
-  }
-
-#ifdef ACTUATORS
-  SetActuatorsFromCommands(commands);
-#endif
-  SendCscFromActuators();
-
-}
-
-static void csc_main_event( void )
-{
-  csc_can_event();
-  xsens_event_task();
-  DatalinkEvent();
-}
-
-int main( void ) {
-  csc_main_init();
-  while(1) {
-    if (sys_time_periodic())
-      csc_main_periodic();
-    csc_main_event();
-  }
-  return 0;
-}
diff --git a/sw/airborne/csc/csc_ap_main.h b/sw/airborne/csc/csc_ap_main.h
deleted file mode 100644
index 22e27b5959..0000000000
--- a/sw/airborne/csc/csc_ap_main.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#ifndef CSC_AP_MAIN_H
-#define CSC_AP_MAIN_H
-
-extern uint8_t csc_trims_set;
-
-#endif /* CSC_AP_MAIN_H */
diff --git a/sw/airborne/csc/csc_autopilot.c b/sw/airborne/csc/csc_autopilot.c
deleted file mode 100644
index 5300708d9b..0000000000
--- a/sw/airborne/csc/csc_autopilot.c
+++ /dev/null
@@ -1,293 +0,0 @@
-/*
- * Copyright (C) 2009 Joby Energy
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file csc_autopilot.c
- */
-
-#include "csc_autopilot.h"
-
-#include <inttypes.h>
-#include <math.h>
-
-#include "commands.h"
-#include "csc_xsens.h"
-#include "led.h"
-#include "math/pprz_algebra_float.h"
-#include "string.h"
-#include "subsystems/radio_control.h"
-#include "pwm_input.h"
-#include "LPC21xx.h"
-#include "print.h"
-#include "booz/booz2_gps.h"
-
-struct control_gains csc_gains;
-struct control_reference csc_reference;
-struct control_reference csc_errors;
-struct control_trims csc_trims;
-struct control_gains csc_gamma;
-float csc_vane_angle;
-float csc_vane_angle_offset = 180;
-
-static const int xsens_id = 0;
-float csc_yaw_rudder;
-float csc_yaw_aileron;
-float csc_yaw_deadband;
-float csc_yaw_setpoint_rate;
-float csc_yaw_setpoint_range;
-float csc_vane_weight;
-float csc_vane_filter_constant;
-
-float csc_drag_pitch = 0;
-float csc_drag_yaw = 0;
-
-static float csc_gps_zero_x = 0;
-static float csc_gps_zero_y = 0;
-struct gps_reference csc_gps_errors;
-int csc_gps_setzero = 0;
-float csc_gps_weight = 0;
-float csc_gps_gain = 0;
-float csc_gps_filter_weight = 0;
-
-#define PWM_INPUT_COUNTS_PER_REV 61358.
-
-#define BOOZ2_NED_METERS 0.00380625
-#define BOOZ2_NED_METERS_SEC 0.0000019073
-
-static void update_vane_angle( void )
-{
-  csc_vane_angle =  (csc_vane_filter_constant * csc_vane_angle) + (1-csc_vane_filter_constant)*RadOfDeg( 360. * pwm_input_duration / PWM_INPUT_COUNTS_PER_REV) - RadOfDeg(csc_vane_angle_offset);
-}
-
-void csc_ap_init( void )
-{
-  csc_gains.roll_kp = 4218;
-  csc_gains.roll_kd = 1000;
-  csc_gains.roll_ki = 50;
-  csc_gains.pitch_kp = 0;
-  csc_gains.pitch_kd = 0;
-  csc_gains.pitch_ki = 0;
-  csc_gains.yaw_kp = 5000;
-  csc_gains.yaw_kd = 800;
-  csc_gains.yaw_ki = 10;
-  csc_yaw_rudder = 0.33;
-  csc_yaw_aileron = 1.00;
-  csc_yaw_deadband = 1.00;
-  csc_vane_weight = 0.1;
-
-  csc_trims.elevator = 2020;
-  csc_trims.aileron = -80;
-  csc_trims.rudder = 80;
-
-  csc_gps_weight = 0;
-  csc_gps_gain = 1;
-  csc_gps_filter_weight = 0;
-
-  csc_gamma.roll_kp = 0;
-  csc_gamma.roll_kd = 0;
-  csc_gamma.roll_ki = 0;
-  csc_gamma.pitch_kp = 0;
-  csc_gamma.pitch_kd = 0;
-  csc_gamma.pitch_ki = 0;
-  csc_gamma.yaw_kp = 0;
-  csc_gamma.yaw_kd = 0;
-  csc_gamma.yaw_ki = 0;
-
-  memset(&csc_reference, 0, sizeof(struct control_reference));
-}
-
-static void update_ki(float new_ki, float *ki, float *ierror)
-{
-  if (new_ki == 0) {
-    *ierror = 0;
-  } else {
-    *ierror *= *ki / new_ki;
-  }
-  *ki = new_ki;
-}
-
-#define I_CMD_BOUND 0.4
-
-static void bound_ierror(float igain, float *ierror)
-{
-  float cmd = *ierror * igain;
-  if (cmd > (MAX_PPRZ * I_CMD_BOUND)) {
-    *ierror = MAX_PPRZ * I_CMD_BOUND / igain;
-  } else if (cmd < (MIN_PPRZ * I_CMD_BOUND)) {
-    *ierror = MIN_PPRZ * I_CMD_BOUND / igain;
-  }
-}
-
-static float compute_ref_trajectory()
-{
-
-}
-
-void csc_autopilot_set_roll_ki(float ki)
-{
-  update_ki(ki, &csc_gains.roll_ki, &csc_errors.eulers_i.phi);
-}
-
-void csc_autopilot_set_pitch_ki(float ki)
-{
-  update_ki(ki, &csc_gains.pitch_ki, &csc_errors.eulers_i.theta);
-}
-
-void csc_autopilot_set_yaw_ki(float ki)
-{
-  update_ki(ki, &csc_gains.yaw_ki, &csc_errors.eulers_i.psi);
-}
-
-static void csc_zero_gps(struct NedCoor_i *ned_pos, struct NedCoor_i *ned_vel)
-{
-  csc_gps_zero_x = ned_pos->x*BOOZ2_NED_METERS;
-  csc_gps_zero_y = ned_pos->y*BOOZ2_NED_METERS;
-  csc_gps_setzero = 0;
-}
-
-static void calculate_gps_errors(struct gps_reference *error, struct NedCoor_i *ned_pos, struct NedCoor_i *ned_vel)
-{
-  error->pos.x = (ned_pos->x*BOOZ2_NED_METERS - csc_gps_zero_x)*csc_gps_gain*(1 - csc_gps_filter_weight) + error->pos.x*csc_gps_filter_weight;
-  error->pos.y = (ned_pos->y*BOOZ2_NED_METERS - csc_gps_zero_y)*csc_gps_gain*(1 - csc_gps_filter_weight) + error->pos.x*csc_gps_filter_weight;
-  error->rate.x = -ned_vel->x*BOOZ2_NED_METERS_SEC*csc_gps_gain*(1 - csc_gps_filter_weight) + error->pos.x*csc_gps_filter_weight;
-  error->rate.y = -ned_vel->y*BOOZ2_NED_METERS_SEC*csc_gps_gain*(1 - csc_gps_filter_weight) + error->pos.x*csc_gps_filter_weight;
-}
-
-static void calculate_errors(struct control_reference *errors)
-{
-  struct FloatEulers xsens_eulers;
-  struct FloatRates xsens_rates;
-
-  xsens_eulers.phi = xsens_phi[xsens_id];
-  xsens_eulers.theta = xsens_theta[xsens_id];
-  xsens_eulers.psi =  xsens_psi[xsens_id];
-
-  xsens_rates.p = xsens_gyro_x[xsens_id];
-  xsens_rates.q = xsens_gyro_y[xsens_id];
-  xsens_rates.r = xsens_gyro_z[xsens_id];
-
-  errors->eulers.phi = xsens_eulers.phi - csc_reference.eulers.phi;
-  errors->eulers.theta = xsens_eulers.theta - csc_reference.eulers.theta;
-  // The following mess mixes in the wind vane and the GPS errors in
-  // an arbitrary fashion.
-  errors->eulers.psi = (csc_vane_angle*csc_vane_weight)
-    + (xsens_eulers.psi - csc_reference.eulers.psi)*(1 - csc_vane_weight);
-  errors->eulers.psi = (1 - csc_gps_weight)*errors->eulers.psi
-    + csc_gps_weight*(cos(xsens_eulers.psi)*csc_gps_errors.pos.x
-       + sin(xsens_eulers.psi)*csc_gps_errors.pos.y);
-
-  errors->rates.p = xsens_rates.p - csc_reference.rates.p;
-  errors->rates.q = xsens_rates.q - csc_reference.rates.q;
-  errors->rates.r = xsens_rates.r - csc_reference.rates.r;
-
-  errors->eulers_i.phi += xsens_eulers.phi;
-  errors->eulers_i.theta += xsens_eulers.theta;
-  errors->eulers_i.psi += xsens_eulers.psi;
-
-  /* Deadband in yaw -- prevents it going nuts around an angle */
-  float yaw_deadband = RadOfDeg(csc_yaw_deadband);
-
-  if (errors->eulers.psi <= yaw_deadband && errors->eulers.psi >= -yaw_deadband) {
-    errors->eulers.psi = 0;
-  } else if (errors->eulers.psi <= yaw_deadband*2 && errors->eulers.psi >= -yaw_deadband*2) {
-    if (errors->eulers.psi < 0) {
-      errors->eulers.psi = (errors->eulers.psi + yaw_deadband)*2;
-    } else
-      errors->eulers.psi = (errors->eulers.psi - yaw_deadband)*2;
-  }
-
-  bound_ierror(csc_gains.roll_ki, &errors->eulers_i.phi);
-  bound_ierror(csc_gains.pitch_ki, &errors->eulers_i.theta);
-  bound_ierror(csc_gains.yaw_ki, &errors->eulers_i.psi);
-}
-
-static void calculate_reference(struct control_reference *reference, int time)
-{
-  reference->eulers.psi = M_PI  / 6.0 * rc_values[RADIO_YAW] + 100*csc_yaw_setpoint_range*sin(0.01*time*csc_yaw_setpoint_rate);
-  reference->eulers.theta = M_PI  / 6.0 * rc_values[RADIO_PITCH];
-  reference->eulers.phi = M_PI / 6.0 * rc_values[RADIO_ROLL];
-
-  reference->eulers.phi /= MAX_PPRZ;
-  reference->eulers.theta /= MAX_PPRZ;
-  reference->eulers.psi /= MAX_PPRZ;
-}
-
-void csc_ap_periodic(int time, struct NedCoor_i *ned_pos, struct NedCoor_i *ned_vel)
-{
-  //static int counter = 0;
-  update_vane_angle();
-  calculate_reference(&csc_reference, time);
-  if (csc_gps_setzero)
-    csc_zero_gps(ned_pos, ned_vel);
-  calculate_gps_errors(&csc_gps_errors, ned_pos, ned_vel);
-  calculate_errors(&csc_errors);
-
-  commands[COMMAND_ROLL] = -csc_gains.roll_kp * (csc_errors.eulers.phi)
-                          + csc_gains.roll_kd * (csc_errors.rates.p)
-                          - csc_gains.roll_ki * (csc_errors.eulers_i.phi);
-  commands[COMMAND_ROLL] += csc_trims.aileron;
-
-  commands[COMMAND_PITCH] = -csc_gains.pitch_kp * csc_errors.eulers.theta
-			   + csc_gains.pitch_kd * csc_errors.rates.q
-			   - csc_gains.pitch_ki * csc_errors.eulers_i.theta;
-  commands[COMMAND_PITCH] += csc_trims.elevator;
-
-  commands[COMMAND_ROLL] += (-csc_gains.yaw_kp * csc_errors.eulers.psi
-			   - csc_gains.yaw_kd * csc_errors.rates.r
-			     - csc_gains.yaw_ki * csc_errors.eulers_i.psi) * csc_yaw_aileron;
-
-  commands[COMMAND_YAW] = csc_trims.rudder;
-
-  commands[COMMAND_YAW] += (-csc_gains.yaw_kp * csc_errors.eulers.psi
-			   - csc_gains.yaw_kd * csc_errors.rates.r
-			    - csc_gains.yaw_ki * csc_errors.eulers_i.psi) * csc_yaw_rudder;
-
-  csc_ap_update_gains(&csc_errors, &csc_gains);
-}
-
-void csc_ap_clear_ierrors( void )
-{
-  csc_errors.eulers_i.phi = 0;
-  csc_errors.eulers_i.theta = 0;
-  csc_errors.eulers_i.psi = 0;
-}
-
-void csc_ap_set_trims( void )
-{
-  csc_trims.aileron = commands[COMMAND_ROLL];
-  csc_trims.elevator = commands[COMMAND_PITCH];
-  csc_trims.rudder = commands[COMMAND_YAW];
-}
-
-void csc_ap_update_gains(struct control_reference *errors, struct control_gains *gains)
-{
-  // Adaptation law based on state error to be controlled, rate and i error
-  gains->pitch_kp += -csc_gamma.pitch_kp*errors->eulers.theta*fabs(errors->eulers.theta);
-  gains->pitch_kd += -csc_gamma.pitch_kd*errors->eulers.theta*xsens_gyro_y[xsens_id];
-  gains->pitch_ki += -csc_gamma.pitch_ki*errors->eulers.theta*errors->eulers_i.theta;
-  gains->roll_kp += -csc_gamma.roll_kp*errors->eulers.phi*fabs(errors->eulers.phi);
-  gains->roll_kd += -csc_gamma.roll_kd*errors->eulers.phi*xsens_gyro_x[xsens_id];
-  gains->roll_ki += -csc_gamma.roll_ki*errors->eulers.phi*errors->eulers_i.phi;
-  gains->yaw_kp += -csc_gamma.yaw_kp*errors->eulers.psi*fabs(errors->eulers.psi);
-  gains->yaw_kd += -csc_gamma.yaw_kd*errors->eulers.psi*xsens_gyro_z[xsens_id];
-  gains->yaw_ki += -csc_gamma.yaw_ki*errors->eulers.psi*errors->eulers_i.psi;
-}
-
diff --git a/sw/airborne/csc/csc_autopilot.h b/sw/airborne/csc/csc_autopilot.h
deleted file mode 100644
index c41cd7bcff..0000000000
--- a/sw/airborne/csc/csc_autopilot.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * Copyright (C) 2008 Joby Energy
- *
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file csc_autpilot.h
- */
-
-#ifndef __CSC_AUTOPILOT_H__
-#define __CSC_AUTOPILOT_H__
-
-#include "types.h"
-#include "std.h"
-#include "math/pprz_algebra_float.h"
-#include "math/pprz_geodetic_int.h"
-
-struct control_gains {
-  float pitch_kp;
-  float pitch_kd;
-  float pitch_ki;
-  float roll_kp;
-  float roll_kd;
-  float roll_ki;
-  float yaw_kp;
-  float yaw_kd;
-  float yaw_ki;
-};
-
-struct control_reference {
-  struct FloatEulers eulers;
-  struct FloatRates rates;
-  struct FloatEulers eulers_i;
-};
-
-struct gps_reference {
-  struct FloatVect3 pos;
-  struct FloatVect3 rate;
-};
-
-struct control_trims {
-  int elevator;
-  int aileron;
-  int rudder;
-};
-
-extern struct control_gains csc_gains;
-extern struct control_reference csc_reference;
-extern struct control_trims csc_trims;
-extern struct control_gains csc_gamma;
-extern float csc_vane_angle;
-extern float csc_vane_angle_offset;
-extern float csc_vane_weight;
-extern float csc_yaw_rudder;
-extern float csc_yaw_aileron;
-extern float csc_yaw_deadband;
-extern float csc_yaw_setpoint_rate;
-extern float csc_yaw_setpoint_range;
-extern float csc_vane_filter_constant;
-extern float csc_drag_pitch;
-extern float csc_drag_yaw;
-
-extern struct gps_reference csc_gps_errors;
-extern int csc_gps_setzero;
-extern float csc_gps_weight;
-extern float csc_gps_gain;
-extern float csc_gps_filter_weight;
-
-void csc_autopilot_set_roll_ki(float ki);
-void csc_autopilot_set_pitch_ki(float ki);
-void csc_autopilot_set_yaw_ki(float ki);
-
-void csc_ap_init( void );
-void csc_ap_periodic (int time, struct NedCoor_i *ned_pos, struct NedCoor_i *ned_vel);
-void csc_ap_set_trims (void );
-void csc_ap_clear_ierrors (void );
-void csc_ap_update_gains(struct control_reference *errors, struct control_gains *gains);
-
-#define PERIODIC_SEND_VANE_SENSOR(_chan) DOWNLINK_SEND_VANE_SENSOR(_chan, &csc_vane_angle)
-
-#endif
diff --git a/sw/airborne/csc/csc_baro.c b/sw/airborne/csc/csc_baro.c
deleted file mode 100644
index 22649b4f4a..0000000000
--- a/sw/airborne/csc/csc_baro.c
+++ /dev/null
@@ -1,169 +0,0 @@
-#include "std.h"
-#include "mcu.h"
-#include "sys_time.h"
-#include "led.h"
-#include "interrupt_hw.h"
-
-#include "mcu_periph/uart.h"
-#include "messages.h"
-#include "downlink.h"
-#include "csc_booz2_ins.h"
-#include "csc_ap_link.h"
-
-#include "mcu_periph/spi_arch.h"
-
-#include "csc_baro.h"
-
-uint8_t  baro_scp_status;
-uint32_t baro_scp_pressure;
-uint16_t baro_scp_temperature;
-float baro_scp_alt;
-bool_t baro_scp_available;
-
-static void baro_scp_start_high_res_measurement(void);
-static void spi1_isr(void);
-
-void baro_scp_periodic(void) {
-  if (cpu_time_sec > 1) {
-    if (baro_scp_status == STA_UNINIT) {
-      baro_scp_start_high_res_measurement();
-      baro_scp_status = STA_INITIALISING;
-    } else {
-      spi1_isr();
-    }
-  }
-}
-
-#define SS_PIN  30
-#define SS_IODIR IO0DIR
-#define SS_IOSET IO0SET
-#define SS_IOCLR IO0CLR
-
-#define ScpSelect()   SetBit(SS_IOCLR,SS_PIN)
-#define ScpUnselect() SetBit(SS_IOSET,SS_PIN)
-
-// DRDY1 aka P0.15
-#define DRDY1_PIN 15
-#define DRDY1_IODIR IO0DIR
-#define DRDY1_IOPIN IO0PIN
-#define DRDY1_PINSEL() PINSEL0 = (PINSEL0 & ~(3 << 30)) | (0 << 30)
-
-#define SSP1_SCK_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 2)) | (2 << 2))
-#define SSP1_MISO_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 4)) | (2 << 4))
-#define SSP1_MOSI_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 6)) | (2 << 6))
-#define SSP1_SSEL_PINSEL() (PINSEL1 = (PINSEL1 & ~(3 << 8)) | (2 << 8))
-
-static uint8_t baro_scp_read_reg_8(uint8_t regno)
-{
-  uint8_t value;
-  uint8_t foo0 __attribute__ ((unused));
-
-  ScpSelect();
-  sys_time_usleep(20);
-  S1SPDR = regno << 2;
-  while(~S1SPSR & _BV(7));
-  foo0 = S1SPDR;
-
-  S1SPDR = 0;
-  while(~S1SPSR & _BV(7));
-  value = S1SPDR;
-  sys_time_usleep(10);
-  ScpUnselect();
-
-  return value;
-}
-
-static void baro_scp_write_reg_8(uint8_t regno, uint8_t value)
-{
-  uint8_t foo0 __attribute__ ((unused));
-  uint8_t foo1 __attribute__ ((unused));
-
-  ScpSelect();
-
-  foo0 = S1SPSR;
-  foo0 = S1SPDR;
-
-  sys_time_usleep(20);
-  S1SPDR = (regno << 2) | (1 << 1);
-  while(~S1SPSR & _BV(7));
-  foo0 = S1SPDR;
-
-  S1SPDR = value;
-  foo1 =  S1SPDR;
-  while(~S1SPSR & _BV(7));
-
-  sys_time_usleep(10);
-  ScpUnselect();
-}
-
-static uint16_t baro_scp_read_reg_16(uint8_t regno)
-{
-  uint8_t lsb, msb;
-  uint8_t foo0 __attribute__ ((unused));
-
-  ScpSelect();
-  sys_time_usleep(20);
-  S1SPDR = regno << 2;
-  while(~S1SPSR & _BV(7));
-  foo0 = S1SPDR;
-
-  S1SPDR = 0;
-  while(~S1SPSR & _BV(7));
-  msb = S1SPDR;
-
-  S1SPDR = 0;
-  while(~S1SPSR & _BV(7));
-  lsb = S1SPDR;
-
-  sys_time_usleep(10);
-  ScpUnselect();
-
-
-  return (uint16_t) ((msb << 8) | lsb);
-}
-
-void baro_scp_init( void )
-{
-
-  SSP1_SCK_PINSEL();
-  SSP1_MISO_PINSEL();
-  SSP1_MOSI_PINSEL();
-  SSP1_SSEL_PINSEL();
-  DRDY1_PINSEL();
-
-  S1SPCR = _BV(3) | _BV(4) | _BV(5);
-  S1SPCCR = 0x80;
-
-  baro_scp_status = STA_UNINIT;
-}
-
-void spi1_isr()
-{
-  uint32_t msb;
-  uint16_t lsb;
-
-  if (bit_is_set(DRDY1_IOPIN, DRDY1_PIN)) {
-    sys_time_usleep(10);
-    baro_scp_temperature = baro_scp_read_reg_16(0x21);
-    msb  = baro_scp_read_reg_8(0x1F);
-    lsb  = baro_scp_read_reg_16(0x20);
-
-    //if (baro_scp_temperature & 0x2000) {
-     // baro_scp_temperature |= 0xC000;
-    //}
-    baro_scp_temperature = baro_scp_temperature & (0xFFFF >> 2);
-    baro_scp_temperature *= 5;
-
-    baro_scp_pressure = lsb;
-    baro_scp_pressure |= (msb << 16);
-    baro_scp_pressure *= 25;
-
-    //baro_scp_alt = 9000 - baro_scp_pressure * 0.00084366;
-    baro_scp_status = STA_VALID;
-  }
-}
-
-/* write 0x0A to 0x03 */
-static void baro_scp_start_high_res_measurement(void) {
-  baro_scp_write_reg_8(0x3, 0xA);
-}
diff --git a/sw/airborne/csc/csc_baro.h b/sw/airborne/csc/csc_baro.h
deleted file mode 100644
index 40e9995a17..0000000000
--- a/sw/airborne/csc/csc_baro.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef CSC_BARO_H
-#define CSC_BARO_H
-
-#include "std.h"
-
-#define STA_UNINIT       0
-#define STA_INITIALISING 1
-#define STA_IDLE         2
-
-extern uint8_t  baro_scp_status;
-extern uint32_t baro_scp_pressure;
-extern uint16_t baro_scp_temperature;
-extern float baro_scp_alt;
-extern bool_t baro_scp_available;
-
-void baro_scp_init(void);
-void baro_scp_periodic(void);
-
-#define STA_UNINIT       0
-#define STA_INITIALISING 1
-#define STA_VALID         2
-
-#define PERIODIC_SEND_BARO_MS5534A() DOWNLINK_SEND_BARO_MS5534A( \
-  &baro_scp_pressure, \
-  &baro_scp_temperature, \
-  &baro_scp_alt \
-  )
-
-#endif
diff --git a/sw/airborne/csc/csc_can.c b/sw/airborne/csc/csc_can.c
deleted file mode 100644
index 3ca2d2dcb9..0000000000
--- a/sw/airborne/csc/csc_can.c
+++ /dev/null
@@ -1,232 +0,0 @@
-#include "csc_can.h"
-#include "string.h"
-
-#include "LPC21xx.h"
-#include "armVIC.h"
-
-#include "led.h"
-
-
-#ifdef USE_CAN1
-
-#define QUEUE_LEN 8
-
-struct MsgQueue {
-  int head;
-  int tail;
-  int full;
-  struct CscCanMsg msgs[QUEUE_LEN];
-};
-
-static struct MsgQueue can1_tx_queue;
-static struct MsgQueue can1_rx_queue;
-
-bool_t can1_msg_received;
-struct CscCanMsg can1_rx_msg;
-static void (* can1_callback)(struct CscCanMsg *);
-
-static void CAN1_Rx_ISR ( void ) __attribute__((naked));
-/*static void CAN1_Tx_ISR ( void ) __attribute__((naked));*/
-static void CAN1_Err_ISR ( void ) __attribute__((naked));
-
-static void msg_queue_init(struct MsgQueue *q)
-{
-  q->head = 0;
-  q->tail = 0;
-  q->full = 0;
-}
-
-static int msg_queue_full(struct MsgQueue *q)
-{
-  return q->full;
-}
-
-static int msg_queue_empty(struct MsgQueue *q)
-{
-  return (q->head == q->tail) && !q->full;
-}
-
-static void msg_enqueue(struct MsgQueue *q, struct CscCanMsg *msg)
-{
-  memcpy(&q->msgs[q->tail], msg, sizeof (struct CscCanMsg));
-  q->tail = (q->tail + 1) % QUEUE_LEN;
-  if (q->head == q->tail) q->full = 1;
-}
-
-static void msg_dequeue(struct MsgQueue *q, struct CscCanMsg *msg)
-{
-  memcpy(msg, &q->msgs[q->head], sizeof(struct CscCanMsg));
-  q->head = (q->head + 1) % QUEUE_LEN;
-  q->full = 0;
-}
-
-
-static void can1_hw_init(void)
-{
-  // Acceptance Filter Mode Register = filter off, receive all
-  AFMR = 0x00000002;
-
-  // Go into Reset mode
-  C1MOD =  1;
-  // Clear Status register (including error counters)
-  C1GSR = 0;
-
-  // Set bit timing
-  C1BTR = CAN1_BTR;
-
-  // Disable All Interrupts
-  C1IER = 0;
-
-  // Enable error interrupts if handled
-#ifdef  CAN1_ERR_VIC_SLOT
-  C1IER =  (1<<0) | /* RIE */
-           (1<<2) | /* EIE */
-           (1<<6) | /* ALIE */
-           (1<<7) | /* BEIE */
-           (1<<7)   /* BEIE */
-    ;
-#endif
-
-  // Get out of reset Mode
-  C1MOD = 0;
-}
-
-void csc_can1_init(void(* callback)(struct CscCanMsg *msg))
-{
-
-  // Initialize the interrupt vector
-  VICIntSelect &= ~VIC_BIT(VIC_CAN1_RX);               // VIC_CAN1_RX selected as IRQ
-  VICIntEnable = VIC_BIT(VIC_CAN1_RX);                 // VIC_CAN1_RX interrupt enabled
-  _VIC_CNTL(CAN1_VIC_SLOT) = VIC_ENABLE | VIC_CAN1_RX; //
-  _VIC_ADDR(CAN1_VIC_SLOT) = (uint32_t)CAN1_Rx_ISR;    // address of the ISR
-
-  // intitialze error interrupt
-#ifdef  CAN1_ERR_VIC_SLOT
-  VICIntSelect &= ~VIC_BIT(VIC_CAN);                  // VIC_CAN selected as IRQ
-  VICIntEnable = VIC_BIT(VIC_CAN);                    // VIC_CAN interrupt enabled
-  _VIC_CNTL(CAN1_ERR_VIC_SLOT) = VIC_ENABLE | VIC_CAN; //
-  _VIC_ADDR(CAN1_ERR_VIC_SLOT) = (uint32_t)CAN1_Err_ISR;
-#endif
-
-  // Set bit 18
-  PINSEL1 |= _BV(18);
-
-  // set can callback before enabling interrupts
-  can1_callback = callback;
-
-  msg_queue_init(&can1_tx_queue);
-  msg_queue_init(&can1_rx_queue);
-
-  // initialize actual hardware
-  can1_hw_init();
-}
-
-static inline uint32_t csc_can1_tx_available()
-{
-  return (C1SR & 0x00000004L);
-}
-
-
-static inline int can1_bus_off()
-{
-  // check for bit seven of CAN1 GSR (Bus-off state)
-  return C1SR & _BV(7);
-}
-
-void csc_can1_send(struct CscCanMsg* msg) {
-
-  if (can1_bus_off()) {
-    can1_hw_init();
-  }
-
-  if (!csc_can1_tx_available()) { /* transmit channel not available */
-    if (! msg_queue_full(&can1_tx_queue)) {
-      msg_enqueue(&can1_tx_queue, msg);
-    }
-
-    //    LED_ON(2);
-    return;
-  }
-  //  LED_OFF(2);
-
-  // Write DLC, RTR and FF
-  C1TFI1 = (msg->frame &  0xC00F0000L);
-  // Write CAN ID
-  C1TID1 = msg->id;
-  // Write first 4 data bytes
-  C1TDA1 = msg->dat_a;
-  // Write second 4 data bytes
-  C1TDB1 = msg->dat_b;
-  // Write self reception request
-  //  C1CMR = 0x30;
-  // write transmission request
-  C1CMR = 0x21;
-
-}
-
-void CAN1_Rx_ISR ( void ) {
- ISR_ENTRY();
-
- can1_rx_msg.id = C1RID;
- if (BOARDID_OF_CANMSG_ID(can1_rx_msg.id) == CSC_BOARD_ID) {
-   can1_rx_msg.frame  = C1RFS;
-   can1_rx_msg.dat_a  = C1RDA;
-   can1_rx_msg.dat_b  = C1RDB;
-   can1_msg_received = TRUE;
-    if (! msg_queue_full(&can1_rx_queue)) {
-      msg_enqueue(&can1_rx_queue, &can1_rx_msg);
-   }
-  }
-
-
- C1CMR = 0x04;             // release receive buffer
- VICVectAddr = 0x00000000; // acknowledge interrupt
-
- ISR_EXIT();
-}
-
-
-/*
-void CAN1_Tx_ISR ( void ) {
- ISR_ENTRY();
-
- ISR_EXIT();
-}
-*/
-
-#endif /* USE_CAN1 */
-
-void csc_can_event(void)
-{
-#ifdef USE_CAN1
-
-  // drain the RX Queue
-  while(!msg_queue_empty(&can1_rx_queue)) {
-    struct CscCanMsg rx_msg;
-    //cpsr = disableIRQ();                  // disable global interrupts
-    msg_dequeue(&can1_rx_queue, &rx_msg);
-    can1_callback(&rx_msg);
-    //restoreIRQ(cpsr);                     // restore global interrupts
-  }
-
-  if(!msg_queue_empty(&can1_tx_queue) && csc_can1_tx_available()) {
-    struct CscCanMsg msg;
-    msg_dequeue(&can1_tx_queue, &msg);
-    csc_can1_send(&msg);
-  }
-
-#endif /* USE_CAN1 */
-}
-
-static uint32_t err_cnt = 0;
-
-void CAN1_Err_ISR ( void ) {
- ISR_ENTRY();
- uint32_t __attribute__ ((unused)) c1icr = C1ICR;
-
- err_cnt++;
- /* LED_ON(ERROR_LED); */
-
- VICVectAddr = 0x00000000; // acknowledge interrupt
- ISR_EXIT();
-}
diff --git a/sw/airborne/csc/csc_can.h b/sw/airborne/csc/csc_can.h
deleted file mode 100644
index fb491e748a..0000000000
--- a/sw/airborne/csc/csc_can.h
+++ /dev/null
@@ -1,46 +0,0 @@
-#ifndef CSC_CAN_H
-#define CSC_CAN_H
-
-#include <inttypes.h>
-#include "std.h"
-
-
-#define BOARDID_OF_CANMSG_ID(_id) (((_id)>>7) & CSC_BOARD_MASK)
-#define MSGID_OF_CANMSG_ID(_id) ((_id) & CSC_MSGID_MASK)
-#define CAN_MSG_LENGTH_OF_FRAME(_f) (((_f)>>16) & 0x0F)
-
-// Common CAN bit rates (assumes 30Mhz PCLK)
-#define   CANBitrate62k5_1MHz           0x001C001D
-#define   CANBitrate125k_2MHz           0x001C000E
-#define   CANBitrate156k25_2_5MHz       0x001C000B
-#define   CANBitrate187k5_3MHz          0x001C0009
-#define   CANBitrate312k5_5MHz          0x001C0005
-#define   CANBitrate375k_6MHz           0x001C0004
-
-#define CSC_BOARD_MASK 0x0F
-#define CSC_MSGID_MASK 0x7F
-
-struct CscCanMsg {
-  uint32_t frame;  // Bits 16..19: DLC - Data Length Counter
-                   // Bit 30: Set if this is a RTR message
-                   // Bit 31: Set if this is a 29-bit ID message
-  uint32_t id;     // CAN Message ID (11-bit or 29-bit)
-  uint32_t dat_a;  // CAN Message Data Bytes 0-3
-  uint32_t dat_b;  // CAN Message Data Bytes 4-7
-};
-
-void csc_can_event(void);
-
-#ifdef USE_CAN1
-void csc_can1_init(void(* callback)(struct CscCanMsg *msg));
-void csc_can1_send(struct CscCanMsg* msg);
-
-//extern bool_t can1_msg_received;
-//extern struct CscCanMsg can1_rx_msg;
-
-
-#endif /* USE_CAN1 */
-
-
-#endif /* CSC_CAN_H */
-
diff --git a/sw/airborne/csc/csc_datalink.c b/sw/airborne/csc/csc_datalink.c
deleted file mode 100644
index 72118be9e0..0000000000
--- a/sw/airborne/csc/csc_datalink.c
+++ /dev/null
@@ -1,32 +0,0 @@
-#define DATALINK_C
-#include "datalink.h"
-
-#include "generated/settings.h"
-#include "downlink.h"
-#include "messages.h"
-#include "dl_protocol.h"
-#include "mcu_periph/uart.h"
-
-#define IdOfMsg(x) (x[1])
-
-void dl_parse_msg(void) {
-  uint8_t msg_id = IdOfMsg(dl_buffer);
-  switch (msg_id) {
-
-  case  DL_PING:
-    {
-      DOWNLINK_SEND_PONG(DefaultChannel);
-    }
-    break;
-
-  case DL_SETTING :
-    {
-      uint8_t i = DL_SETTING_index(dl_buffer);
-      float var = DL_SETTING_value(dl_buffer);
-      DlSetting(i, var);
-      DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &var);
-    }
-    break;
-
-  }
-}
diff --git a/sw/airborne/csc/csc_main.c b/sw/airborne/csc/csc_main.c
deleted file mode 100644
index 90474cd2fc..0000000000
--- a/sw/airborne/csc/csc_main.c
+++ /dev/null
@@ -1,228 +0,0 @@
-/*
- * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $
- *
- * Copyright (C) 2008  Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include <inttypes.h>
-
-#include "csc_main.h"
-
-#include "std.h"
-
-#include "mcu.h"
-#include "sys_time.h"
-#include "led.h"
-#include "interrupt_hw.h"
-#include "mcu_periph/uart.h"
-#include "csc_telemetry.h"
-#include "generated/periodic.h"
-#include "downlink.h"
-#include "i2c.h"
-
-#include "csc_servos.h"
-#include "csc_throttle.h"
-#include "csc_adc.h"
-#include "csc_rc_spektrum.h"
-#include "csc_msg_def.h"
-
-#include "csc_can.h"
-#include "csc_ap_link.h"
-static inline void on_servo_cmd(struct CscServoCmd *cmd);
-static inline void on_motor_cmd(struct CscMotorMsg *msg);
-
-#define SERVO_TIMEOUT (SYS_TICS_OF_SEC(0.1) / PERIODIC_TASK_PERIOD)
-#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
-
-static uint32_t servo_cmd_timeout = 0;
-static uint32_t can_msg_count = 0;
-
-// gps stuff stolen from antoine's code
-#include "booz/booz2_gps.h"
-#include "math/pprz_geodetic_int.h"
-
-struct LtpDef_i  booz_ins_ltp_def;
-         bool_t  booz_ins_ltp_initialised;
-struct NedCoor_i booz_ins_gps_pos_cm_ned;
-struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
-
-static void csc_main_init( void ) {
-
-  mcu_init();
-  sys_time_init();
-  led_init();
-
-#ifdef USE_UART0
-  Uart0Init();
-#endif
-#ifdef USE_UART1
-  Uart1Init();
-#endif
-
-#ifdef SPEKTRUM_LINK
-  spektrum_init();
-#endif
-
-
-#ifdef USE_GPS
-  booz2_gps_init();
-#endif
-
-  csc_ap_link_init();
-  csc_ap_link_set_servo_cmd_cb(on_servo_cmd);
-  csc_ap_link_set_motor_cmd_cb(on_motor_cmd);
-
-  csc_adc_init();
-
-  #ifdef USE_I2C0
-  i2c_init();
-  #endif
-  // be sure to call servos_init after uart1 init since they are sharing pins
-  csc_servos_init();
-#ifdef USE_CSC_THROTTLE
-  csc_throttle_init();
-#endif
-  mcu_int_enable();
-
-
-}
-
-
-static void csc_main_periodic( void ) {
-  static uint32_t zeros[4] = {0, 0, 0, 0};
-  static uint32_t csc_loops = 0;
-
-  #ifdef DOWNLINK
-  PeriodicSendAp(DefaultChannel);
-  #endif
-
-  if (servo_cmd_timeout > SERVO_TIMEOUT) {
-    LED_OFF(CAN_LED);
-    csc_servos_set(zeros);
-  } else {
-    servo_cmd_timeout++;
-  }
-
-  if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
-    csc_ap_link_send_status(csc_loops, can_msg_count);
-  }
-  if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
-    csc_adc_periodic();
-  }
-
-}
-
-static inline void on_gps_event(void) {
-  if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
-    if (!booz_ins_ltp_initialised) {
-      ltp_def_from_ecef_i(&booz_ins_ltp_def, &booz_gps_state.ecef_pos);
-      booz_ins_ltp_initialised = TRUE;
-    }
-    ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
-    ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
-  }
-
-  struct CscGPSFixMsg fix_msg;
-  struct CscGPSAccMsg acc_msg;
-  struct CscGPSPosMsg pos_msg;
-
-  fix_msg.gps_fix = (booz_gps_state.fix == BOOZ2_GPS_FIX_3D);
-  fix_msg.vx = booz_ins_gps_speed_cm_s_ned.x;
-  fix_msg.vy = booz_ins_gps_speed_cm_s_ned.y;
-  fix_msg.vz = booz_ins_gps_speed_cm_s_ned.z;
-  csc_ap_send_msg(CSC_GPS_FIX_ID, (const uint8_t *) &fix_msg, sizeof(fix_msg));
-
-  acc_msg.pacc = booz_gps_state.pacc;
-  acc_msg.sacc = booz_gps_state.sacc;
-  csc_ap_send_msg(CSC_GPS_ACC_ID, (const uint8_t *) &acc_msg, sizeof(acc_msg));
-
-  pos_msg.val = booz_ins_gps_pos_cm_ned.x;
-  pos_msg.axis = CSC_GPS_AXIS_IDX_X;
-  csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg));
-
-  pos_msg.val = booz_ins_gps_pos_cm_ned.y;
-  pos_msg.axis = CSC_GPS_AXIS_IDX_Y;
-  csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg));
-
-  pos_msg.val = booz_ins_gps_pos_cm_ned.z;
-  pos_msg.axis = CSC_GPS_AXIS_IDX_Z;
-  csc_ap_send_msg(CSC_GPS_POS_ID, (const uint8_t *) &pos_msg, sizeof(pos_msg));
-
-}
-
-static void csc_main_event( void ) {
-
-  csc_can_event();
-#ifdef USE_CSC_THROTTLE
-  csc_throttle_event_task();
-#endif
-#ifdef SPEKTRUM_LINK
-  spektrum_event_task();
-#endif
-#ifdef USE_GPS
-  Booz2GpsEvent(on_gps_event);
-#endif
-}
-
-
-#define MIN_SERVO SYS_TICS_OF_USEC(1000)
-#define MAX_SERVO SYS_TICS_OF_USEC(2000)
-
-static inline void on_servo_cmd(struct CscServoCmd *cmd)
-{
-
-  uint32_t servos_checked[4];
-  uint32_t i;
-  for (i=0; i<4; i++)
-    servos_checked[i] = cmd->servos[i];
-    //    servos_checked[i] = Chop(servos[i],MIN_SERVO, MAX_SERVO);
-  csc_servos_set(servos_checked);
-
-  servo_cmd_timeout = 0;
-  ++can_msg_count;
-
-  //  DOWNLINK_SEND_CSC_CAN_MSG(&can1_rx_msg.frame, &can1_rx_msg.id,
-  //  &can1_rx_msg.dat_a, &can1_rx_msg.dat_b);
-  //  DOWNLINK_SEND_ADC_GENERIC(&servos[0], &servos[1]);
-
-}
-
-
-static inline void on_motor_cmd(struct CscMotorMsg *msg)
-{
-  // always send to throttle_id zero, only one motorcontrol per csc board
-  const static uint8_t throttle_id = 0;
-
-  #ifdef USE_CSC_THROTTLE
-  csc_throttle_send_msg(throttle_id, msg->cmd_id, msg->arg1, msg->arg2);
-  #endif
-}
-
-int main( void ) {
-  csc_main_init();
-  while(1) {
-  if (sys_time_periodic())
-    csc_main_periodic();
-    csc_main_event();
-  }
-  return 0;
-}
-
diff --git a/sw/airborne/csc/csc_main.h b/sw/airborne/csc/csc_main.h
deleted file mode 100644
index 64b385a42c..0000000000
--- a/sw/airborne/csc/csc_main.h
+++ /dev/null
@@ -1,4 +0,0 @@
-#ifndef CSC_MAIN_H
-#define CSC_MAIN_H
-
-#endif /* CSC_MAIN_H */
diff --git a/sw/airborne/csc/csc_me4_link.c b/sw/airborne/csc/csc_me4_link.c
deleted file mode 100644
index 35d9af5bf1..0000000000
--- a/sw/airborne/csc/csc_me4_link.c
+++ /dev/null
@@ -1,75 +0,0 @@
-#include "csc_me4_link.h"
-
-#include "LPC21xx.h"
-#include "armVIC.h"
-#include "led.h"
-
-
-struct CscMe4Link csc_me4_link;
-
-void SPI1_ISR(void) __attribute__((naked));
-
-
-
-#define PINSEL1_SCK  (1<<2)
-#define PINSEL1_MISO (1<<4)
-#define PINSEL1_MOSI (1<<6)
-#define PINSEL1_SSEL (1<<8)
-
-#define S1SPCR_CPHA  (0<<3)  /* sample on first edge */
-#define S1SPCR_CPOL  (0<<4)  /* clock idles low      */
-#define S1SPCR_MSTR  (0<<5)  /* slave mode           */
-#define S1SPCR_LSBF  (0<<6)  /* lsb first            */
-#define S1SPCR_SPIE  (1<<7)  /* interrupt enable     */
-
-#define S1SPCR_VAL (S1SPCR_CPHA|S1SPCR_CPOL|S1SPCR_MSTR|S1SPCR_LSBF|S1SPCR_SPIE)
-#define S1SPCCR_VAL 0x64
-
-
-/* S1SPSR bits */
-#define ROVR 5
-#define WCOL 6
-#define SPIF 7
-
-void csc_me4_link_init(void) {
-  /* setup pin mux for SPI1 (SCK, MISO, MOSI, SS) */
-  PINSEL1 |= PINSEL1_SCK | PINSEL1_MISO | PINSEL1_MOSI | PINSEL1_SSEL;
-  /* configure SPI1 */
-  S0SPCR  = S1SPCR_VAL;
-  S1SPCCR = S1SPCCR_VAL;
-  /* initialize interrupt vector */
-  VICIntSelect &= ~VIC_BIT(VIC_SPI1);                       // SPI1 selected as IRQ
-  VICIntEnable = VIC_BIT(VIC_SPI1);                         // SPI1 interrupt enabled
-  _VIC_CNTL(CSC_ME4_LINK_VIC_SLOT) = VIC_ENABLE | VIC_SPI0;
-  _VIC_ADDR(CSC_ME4_LINK_VIC_SLOT) = (uint32_t)SPI1_ISR;    // address of the ISR
-}
-
-
-
-
-void csc_me4_link_periodic(void) {
-
-}
-
-
-void SPI1_ISR(void) {
-  ISR_ENTRY();
-
-  static uint8_t cnt = 0;
-  LED_TOGGLE(2);
-
-  /* transfer complete  */
-  if ( bit_is_set(S1SPSR, SPIF)) {
-    uint8_t foo __attribute__ ((unused)) = S1SPDR;
-    S1SPDR = cnt;
-    cnt++;
-  }
-
-  /* clear_it */
-  S1SPINT = 1<<SPI1IF;
-
-
- VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
- ISR_EXIT();
-}
-
diff --git a/sw/airborne/csc/csc_me4_link.h b/sw/airborne/csc/csc_me4_link.h
deleted file mode 100644
index 6a13f5e9fd..0000000000
--- a/sw/airborne/csc/csc_me4_link.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef CSC_ME4_LINK_H
-#define CSC_ME4_LINK_H
-
-#include "std.h"
-
-struct CscMe4Link {
-  bool_t    timeout;
-  uint32_t  last_msg;
-};
-
-extern struct CscMe4Link csc_me4_link;
-
-extern void csc_me4_link_init(void);
-extern void csc_me4_link_periodic(void);
-
-
-
-#endif /* CSC_ME4_LINK_H */
diff --git a/sw/airborne/csc/csc_msg_def.h b/sw/airborne/csc/csc_msg_def.h
deleted file mode 100644
index 55d4f9551f..0000000000
--- a/sw/airborne/csc/csc_msg_def.h
+++ /dev/null
@@ -1,122 +0,0 @@
-#ifndef CSC_MSG_DEF_H
-#define CSC_MSG_DEF_H
-
-#include "paparazzi.h"
-
-#define CSC_GPS_AXIS_IDX_X		0
-#define CSC_GPS_AXIS_IDX_Y		1
-#define CSC_GPS_AXIS_IDX_Z		2
-
-typedef enum {
-	CSC_SERVO_CMD_ID 				=  0,
-	CSC_MOTOR_CMD_ID 				=  1,
-	CSC_PROP_CMD_ID 				=  2,
-	CSC_MOTOR_STATUS_ID			=  3,
-	CSC_BOARD_STATUS_ID			=  4,
-	CSC_BOARD_ADCVOLTS_ID		=  5,
-	CSC_RC_ID								=  6,
-	CSC_GPS_FIX_ID					=  7,
-	CSC_GPS_POS_ID					=  8,
-	CSC_GPS_ACC_ID					=  9,
-	CSC_PROP2_CMD_ID				= 10,
-	CSC_VANE_MSG_ID					= 11,
-	CSC_PRESSURE_MSG_ID			= 12,
-	CSC_BARO_MSG_ID 				= 13,
-	CSC_BAT_MSG_ID 					= 14,
-	CSC_AIRSPEED_MSG_ID 		= 15,
-
-	CSC_ID_COUNT
-} csc_can_msg_id;
-
-/* Received from the autopilot */
-struct CscServoCmd {
-  uint16_t servos[4];
-} __attribute__((packed));
-
-/* For simple blades which have a positive speed,
-   and variable pitch blades. */
-struct CscPropCmd {
-  uint8_t speeds[4];
-} __attribute__((packed));
-
-/* Send and Received between autopilot and csc */
-struct CscMotorMsg {
-  uint8_t  cmd_id;
-  uint16_t arg1;
-  uint16_t arg2;
-} __attribute__((packed));
-
-struct CscStatusMsg {
-  uint32_t loop_count;
-  uint32_t msg_count;
-} __attribute__((packed));
-
-struct CscADCMsg {
-  float ADCVolts1;
-  float ADCVolts2;
-} __attribute__((packed));
-
-struct CscRCMsg {
-  uint16_t right_stick_vertical;
-  uint16_t right_stick_horizontal;
-  uint16_t left_stick_horizontal_and_aux2;
-  uint16_t left_stick_vertical_and_flap_mix;
-} __attribute__((packed));
-
-struct CscGPSFixMsg {
-  uint8_t gps_fix;
-  uint8_t num_sv;
-  int16_t vx;
-  int16_t vy;
-  int16_t vz;
-} __attribute__((packed));
-
-struct CscGPSPosMsg {
-  int32_t val;
-  uint8_t axis;
-} __attribute__((packed));
-
-struct CscGPSAccMsg {
-  uint32_t pacc;
-  uint32_t sacc;
-} __attribute__((packed));
-
-struct CscVaneMsg {
-  float vane_angle1;
-  float vane_angle2;
-} __attribute__((packed));
-
-struct CscPressureMsg {
-  float pressure1;
-  float pressure2;
-} __attribute__((packed));
-
-/*
-struct CscAirspeedMsg {
-  float airspeed1;
-  float airspeed2;
-} __attribute__((packed));
-*/
-
-struct CscAirspeedMsg {
-  float airspeed;
-  uint8_t sensor_addr;
-} __attribute__((packed));
-
-struct CscBatMsg {
-  uint16_t volts;
-  uint16_t amps;
-  uint8_t msgctr;
-} __attribute__((packed));
-
-struct CscBaroMsg {
-  uint32_t baro_pressure;
-  uint16_t baro_temperature;
-  uint8_t baro_status;
-  uint8_t baro_sensor_addr;
-} __attribute__((packed));
-
-#define CSC_RC_SCALE 20
-#define CSC_RC_OFFSET 2*(MAX_PPRZ/CSC_RC_SCALE) /* Sorry this is a bit arbitrary. - mmt */
-
-#endif
diff --git a/sw/airborne/csc/csc_protocol.c b/sw/airborne/csc/csc_protocol.c
deleted file mode 100644
index f2027347eb..0000000000
--- a/sw/airborne/csc/csc_protocol.c
+++ /dev/null
@@ -1,190 +0,0 @@
-/*
- * $Id:$
- *
- * Copyright (C) 2010 Piotr Esden-Tempski <piotr@esden.net>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include <stdint.h>
-#include <string.h>
-
-#include "csc_protocol.h"
-#include "mcu_periph/can.h"
-#include "csc_msg_def.h"
-
-// #include "mcu.h"
-// #include "sys_time.h"
-// #include "downlink.h"
-
-#define CSCP_QUEUE_LEN 8
-
-struct cscp_msg {
-	uint8_t id;
-	uint8_t len;
-	uint8_t data[8];
-};
-
-struct cscp_msg_queue {
-	int head;
-	int tail;
-	int full;
-	int empty;
-	struct cscp_msg msgs[CSCP_QUEUE_LEN];
-} cscp_msg_queue;
-
-/*
- * This handle is being used internally in the protocol driver
- * data points to user supplied memory (user allocated struct)
- * this way we make sure that the user allocates only the amount
- * of memory necessary and we don't have to mess around with malloc
- */
-struct cscp_callback_handle {
-	cscp_callback_t callback;
-	void *data;
-};
-
-struct cscp_callback_handle cscp_callback_handles[CSC_ID_COUNT];
-
-void cscp_can_rx_callback(uint32_t id, uint8_t *buf, int len);
-void cscp_queue_init(void);
-int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len);
-int cscp_peek_msg_id(void);
-int cscp_dequeue(uint8_t *buf);
-void cscp_callback_init(void);
-
-void cscp_init(void)
-{
-	can_init(cscp_can_rx_callback);
-	cscp_queue_init();
-	cscp_callback_init();
-}
-
-static inline uint32_t cscp_can_id(uint8_t client_id, uint8_t msg_id)
-{
-	return ((client_id & 0xF) << 7) | (msg_id & 0x7F);
-}
-
-static inline uint32_t cscp_msg_id(uint32_t id)
-{
-	return (id & 0x7F);
-}
-
-int cscp_transmit(uint32_t client_id, uint8_t msg_id, const uint8_t *buf, uint8_t len)
-{
-	uint32_t id = cscp_can_id(client_id, msg_id);
-	return can_transmit(id, buf, len);
-}
-
-void cscp_can_rx_callback(uint32_t id, uint8_t *buf, int len)
-{
-	/* just store the incoming data in a buffer with very little processing */
-
-	/* TODO we should handle the return value of enqueue somehow,
-	 * the return value tells us if the queue had enough space to
-	 * store our message or not
-	 */
-	uint32_t msg_id = cscp_msg_id(id);
-
-	cscp_enqueue(msg_id, buf, len);
-}
-
-void cscp_event(void)
-{
-	/* this periodig is the main csc protocol event dispatcher */
-	int msg_id = cscp_peek_msg_id();
-
-	if (msg_id == -1){
-		return;
-	}
-
-	if(cscp_callback_handles[msg_id].callback){
-		cscp_dequeue(cscp_callback_handles[msg_id].data);
-		cscp_callback_handles[msg_id].callback(cscp_callback_handles[msg_id].data);
-		return;
-	}
-
-}
-
-void cscp_queue_init(void)
-{
-	cscp_msg_queue.head  = 0;
-	cscp_msg_queue.tail  = 0;
-	cscp_msg_queue.full  = 0;
-	cscp_msg_queue.empty = 1;
-}
-
-int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len)
-{
-	if (cscp_msg_queue.full) {
-		cscp_msg_queue.empty = 0;
-		return 1;
-	}
-	if(!cscp_callback_handles[msg_id].callback) {
-		return 2;
-	}
-
-	cscp_msg_queue.msgs[cscp_msg_queue.tail].id = msg_id;
-	cscp_msg_queue.msgs[cscp_msg_queue.tail].len = len;
-	memcpy(cscp_msg_queue.msgs[cscp_msg_queue.tail].data, buf, len);
-	cscp_msg_queue.tail  = (cscp_msg_queue.tail + 1) % CSCP_QUEUE_LEN;
-	cscp_msg_queue.empty = 0;
-	if (cscp_msg_queue.head == cscp_msg_queue.tail) {
-		cscp_msg_queue.full = 1;
-	}
-
-	return 0;
-}
-
-int cscp_peek_msg_id(void)
-{
-	if (cscp_msg_queue.empty) {
-		return -1;
-	}
-	return cscp_msg_queue.msgs[cscp_msg_queue.head].id;
-}
-
-int cscp_dequeue(uint8_t *buf)
-{
-	if (cscp_msg_queue.empty) {
-		return 1;
-	}
-
-	memcpy(buf,
-		cscp_msg_queue.msgs[cscp_msg_queue.head].data,
-		cscp_msg_queue.msgs[cscp_msg_queue.head].len);
-	cscp_msg_queue.head = (cscp_msg_queue.head + 1) % CSCP_QUEUE_LEN;
-	cscp_msg_queue.full = 0;
-	if (cscp_msg_queue.head == cscp_msg_queue.tail) {
-		cscp_msg_queue.empty = 1;
-	}
-
-	return 0;
-}
-
-void cscp_callback_init(void)
-{
-	memset(cscp_callback_handles, 0, sizeof(cscp_callback_handles));
-}
-
-void cscp_register_callback(uint32_t msg_id, cscp_callback_t callback, uint8_t *data)
-{
-	cscp_callback_handles[msg_id].callback = callback;
-	cscp_callback_handles[msg_id].data = data;
-}
diff --git a/sw/airborne/csc/csc_rc_spektrum.c b/sw/airborne/csc/csc_rc_spektrum.c
deleted file mode 100644
index 9347a8d9f7..0000000000
--- a/sw/airborne/csc/csc_rc_spektrum.c
+++ /dev/null
@@ -1,156 +0,0 @@
-/*
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
- * Copyright (C) 2009 Joby Energy
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file csc_rc_spektrum.c
- * \brief Parser for the Spektrum protocol
- */
-
-#include "csc_rc_spektrum.h"
-
-#include <inttypes.h>
-
-#include "led.h"
-
-#include "paparazzi.h"
-#include "downlink.h"
-#include "messages.h"
-#include "mcu_periph/uart.h"
-#include "string.h"
-#include "csc_ap_link.h"
-#include "csc_msg_def.h"
-#include "print.h"
-
-#define __SpektrumLink(dev, _x) dev##_x
-#define _SpektrumLink(dev, _x)  __SpektrumLink(dev, _x)
-#define SpektrumLink(_x) _SpektrumLink(SPEKTRUM_LINK, _x)
-
-#define SPEKTRUM_BUFFER_SIZE  64
-static volatile uint8_t msg_received = 0;
-static uint8_t msg_buf[SPEKTRUM_BUFFER_SIZE];
-
-#define UNINIT        0
-#define GOT_SYNC1     1
-#define GOT_SYNC2     2
-
-#define SYNC1	      3
-#define SYNC2	      1
-#define MSG_LENGTH    14
-
-#define bswap_16(x)   ((((x) & 0xFF00) >> 8) | (((x) & 0x00FF) << 8))
-
-void spektrum_init( void )
-{
-
-}
-
-void spektrum_periodic_task ( void )
-{
-
-}
-
-
-#define ROLL_OFFSET 1544
-#define PITCH_OFFSET 2551
-#define THRUST_OFFSET 3793
-#define YAW_OFFSET 3595
-#define MODE_OFFSET 5632
-#define AUX2_OFFSET 25000 // offset for AUX2 (not byte swapped)
-
-// Called after receipt of valid message
-static void spektrum_parse_msg( )
-{
-  struct spektrum_frame *frame = (struct spektrum_frame *) msg_buf;
-  struct CscRCMsg msg;
-  int16_t flap_flag;
-  int16_t aux2_flag;
-
-  // only copy the channels we need for now to fit within can frame size
-
-  msg.right_stick_horizontal =  (bswap_16(frame->right_horizontal) - ROLL_OFFSET) + CSC_RC_OFFSET;
-  msg.right_stick_vertical   =  (bswap_16(frame->right_vertical)   - PITCH_OFFSET) + CSC_RC_OFFSET;
-  msg.left_stick_horizontal_and_aux2  =  -1*(bswap_16(frame->left_horizontal)  - YAW_OFFSET) + CSC_RC_OFFSET;
-  msg.left_stick_vertical_and_flap_mix = -1*(bswap_16(frame->left_vertical) - THRUST_OFFSET) + CSC_RC_OFFSET;
-
-  flap_flag = CSC_RC_SCALE * (bswap_16(frame->flap_mix) - MODE_OFFSET);
-  if(flap_flag > MAX_PPRZ/2){
-    flap_flag = 0;
-  } else if (flap_flag > MIN_PPRZ/2){
-    flap_flag = 1;
-  } else {
-    flap_flag = 2;
-  }
-  flap_flag =  flap_flag << 13;
-
-  // boolean value for aux2 switch
-  aux2_flag = frame->aux2 > AUX2_OFFSET;
-
-  msg.left_stick_vertical_and_flap_mix = msg.left_stick_vertical_and_flap_mix | flap_flag;
-  msg.left_stick_horizontal_and_aux2 |= aux2_flag << 13;
-
-  csc_ap_send_msg(CSC_RC_ID, (const uint8_t *) &msg, sizeof(struct CscRCMsg));
-}
-
-
-// Simple state machine parser for spektrum messages
-static void parse_spektrum_stream( uint8_t c )
-{
-  // Parser state
-  static uint8_t parser_status = UNINIT;
-  static uint8_t msg_idx = 0;
-
-  switch (parser_status) {
-  case UNINIT:
-    // Look for start byte 1
-    if (c != SYNC1)
-      parser_status = UNINIT;
-    parser_status++;
-    break;
-  case GOT_SYNC1:
-    // Look for start byte 2
-    if (c != SYNC2)
-      parser_status = UNINIT;
-    parser_status++;
-    break;
-  case GOT_SYNC2:
-    // Save message contents
-    msg_buf[msg_idx++] = c;
-    if (msg_idx >= MSG_LENGTH) {
-      msg_idx = 0;
-      parser_status = UNINIT;
-      msg_received = TRUE;
-    }
-    break;
-  }
-}
-
-void spektrum_event_task( void )
-{
-  while (SpektrumLink(ChAvailable()) && !msg_received) {
-    parse_spektrum_stream(SpektrumLink(Getch()));
-  }
-
-  if (msg_received) {
-    spektrum_parse_msg();
-    msg_received = FALSE;
-  }
-}
diff --git a/sw/airborne/csc/csc_rc_spektrum.h b/sw/airborne/csc/csc_rc_spektrum.h
deleted file mode 100644
index 982c2c0b47..0000000000
--- a/sw/airborne/csc/csc_rc_spektrum.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * Copyright (C) 2009 Joby Energy
- *
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file csc_rc_spektrum.h
- */
-
-#ifndef __CSC_RC_SPEKTRUM_H__
-#define __CSC_RC_SPEKTRUM_H__
-
-#include "types.h"
-#include "std.h"
-
-struct spektrum_frame
-{
-  uint16_t right_horizontal;
-  uint16_t flap_mix;
-  uint16_t gear;
-  uint16_t right_vertical;
-  uint16_t aux2;
-  uint16_t left_vertical;
-  uint16_t left_horizontal;
-} __attribute__((__packed__));
-
-void spektrum_init(void);
-void spektrum_event_task(void);
-void spektrum_periodic_task(void);
-
-#endif
diff --git a/sw/airborne/csc/csc_servos.c b/sw/airborne/csc/csc_servos.c
deleted file mode 100644
index 1c26a5c95d..0000000000
--- a/sw/airborne/csc/csc_servos.c
+++ /dev/null
@@ -1,76 +0,0 @@
-#include "csc_servos.h"
-
-#include "LPC21xx.h"
-#include "std.h"
-#include "sys_time.h"
-#include "firmwares/rotorcraft/actuators.h"
-#include "generated/airframe.h"
-#include ACTUATORS
-
-#define CSC_SERVOS_NB 4
-
-static uint32_t csc_servos_rng[] = {SYS_TICS_OF_USEC(SERVO_S1_MAX-SERVO_S1_MIN),
-                    SYS_TICS_OF_USEC(SERVO_S2_MAX-SERVO_S2_MIN),
-                    SYS_TICS_OF_USEC(SERVO_S3_MAX-SERVO_S3_MIN),
-                    SYS_TICS_OF_USEC(SERVO_S4_MAX-SERVO_S4_MIN)};
-static uint32_t csc_servos_min[] = {SYS_TICS_OF_USEC(SERVO_S1_MIN),
-                    SYS_TICS_OF_USEC(SERVO_S2_MIN),
-                    SYS_TICS_OF_USEC(SERVO_S3_MIN),
-                    SYS_TICS_OF_USEC(SERVO_S4_MIN)};
-
-
-void csc_servos_init(void)
-{
-  actuators_init();
-}
-
-/* val == 0 literally mean off, otherwise, val 1-255 are continuous angle */
-void csc_servo_normalized_set(uint8_t id, uint16_t val)
-{
-  if(id > 3) return;
-  if(val == 0) csc_servo_set(id,0);
-  else{
-    uint32_t ticks = csc_servos_rng[id]*(val-1);
-
-    csc_servo_set(id,ticks/((1<<16)-2) + csc_servos_min[id]);
-  }
-}
-
-
-void csc_servos_commit()
-{
-  ActuatorsCommit();
-}
-
-void csc_servo_set(uint8_t id, uint32_t val)
-{
-  if(id > 3) return;
-
-#ifdef CSC_MOTORS_I2C
-  Actuator(id) = val;
-#else
-  switch(id){
-  case 0: Actuator(0) = val; break;
-  case 1: Actuator(5) = val; break;
-  case 2: Actuator(4) = val; break;
-  case 3: Actuator(3) = val; break;
-  }
-#endif
-}
-
-void csc_servos_set(int32_t* val)
-{
-#ifdef CSC_MOTORS_I2C
-  Actuator(0) = val[0];
-  Actuator(1) = val[1];
-  Actuator(2) = val[2];
-  Actuator(3) = val[3];
-#else
-  Actuator(0) = val[0];
-  Actuator(5) = val[1];
-  Actuator(4) = val[2];
-  Actuator(3) = val[3];
-#endif
-
-  csc_servos_commit();
-}
diff --git a/sw/airborne/csc/csc_servos.h b/sw/airborne/csc/csc_servos.h
deleted file mode 100644
index 1bd28a4400..0000000000
--- a/sw/airborne/csc/csc_servos.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef CSC_SERVOS_H
-#define CSC_SERVOS_H
-
-#include <inttypes.h>
-
-/*
-   lpc2129 pwm pinout
-
-LPC   shared         port   csc servo
-PWM1  TXD0           P0.0    4
-PWM2  SSEL0  EINT2   P0.7    0
-PWM3  RXD0   EINT0   P0.1    5
-PWM4  TXD1   AD1_1   P0.8    1
-PWM5  AD1_6  CAP1_3  P0.21   2
-PWM6  RXD1   EINT3   P0.9    3
-
-*/
-
-extern void csc_servos_init(void);
-extern void csc_servos_set(int32_t* val);
-extern void csc_servo_set(uint8_t id, uint32_t ticks);
-extern void csc_servo_normalized_set(uint8_t id, uint16_t val);
-extern void csc_servos_commit(void);
-
-
-#endif /* CSC_SERVOS_H */
-
diff --git a/sw/airborne/csc/csc_telemetry.h b/sw/airborne/csc/csc_telemetry.h
deleted file mode 100644
index 592c0754fd..0000000000
--- a/sw/airborne/csc/csc_telemetry.h
+++ /dev/null
@@ -1,145 +0,0 @@
-/*
- * $Id: booz2_telemetry.c 3002 2009-02-10 11:36:07Z poine $
- *
- * Copyright (C) 2008  Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#ifndef CSC_TELEMETRY_H
-#define CSC_TELEMETRY_H
-
-#include "firmwares/rotorcraft/actuators.h"
-#include <inttypes.h>
-
-extern uint8_t telemetry_mode_Ap_DefaultChannel;
-
-#include "downlink.h"
-#include "generated/settings.h"
-
-
-#define PERIODIC_SEND_DL_VALUE(_chan) PeriodicSendDlValue(_chan)
-
-#define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM)
-
-#define PERIODIC_SEND_DOWNLINK(_chan) { \
-  static uint16_t last; \
-  uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_0; \
-  last = downlink_nb_bytes; \
-  DOWNLINK_SEND_DOWNLINK(_chan, &downlink_nb_ovrn, &rate, &downlink_nb_msgs); \
-}
-
-
-#ifdef PROPS_NB
-
-#include "mercury_ap.h"
-#define PERIODIC_SEND_MERCURY_PROPS(_chan) DOWNLINK_SEND_MERCURY_PROPS(_chan, &(mixed_commands[0]),&(mixed_commands[1]),&(mixed_commands[2]),&(mixed_commands[3]))
-
-#endif /*  PROPS_NB */
-
-#ifdef BUSS_TWI_BLMC_NB
-#define PERIODIC_SEND_MERCURY_PROPS(_chan) DOWNLINK_SEND_MERCURY_PROPS(_chan, &(motor_power[0]),&(motor_power[1]),&(motor_power[2]),&(motor_power[3]))
-#endif /* BUSS_TWI_BLMC_NB */
-
-#ifdef COMMANDS_NB
-#include "commands.h"
-#define PERIODIC_SEND_COMMANDS(_chan) DOWNLINK_SEND_COMMANDS(_chan, COMMANDS_NB, commands)
-#endif /* COMMANDS_NB */
-#define PERIODIC_SEND_SIMPLE_COMMANDS(_chan) DOWNLINK_SEND_SIMPLE_COMMANDS(_chan, &commands[0], &commands[1], &commands[2])
-#define PERIODIC_SEND_CONTROLLER_GAINS(_chan) DOWNLINK_SEND_CONTROLLER_GAINS(_chan, &csc_gains.roll_kp, &csc_gains.roll_kd, &csc_gains.roll_ki, &csc_gains.pitch_kp, &csc_gains.pitch_kd, &csc_gains.pitch_ki, &csc_gains.yaw_kp, &csc_gains.yaw_kd, &csc_gains.yaw_ki)
-
-#ifdef SERVOS_NB
-#define PERIODIC_SEND_ACTUATORS(_chan) DOWNLINK_SEND_ACTUATORS(_chan, SERVOS_NB, actuators)
-#endif
-
-#ifdef RADIO_CONTROL
-#include "subsystems/radio_control.h"
-#define PERIODIC_SEND_RC(_chan) DOWNLINK_SEND_RC(_chan, RADIO_CONTROL_NB_CHANNEL, radio_control.values)
-#define PERIODIC_SEND_FBW_STATUS(_chan) { uint16_t current; DOWNLINK_SEND_FBW_STATUS(_chan, &radio_control.status, &pprz_mode, &vsupply, &current); }
-#else
-#ifdef RADIO_CONTROL
-#define PERIODIC_SEND_RC(_chan) DOWNLINK_SEND_RC(_chan, PPM_NB_PULSES, rc_values)
-#define PERIODIC_SEND_FBW_STATUS(_chan) { uint16_t current; DOWNLINK_SEND_FBW_STATUS(_chan, &rc_status, &pprz_mode, &vsupply, &current); }
-#endif
-#endif /* RADIO_CONTROL */
-
-#ifdef USE_GPS
-#define PERIODIC_SEND_BOOZ2_GPS(_chan) {			\
-DOWNLINK_SEND_BOOZ2_GPS( _chan,                 \
-                         &booz_ins_gps_pos_cm_ned.x,		\
-                         &booz_ins_gps_pos_cm_ned.y,		\
-                 &booz_ins_gps_pos_cm_ned.z,	\
-                 &booz_gps_state.ecef_speed.x,	\
-                 &booz_gps_state.ecef_speed.y,	\
-                 &booz_gps_state.ecef_speed.z,	\
-                 &booz_gps_state.pacc,		\
-                 &booz_gps_state.sacc,		\
-                 &booz_gps_state.pdop,		\
-                 &booz_gps_state.num_sv,		\
-                 &booz_gps_state.fix)		\
-  }
-#endif
-
-#define PERIODIC_SEND_GPS_ERROR(_chan) {				\
-DOWNLINK_SEND_GPS_ERROR( _chan,                 \
-                 &csc_gps_errors.pos.x,		\
-               &csc_gps_errors.pos.y,		\
-               &csc_gps_errors.pos.z,		\
-               &csc_gps_errors.rate.x,		\
-               &csc_gps_errors.rate.y,		\
-               &csc_gps_errors.rate.z)      \
-    }
-
-#define PERIODIC_SEND_INS3(_chan) { \
-DOWNLINK_SEND_INS3(_chan,     \
-&booz_ins_gps_pos_cm_ned.x,	\
-&booz_ins_gps_pos_cm_ned.y,     \
-&booz_ins_gps_pos_cm_ned.z,     \
-&booz_ins_gps_speed_cm_s_ned.x,   \
-&booz_ins_gps_speed_cm_s_ned.y,   \
-&booz_ins_gps_speed_cm_s_ned.z    \
-) }
-
-#define PERIODIC_SEND_GPS_SOL(_chan) DOWNLINK_SEND_GPS_SOL(_chan, &booz_gps_state.pacc, \
-  &booz_gps_state.sacc, &booz_gps_state.pdop, &booz_gps_state.num_sv)
-
-#define PERIODIC_SEND_GPS(_chan) { uint32_t zero = 0; DOWNLINK_SEND_GPS(_chan, &booz_gps_state.fix, \
-  &booz_ins_gps_pos_cm_ned.y, \
-  &booz_ins_gps_pos_cm_ned.x, \
-  &zero, \
-  &booz_ins_gps_pos_cm_ned.z, \
-  &zero, \
-  &zero, \
-  &zero, \
-  &zero, \
-  &zero, &zero) }
-
-#ifdef USE_AIRSPEED
-#include "estimator.h"
-#define PERIODIC_SEND_AIRSPEED(_chan) { float empty; DOWNLINK_SEND_AIRSPEED (_chan, &estimator_airspeed,&empty,&empty,&empty,&empty) }
-#else
-#define PERIODIC_SEND_AIRSPEED(_chan) {}
-#endif
-
-#ifdef USE_BARO_ETS
-#include "baro_ets.h"
-#define PERIODIC_SEND_ESTIMATOR(_chan) { float empty; DOWNLINK_SEND_ESTIMATOR(_chan, &baro_ets_altitude, &empty) }
-#endif
-
-#endif /* CSC_TELEMETRY_H */
diff --git a/sw/airborne/csc/csc_throttle.c b/sw/airborne/csc/csc_throttle.c
deleted file mode 100644
index 008a269fe4..0000000000
--- a/sw/airborne/csc/csc_throttle.c
+++ /dev/null
@@ -1,234 +0,0 @@
-/*
- * Copyright (C) 2009 Joby Energy
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file uart_throttle.c
- */
-
-#include "csc_throttle.h"
-
-#include <inttypes.h>
-
-#include "led.h"
-
-//#include "downlink.h"
-//#include "messages.h"
-#include "mcu_periph/uart.h"
-#include "csc_ap_link.h"
-//#include "print.h"
-//#include "com_stats.h"
-
-#define THROTTLE_START1 0xAC
-#define THROTTLE_START2 0xBE
-
-#define THROTTLE_ID 0
-
-uint32_t throttle_err_count;
-uint32_t throttle_recv_count;
-
-#define __ThrottleLink(dev, _x) dev##_x
-#define _ThrottleLink(dev, _x)  __ThrottleLink(dev, _x)
-#define ThrottleLink(_x) _ThrottleLink(THROTTLE_LINK, _x)
-
-#define ThrottlePrintString(s) ThrottleLink(PrintString(s))
-#define ThrottleUartSend1(c) ThrottleLink(Transmit(c))
-
-#define UNINIT         0
-#define GOT_START1     1
-#define GOT_START2     2
-#define GOT_TID        3
-#define GOT_CMD        4
-#define GOT_ARG1H      5
-#define GOT_ARG1L      6
-#define GOT_ARG2H      7
-#define GOT_ARG2L      8
-#define GOT_CHECKSUML  9
-
-#define THROTTLE_CMD_INFO 1
-#define THROTTLE_CMD_STOP 2
-#define THROTTLE_CMD_START 3
-#define THROTTLE_CMD_CONFIGURE 4
-#define THROTTLE_CMD_FAULT_CLEAR 5
-#define THROTTLE_CMD_SET_SPEED 6
-#define THROTTLE_CMD_GET_RREG 7
-#define THROTTLE_CMD_SET_RREG 8
-#define THROTTLE_CMD_REG_RVALUE 9
-#define THROTTLE_CMD_GET_MREG 10
-#define THROTTLE_CMD_SET_MREG 11
-#define THROTTLE_CMD_REG_MVALUE 12
-
-static volatile uint8_t uart_msg_received;
-
-struct throttle_msg {
-  uint8_t throttle_id;
-  uint8_t cmd_id;
-  uint16_t arg1;
-  uint16_t arg2;
-  uint16_t checksum;
-};
-
-static struct throttle_msg recv_msg;
-
-static void parse_uart_msg(uint8_t c);
-static void throttle_send_message(struct throttle_msg *send);
-static void send_ap_msg( struct throttle_msg *msg);
-
-void csc_throttle_init( void )
-{
-
-  throttle_err_count = 0;
-  throttle_recv_count = 0;
-}
-
-static uint16_t calculate_checksum(struct throttle_msg *send)
-{
-  return send->throttle_id + send->cmd_id + send->arg1 + send->arg2;
-}
-
-void csc_throttle_event_task( void )
-{
-  while (!uart_msg_received && ThrottleLink(ChAvailable())) {
-    parse_uart_msg(ThrottleLink(Getch()));
-  }
-  if (uart_msg_received) {
-    send_ap_msg(&recv_msg);
-    uart_msg_received = FALSE;
-  }
-}
-
-static void send_ap_msg( struct throttle_msg *msg)
-{
-
-  struct CscMotorMsg ap_msg;
-
-  if (calculate_checksum(&recv_msg) != recv_msg.checksum) {
-    throttle_err_count++;
-    return;
-  }
-
-  throttle_recv_count++;
-
-  ap_msg.cmd_id = msg->cmd_id;
-  ap_msg.arg1 =  msg->arg1;
-  ap_msg.arg2 = msg->arg2;
-
-  csc_ap_send_msg(CSC_MOTOR_STATUS_ID, (const char *)&ap_msg, sizeof(ap_msg));
-}
-
-
-void csc_throttle_send_msg(uint8_t throttle_id, uint8_t cmd_id, uint16_t arg1, uint16_t arg2)
-{
-  struct throttle_msg msg;
-
-  msg.throttle_id = throttle_id;
-  msg.cmd_id = cmd_id;
-  msg.arg1 = arg1;
-  msg.arg2 = arg2;
-
-  msg.checksum = calculate_checksum(&msg);
-
-  throttle_send_message(&msg);
-}
-
-static void throttle_send_message(struct throttle_msg *send)
-{
-
-  ThrottleUartSend1(THROTTLE_START1);
-  ThrottleUartSend1(THROTTLE_START2);
-  ThrottleUartSend1(send->throttle_id);
-  ThrottleUartSend1(send->cmd_id);
-  ThrottleUartSend1(send->arg1 >> 8);
-  ThrottleUartSend1(send->arg1 & 0xFF);
-  ThrottleUartSend1(send->arg2 >> 8);
-  ThrottleUartSend1(send->arg2 & 0xFF);
-  ThrottleUartSend1(send->checksum >> 8);
-  ThrottleUartSend1(send->checksum & 0xFF);
-  LED_TOGGLE(THROTTLE_LED);
-}
-
-// Simple state machine parser for throttle messages
-// Passed serial bytes one per call and parses stream into message id, data length, and data buffer
-// for use at higher level
-static void parse_uart_msg( uint8_t c ) {
-  static uint8_t throttle_status;
-
-  switch (throttle_status) {
-  case UNINIT:
-    // Look for throttle start byte 1
-    if (c != THROTTLE_START1)
-      goto error;
-    throttle_status++;
-    break;
-  case GOT_START1:
-    // Look for throttle start byte 2
-    if (c != THROTTLE_START2)
-      goto error;
-    throttle_status++;
-    break;
-  case GOT_START2:
-    // Look for throttle ID
-    recv_msg.throttle_id = c;
-    throttle_status++;
-    break;
-  case GOT_TID:
-    // Save command ID
-    recv_msg.cmd_id = c;
-    throttle_status++;
-    break;
-  case GOT_CMD:
-    // Save arg1 high
-    recv_msg.arg1 = (c << 8);
-    throttle_status++;
-    break;
-  case GOT_ARG1H:
-    // Save arg1 low
-    recv_msg.arg1 |= c;
-    throttle_status++;
-    break;
-  case GOT_ARG1L:
-    // Save arg2 high
-    recv_msg.arg2 = (c << 8);
-    throttle_status++;
-    break;
-  case GOT_ARG2H:
-    // Save arg2 low
-    recv_msg.arg2 |= c;
-    throttle_status++;
-    break;
-  case GOT_ARG2L:
-    // Read checksum byte high
-    recv_msg.checksum = (c << 8);
-    throttle_status++;
-    break;
-  case GOT_CHECKSUML:
-    recv_msg.checksum |= c;
-    // Notification for valid message received
-    uart_msg_received = TRUE;
-    goto restart;
-    break;
-  }
-  return;
- error:
- restart:
-  // Start over (Reset parser state)
-  throttle_status = UNINIT;
-  return;
-}
diff --git a/sw/airborne/csc/csc_vane.c b/sw/airborne/csc/csc_vane.c
deleted file mode 100644
index d47efab785..0000000000
--- a/sw/airborne/csc/csc_vane.c
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "csc_adc.h"
-#include "csc_ap_link.h"
-#include "csc_vane.h"
-
-#include "LPC21xx.h"
-#include "led.h"
-#include "pwm_input.h"
-
-#define PWM_INPUT_COUNTS_PER_REV 61358.
-#define PWM_INPUT_MSG_PRESCALE 8
-#define VANE_NB 2
-const static float avg_factor = 0.0;
-
-void csc_vane_init(void)
-{
-
-}
-
-void csc_vane_periodic(void)
-{
-  static float angle[VANE_NB];
-  static uint8_t prescale = 0;
-
-  for (int i = 0; i < VANE_NB; i++) {
-    angle[i] = avg_factor * angle[i] +
-								((1. - avg_factor) *
-								 RadOfDeg(360. * pwm_input_duration[i] / PWM_INPUT_COUNTS_PER_REV));
-  }
-
-  if (prescale++ >= PWM_INPUT_MSG_PRESCALE) {
-    prescale = 0;
-    csc_ap_link_send_vane(angle);
-  }
-}
-
diff --git a/sw/airborne/csc/csc_vane.h b/sw/airborne/csc/csc_vane.h
deleted file mode 100644
index d673c46fc5..0000000000
--- a/sw/airborne/csc/csc_vane.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#include "csc_adc.h"
-#include "csc_ap_link.h"
-
-#include "LPC21xx.h"
-#include "led.h"
-
-void csc_vane_init(void);
-void csc_vane_periodic(void);
diff --git a/sw/airborne/csc/csc_xsens.c b/sw/airborne/csc/csc_xsens.c
deleted file mode 100644
index ee0a2e6ec0..0000000000
--- a/sw/airborne/csc/csc_xsens.c
+++ /dev/null
@@ -1,461 +0,0 @@
-/*
- * Paparazzi mcu0 $Id: quad_xsens.c,v 1.2 2008/05/07 12:54:23 gautier Exp $
- *
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file csc_xsens.c
- * \brief Parser for the Xsens protocol
- */
-
-#include "csc_xsens.h"
-
-#include <inttypes.h>
-
-#include "led.h"
-
-#include "downlink.h"
-#include "messages.h"
-#include "mcu_periph/uart.h"
-//#include "com_stats.h"
-#include "math/pprz_algebra_float.h"
-#include "string.h"
-
-void parse_xsens_msg(uint8_t xsens_id, uint8_t c );
-
-#define __Xsens2Link(dev, _x) dev##_x
-#define _Xsens2Link(dev, _x)  __Xsens2Link(dev, _x)
-#define Xsens2Link(_x) _Xsens2Link(XSENS2_LINK, _x)
-
-#define Xsens2Buffer() Xsens2Link(ChAvailable())
-#define ReadXsens2Buffer() { while (Xsens2Link(ChAvailable())&&!xsens_msg_received[1]) parse_xsens_msg(1, Xsens2Link(Getch())); }
-
-#define Xsens2UartSend1(c) Xsens2Link(Transmit(c))
-#define Xsens2UartInitParam(_a,_b,_c) Xsens2Link(InitParam(_a,_b,_c))
-#define Xsens2UartRunning Xsens2Link(TxRunning)
-
-#define Xsens2InitCheksum() { send_ck[1] = 0; }
-#define Xsens2UpdateChecksum(c) { send_ck[1] += c; }
-
-#define Xsens2Send1(c) { uint8_t i8=c; XSens2UartSend1(i8); Xsens2UpdateChecksum(i8); }
-#define Xsens2Send1ByAddr(x) { Xsens2Send1(*x); }
-#define Xsens2Send2ByAddr(x) { Xsens2Send1(*(x+1)); Xsens2Send1(*x); }
-#define Xsens2Send4ByAddr(x) { Xsens2Send1(*(x+3)); Xsens2Send1(*(x+2)); Xsens2Send1(*(x+1)); Xsens2Send1(*x); }
-
-#define Xsens2Header(msg_id, len) { \
-  Xsens2UartSend1(XSENS_START); \
-  Xsens2InitCheksum(); \
-  Xsens2Send1(XSENS_BID); \
-  Xsens2Send1(msg_id); \
-  Xsens2Send1(len); \
-}
-#define Xsens2Trailer() { uint8_t i8=0x100-send_ck[1]; Xsens2UartSend1(i8); }
-
-#define __Xsens1Link(dev, _x) dev##_x
-#define _Xsens1Link(dev, _x)  __Xsens1Link(dev, _x)
-#define Xsens1Link(_x) _Xsens1Link(XSENS1_LINK, _x)
-
-#define Xsens1Buffer() Xsens1Link(ChAvailable())
-#define ReadXsens1Buffer() { while (Xsens1Link(ChAvailable())&&!xsens_msg_received[0]) parse_xsens_msg(0, Xsens1Link(Getch())); }
-
-#define Xsens1UartSend1(c) Xsens1Link(Transmit(c))
-#define Xsens1UartInitParam(_a,_b,_c) Xsens1Link(InitParam(_a,_b,_c))
-#define Xsens1UartRunning Xsens1Link(TxRunning)
-
-#define Xsens1InitCheksum() { send_ck[0] = 0; }
-#define Xsens1UpdateChecksum(c) { send_ck[0] += c; }
-
-#define Xsens1Send1(c) { uint8_t i8=c; XSens1UartSend1(i8); Xsens1UpdateChecksum(i8); }
-#define Xsens1Send1ByAddr(x) { Xsens1Send1(*x); }
-#define Xsens1Send2ByAddr(x) { Xsens1Send1(*(x+1)); Xsens1Send1(*x); }
-#define Xsens1Send4ByAddr(x) { Xsens1Send1(*(x+3)); Xsens1Send1(*(x+2)); Xsens1Send1(*(x+1)); Xsens1Send1(*x); }
-
-#define Xsens1Header(msg_id, len) { \
-  Xsens1UartSend1(XSENS_START); \
-  Xsens1InitCheksum(); \
-  Xsens1Send1(XSENS_BID); \
-  Xsens1Send1(msg_id); \
-  Xsens1Send1(len); \
-}
-#define Xsens1Trailer() { uint8_t i8=0x100-send_ck[0]; Xsens1UartSend1(i8); }
-
-
-/** Includes macros generated from xsens_MTi.xml */
-#include "xsens_protocol.h"
-
-uint8_t xsens_mode[XSENS_COUNT]; // Receiver status
-volatile uint8_t xsens_msg_received[XSENS_COUNT];
-
-float xsens_phi[XSENS_COUNT];
-float xsens_theta[XSENS_COUNT];
-float xsens_psi[XSENS_COUNT];
-
-float xsens_r_a[XSENS_COUNT];
-float xsens_r_b[XSENS_COUNT];
-float xsens_r_c[XSENS_COUNT];
-float xsens_r_d[XSENS_COUNT];
-float xsens_r_e[XSENS_COUNT];
-float xsens_r_f[XSENS_COUNT];
-float xsens_r_g[XSENS_COUNT];
-float xsens_r_h[XSENS_COUNT];
-float xsens_r_i[XSENS_COUNT];
-
-struct FloatRMat xsens_rmat[XSENS_COUNT];
-struct FloatRMat xsens_rmat_neutral[XSENS_COUNT];
-struct FloatRMat xsens_rmat_adj[XSENS_COUNT];
-
-float xsens_accel_x[XSENS_COUNT];
-float xsens_accel_y[XSENS_COUNT];
-float xsens_accel_z[XSENS_COUNT];
-
-float xsens_mag_x[XSENS_COUNT];
-float xsens_mag_y[XSENS_COUNT];
-float xsens_mag_z[XSENS_COUNT];
-
-float xsens_gyro_x[XSENS_COUNT];
-float xsens_gyro_y[XSENS_COUNT];
-float xsens_gyro_z[XSENS_COUNT];
-
-// Raw data
-
-uint16_t xsens_raw_accel_x[XSENS_COUNT];
-uint16_t xsens_raw_accel_y[XSENS_COUNT];
-uint16_t xsens_raw_accel_z[XSENS_COUNT];
-
-uint16_t xsens_raw_mag_x[XSENS_COUNT];
-uint16_t xsens_raw_mag_y[XSENS_COUNT];
-uint16_t xsens_raw_mag_z[XSENS_COUNT];
-
-uint16_t xsens_raw_gyro_x[XSENS_COUNT];
-uint16_t xsens_raw_gyro_y[XSENS_COUNT];
-uint16_t xsens_raw_gyro_z[XSENS_COUNT];
-
-// adjustable heading offset
-float xsens_psi_offset[XSENS_COUNT];
-
-float xsens_mag_heading[XSENS_COUNT];
-
-int xsens_setzero = 0;
-
-#define XSENS_MSG_BUF 1
-#define XSENS_MAX_PAYLOAD 254
-uint8_t xsens_msg_buf[XSENS_COUNT][XSENS_MSG_BUF][XSENS_MAX_PAYLOAD];
-static volatile uint8_t xsens_msg_buf_count[XSENS_COUNT]; // buffer count
-static volatile uint8_t xsens_msg_buf_pi[XSENS_COUNT]; // produce index
-static volatile uint8_t xsens_msg_buf_ci[XSENS_COUNT]; // consume index
-
-//#define XSENS_RAW_MODE
-
-// See Page 25 MT Low-Level Comm Doc
-#define XSENS_OUTPUT_CALIBRATED (1 << 1)
-#define XSENS_OUTPUT_ORIENTATION (1 << 2)
-#define XSENS_OUTPUT_AUXDATA (1 << 3)
-#define XSENS_OUTPUT_STATUS (1 << 11)
-#define XSENS_RAW_INERTIAL (1 << 14)
-#ifdef XSENS_RAW_MODE
-#define XSENS_DEFAULT_OUTPUT_MODE (XSENS_RAW_INERTIAL | XSENS_OUTPUT_STATUS)
-#else
-#define XSENS_DEFAULT_OUTPUT_MODE (XSENS_OUTPUT_ORIENTATION | XSENS_OUTPUT_STATUS | XSENS_OUTPUT_CALIBRATED)
-#endif
-
-// output settings : (Page 26 MT Low-Level Comm Doc)
-// Sample Counter, rotation matrix, floating point output
-#define XSENS_OUTPUT_AUX1 (1 << 10)
-#define XSENS_OUTPUT_AUX2 (1 << 11)
-#define XSENS_ORIENTATION (2 << 2)
-#define XSENS_ACCELS (1 << 4)
-#define XSENS_GYROS (1 << 5)
-#define XSENS_MAGS (1 << 6)
-#define XSENS_TIMESTAMP (1 << 0)
-#ifdef XSENS_RAW_MODE
-#define XSENS_DEFAULT_OUTPUT_SETTINGS (XSENS_ACCELS | XSENS_MAGS | XSENS_GYROS | XSENS_TIMESTAMP)
-#else
-#define XSENS_DEFAULT_OUTPUT_SETTINGS (XSENS_ORIENTATION | XSENS_ACCELS | XSENS_MAGS | XSENS_GYROS | XSENS_TIMESTAMP)
-#endif
-
-#define UNINIT        0
-#define GOT_START     1
-#define GOT_BID       2
-#define GOT_MID       3
-#define GOT_LEN       4
-#define GOT_DATA      5
-#define GOT_CHECKSUM  6
-
-uint8_t xsens_errorcode[XSENS_COUNT];
-uint8_t xsens_msg_status[XSENS_COUNT];
-uint16_t xsens_time_stamp[XSENS_COUNT];
-uint16_t xsens_output_mode[XSENS_COUNT];
-uint32_t xsens_output_settings[XSENS_COUNT];
-
-static uint8_t msg_id[XSENS_COUNT][XSENS_MSG_BUF];
-static uint8_t xsens_len[XSENS_COUNT][XSENS_MSG_BUF];
-static uint8_t xsens_msg_idx[XSENS_COUNT];
-static uint8_t ck[XSENS_COUNT];
-static uint8_t send_ck[XSENS_COUNT];
-
-
-void xsens_init( void )
-{
-  for (int i = 0; i < XSENS_COUNT; i++) {
-    send_ck[i] = 0;
-    xsens_msg_status[i] = 0;
-    xsens_time_stamp[i] = 0;
-    xsens_output_mode[i] = XSENS_DEFAULT_OUTPUT_MODE;
-    xsens_output_settings[i] = XSENS_DEFAULT_OUTPUT_SETTINGS;
-    xsens_msg_buf_count[i] = 0;
-    xsens_msg_buf_pi[i] = 0;
-    xsens_msg_buf_ci[i] = 0;
-    FLOAT_RMAT_ZERO(xsens_rmat_neutral[i]);
-  }
-  // Also TODO: set scenario to aerospace
-  // set magnetic declination angle
-  // Probably quicker to just set everything once via MT Manager software
-  // instead of setting via paparazzi (MT-G should store setting in flash)
-  //XSENS_GoToConfig();
-  //XSENS_SetOutputMode(xsens2_output_mode);
-  //XSENS_SetOutputSettings(xsens2_output_settings);
-  //XSENS_GoToMeasurment();
-}
-
-void xsens_periodic_task ( void )
-{
-  for (int i = 0; i < XSENS_COUNT; i++)
-  {
-    if (xsens_msg_buf_count[i] > 0) {
-      xsens_parse_msg(i);
-      xsens_msg_buf_count[i]--;
-      xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1) % XSENS_MSG_BUF;
-    }
-    if (xsens_setzero) {
-      memcpy(&xsens_rmat_neutral[i], &xsens_rmat[i], sizeof(struct FloatRMat));
-    }
-  }
-  xsens_setzero = 0;
-}
-
-void xsens_event_task( void )
-{
-  while (Xsens1Link(ChAvailable()) && !xsens_msg_received[0]) {
-    parse_xsens_msg(0, Xsens1Link(Getch()));
-  }
-
-/*
-  while (Xsens2Link(ChAvailable()) && !xsens_msg_received[1])
-    parse_xsens_msg(1, Xsens2Link(Getch()));
-*/
-
-  for (int i = 0; i < XSENS_COUNT; i++) {
-    if (xsens_msg_received[i]) {
-
-      // first make room
-      while (xsens_msg_buf_count[i] >= XSENS_MSG_BUF) {
-        // throwing away old stuff
-        xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1 ) % XSENS_MSG_BUF;
-        xsens_msg_buf_count[i]--;
-      }
-
-      xsens_msg_buf_pi[i] = (xsens_msg_buf_pi[i] + 1 ) % XSENS_MSG_BUF;
-      xsens_msg_buf_count[i]++;
-
-      //xsens_parse_msg(i);
-      xsens_msg_received[i] = FALSE;
-    }
-  }
-}
-
-// Called after receipt of valid message to
-void xsens_parse_msg( uint8_t xsens_id ) {
-  uint8_t buf_slot = xsens_msg_buf_ci[xsens_id];
-
-  if (msg_id[xsens_id][buf_slot] == XSENS_ReqOutputModeAck_ID) {
-    xsens_output_mode[xsens_id] = XSENS_ReqOutputModeAck_mode(xsens_msg_buf[xsens_id]);
-  }
-  else if (msg_id[xsens_id][buf_slot] == XSENS_Error_ID) {
-    xsens_errorcode[xsens_id] = XSENS_Error_errorcode(xsens_msg_buf[xsens_id]);
-  }
-  else if (msg_id[xsens_id][buf_slot] == XSENS_MTData_ID) {
-    uint8_t offset = 0;
-    // test RAW modes else calibrated modes
-      if (XSENS_MASK_RAWInertial(xsens_output_mode[xsens_id])){// || (XSENS_MASK_RAWGPS(xsens2_output_mode))) {
-	xsens_raw_accel_x[xsens_id] = XSENS_DATA_RAWInertial_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
-	xsens_raw_accel_y[xsens_id] = XSENS_DATA_RAWInertial_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
-	xsens_raw_accel_z[xsens_id] = XSENS_DATA_RAWInertial_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-	xsens_raw_gyro_x[xsens_id] = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
-	xsens_raw_gyro_y[xsens_id] = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
-	xsens_raw_gyro_z[xsens_id] = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-        xsens_raw_mag_x[xsens_id]  = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
-	xsens_raw_mag_y[xsens_id]  = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
-	xsens_raw_mag_z[xsens_id]  = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-      }
-      if (XSENS_MASK_Temp(xsens_output_mode[xsens_id])) {
-        offset += XSENS_DATA_Temp_LENGTH;
-      }
-      if (XSENS_MASK_Calibrated(xsens_output_mode[xsens_id])) {
-        if (XSENS_MASK_AccOut(xsens_output_settings[xsens_id])) {
-          xsens_accel_x[xsens_id] = XSENS_DATA_Calibrated_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_accel_y[xsens_id] = XSENS_DATA_Calibrated_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_accel_z[xsens_id] = XSENS_DATA_Calibrated_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-        }
-        if (XSENS_MASK_GyrOut(xsens_output_settings[xsens_id])) {
-          xsens_gyro_x[xsens_id] = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_gyro_y[xsens_id] = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_gyro_z[xsens_id] = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-        }
-        if (XSENS_MASK_MagOut(xsens_output_settings[xsens_id])) {
-          xsens_mag_x[xsens_id] = XSENS_DATA_Calibrated_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_mag_y[xsens_id] = XSENS_DATA_Calibrated_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_mag_z[xsens_id] = XSENS_DATA_Calibrated_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-          float pitch = xsens_phi[xsens_id];
-          float roll = -xsens_theta[xsens_id];
-          float tilt_comp_x = xsens_mag_x[xsens_id] * cos(pitch)
-                            + xsens_mag_y[xsens_id] * sin(roll) * sin(pitch)
-                            - xsens_mag_z[xsens_id] * cos(roll) * sin(pitch);
-          float tilt_comp_y = xsens_mag_y[xsens_id] * cos(roll)
-                            + xsens_mag_z[xsens_id] * sin(roll);
-          xsens_mag_heading[xsens_id] = -atan2( tilt_comp_y, tilt_comp_x);
-        }
-        offset += XSENS_DATA_Calibrated_LENGTH;
-      }
-      if (XSENS_MASK_Orientation(xsens_output_mode[xsens_id])) {
-        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x0) {
-          offset += XSENS_DATA_Quaternion_LENGTH;
-        }
-        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x1) {
-          xsens_phi[xsens_id] = XSENS_DATA_Euler_roll(xsens_msg_buf[xsens_id][buf_slot],offset)  * M_PI/180;
-          xsens_theta[xsens_id] =XSENS_DATA_Euler_pitch(xsens_msg_buf[xsens_id][buf_slot],offset) * M_PI/180;
-          xsens_psi[xsens_id] =  XSENS_DATA_Euler_yaw(xsens_msg_buf[xsens_id][buf_slot],offset)   * M_PI/180;
-          offset += XSENS_DATA_Euler_LENGTH;
-        }
-        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x2) {
-          xsens_rmat[xsens_id].m[0] = XSENS_DATA_Matrix_a(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[1] = XSENS_DATA_Matrix_b(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[2] = XSENS_DATA_Matrix_c(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[3] = XSENS_DATA_Matrix_d(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[4] = XSENS_DATA_Matrix_e(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[5] = XSENS_DATA_Matrix_f(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[6] = XSENS_DATA_Matrix_g(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[7] = XSENS_DATA_Matrix_h(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[8] = XSENS_DATA_Matrix_i(xsens_msg_buf[xsens_id][buf_slot],offset);
-
-	  /* //	  FLOAT_RMAT_COMP_INV(xsens_rmat_adj[xsens_id], xsens_rmat_neutral[xsens_id], xsens_rmat[xsens_id]); */
-	  struct FloatRMat xsens_rmat_temp[XSENS_COUNT];
-	  FLOAT_RMAT_INV(xsens_rmat_temp[xsens_id], xsens_rmat[xsens_id]);
-
-	  FLOAT_RMAT_COMP(xsens_rmat_adj[xsens_id], xsens_rmat_temp[xsens_id], xsens_rmat_neutral[xsens_id]);
-
-	  xsens_phi[xsens_id] = -atan2(xsens_rmat_adj[xsens_id].m[7], xsens_rmat_adj[xsens_id].m[8]);
-	  xsens_theta[xsens_id] = asin(xsens_rmat_adj[xsens_id].m[6]);
-	  xsens_psi[xsens_id] = atan2(xsens_rmat_adj[xsens_id].m[3], xsens_rmat_adj[xsens_id].m[0]);
-
-	  xsens_psi[xsens_id] -= RadOfDeg(xsens_psi_offset[xsens_id]);
-
-	  /* FLOAT_RMAT_COMP(xsens_rmat_adj[xsens_id], xsens_rmat_neutral[xsens_id], xsens_rmat[xsens_id]); */
-
-          /* // Calculate roll, pitch, yaw from rotation matrix ( p 31-33 MTi-G USer Man and Tech Doc) */
-          /* xsens_phi[xsens_id] = -atan2 (xsens_rmat_adj[xsens_id].m[7], xsens_rmat_adj[xsens_id].m[8]); */
-          /* xsens_theta[xsens_id] = asin (xsens_rmat_adj[xsens_id].m[6]); */
-          /* xsens_psi[xsens_id] = atan2 (xsens_rmat_adj[xsens_id].m[3], xsens_rmat_adj[xsens_id].m[0]); */
-
-
-
-          offset += XSENS_DATA_Matrix_LENGTH;
-        }
-      }
-      if (XSENS_MASK_Status(xsens_output_mode[xsens_id])) {
-        xsens_msg_status[xsens_id] = XSENS_DATA_Status_status(xsens_msg_buf[xsens_id][buf_slot],offset);
-        xsens_mode[xsens_id] = xsens_msg_status[xsens_id];
-        offset += XSENS_DATA_Status_LENGTH;
-      }
-      if (XSENS_MASK_TimeStamp(xsens_output_settings[xsens_id])) {
-        uint16_t ts = XSENS_DATA_TimeStamp_ts(xsens_msg_buf[xsens_id][buf_slot],offset);
-        if (xsens_time_stamp[xsens_id] + 1 != ts)
-          //xsens_err_count[xsens_id]++;
-        xsens_time_stamp[xsens_id] = ts;
-        offset += XSENS_DATA_TimeStamp_LENGTH;
-      }
-  }
-}
-
-
-// Simple state machine parser for XSENS messages (MT Low-Level Comm Doc)
-// Passed serial bytes one per call and parses stream into message id, data length, and data buffer
-// for use at higher level
-void parse_xsens_msg( uint8_t xsens_id, uint8_t c ) {
-  static uint8_t xsens_status[XSENS_COUNT];
-  uint8_t buf_slot = xsens_msg_buf_pi[xsens_id]; // produce index
-
-  // keep track of checksum byte
-  ck[xsens_id] += c;
-  switch (xsens_status[xsens_id]) {
-  case UNINIT:
-    // Look for xsens start byte
-    if (c != XSENS_START)
-      goto error;
-    xsens_status[xsens_id]++;
-    ck[xsens_id] = 0; // Reset checksum
-    break;
-  case GOT_START:
-    // Look for xsens Bus ID
-    if (c != XSENS_BID)
-      goto error;
-    xsens_status[xsens_id]++;
-    break;
-  case GOT_BID:
-    // Save message ID
-    msg_id[xsens_id][buf_slot] = c;
-    xsens_status[xsens_id]++;
-    break;
-  case GOT_MID:
-    // Save message length
-    xsens_len[xsens_id][buf_slot] = c;
-    // check for valid message length
-    if (xsens_len[xsens_id][buf_slot] > XSENS_MAX_PAYLOAD)
-      goto error;
-    xsens_msg_idx[xsens_id] = 0; // Reset buffer index
-    xsens_status[xsens_id]++;
-    break;
-  case GOT_LEN:
-    // Read byte into data buffer
-    xsens_msg_buf[xsens_id][buf_slot][xsens_msg_idx[xsens_id]] = c;
-    xsens_msg_idx[xsens_id]++;
-    // Terminate reading at end of data field
-    if (xsens_msg_idx[xsens_id] >= xsens_len[xsens_id][buf_slot])
-      xsens_status[xsens_id]++;
-    break;
-  case GOT_DATA:
-    // Check for valid checksum
-    if (ck[xsens_id] != 0) {
-      //xsens_err_count[xsens_id]++;
-      goto error;
-    }
-    // Notification for valid message received
-    xsens_msg_received[xsens_id] = TRUE;
-    //xsens_recv_count[xsens_id]++;
-    goto restart;
-    break;
-  }
-  return;
- error:
- restart:
-  // Start over (Reset parser state)
-  xsens_status[xsens_id] = UNINIT;
-  return;
-}
diff --git a/sw/airborne/csc/csc_xsens.h b/sw/airborne/csc/csc_xsens.h
deleted file mode 100644
index 312fe5a7ef..0000000000
--- a/sw/airborne/csc/csc_xsens.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
- * Copyright (C) 2008 Joby Energy
- *
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file xsens.h
- */
-
-#ifndef __CSC_XSENS_H__
-#define __CSC_XSENS_H__
-
-#include "types.h"
-#include "math/pprz_algebra_float.h"
-
-#define XSENS_COUNT 1
-
-extern uint8_t xsens_mode[XSENS_COUNT]; // Receiver status
-extern volatile uint8_t xsens_msg_received[XSENS_COUNT];
-
-extern float xsens_phi[XSENS_COUNT];
-extern float xsens_theta[XSENS_COUNT];
-extern float xsens_psi[XSENS_COUNT];
-extern uint8_t xsens_msg_status[XSENS_COUNT];
-
-extern float xsens_r_a[XSENS_COUNT];
-extern float xsens_r_b[XSENS_COUNT];
-extern float xsens_r_c[XSENS_COUNT];
-extern float xsens_r_d[XSENS_COUNT];
-extern float xsens_r_e[XSENS_COUNT];
-extern float xsens_r_f[XSENS_COUNT];
-extern float xsens_r_g[XSENS_COUNT];
-extern float xsens_r_h[XSENS_COUNT];
-extern float xsens_r_i[XSENS_COUNT];
-
-extern float xsens_accel_x[XSENS_COUNT];
-extern float xsens_accel_y[XSENS_COUNT];
-extern float xsens_accel_z[XSENS_COUNT];
-extern float xsens_mag_x[XSENS_COUNT];
-extern float xsens_mag_y[XSENS_COUNT];
-extern float xsens_mag_z[XSENS_COUNT];
-extern float xsens_gyro_x[XSENS_COUNT];
-extern float xsens_gyro_y[XSENS_COUNT];
-extern float xsens_gyro_z[XSENS_COUNT];
-extern float xsens_mag_heading[XSENS_COUNT];
-
-// adjustable heading offset
-extern float xsens_psi_offset[XSENS_COUNT];
-
-extern uint16_t xsens_time_stamp[XSENS_COUNT];
-
-extern int xsens_setzero;
-
-extern struct FloatRMat xsens_rmat_neutral[XSENS_COUNT];
-
-#define PERIODIC_SEND_IMU_GYRO(_chan) DOWNLINK_SEND_IMU_GYRO (_chan, 	\
-  &xsens_gyro_x, &xsens_gyro_y, &xsens_gyro_z \
-)
-
-#define PERIODIC_SEND_IMU_ACCEL(_chan) DOWNLINK_SEND_IMU_ACCEL (_chan, 	\
-  &xsens_accel_x, &xsens_accel_y, &xsens_accel_z \
-)
-
-#define PERIODIC_SEND_IMU_MAG(_chan) DOWNLINK_SEND_IMU_MAG (_chan, 	\
-  &xsens_mag_x, &xsens_mag_y, &xsens_mag_z \
-)
-
-#define PERIODIC_SEND_ATTITUDE(_chan) DOWNLINK_SEND_ATTITUDE (_chan,	\
-  &xsens_phi, &xsens_psi, &xsens_theta \
-)
-
-#define PERIODIC_SEND_RMAT_DEBUG(_chan) DOWNLINK_SEND_RMAT_DEBUG (_chan,  \
-  &xsens_rmat_neutral[0].m[0], \
-  &xsens_rmat_neutral[0].m[1], \
-  &xsens_rmat_neutral[0].m[2], \
-  &xsens_rmat_neutral[0].m[3], \
-  &xsens_rmat_neutral[0].m[4], \
-  &xsens_rmat_neutral[0].m[5], \
-  &xsens_rmat_neutral[0].m[6], \
-  &xsens_rmat_neutral[0].m[7], \
-  &xsens_rmat_neutral[0].m[8] \
-)
-
-#include "std.h"
-
-void xsens_init(void);
-void xsens_parse_msg(uint8_t xsens_id);
-void xsens_event_task(void);
-void xsens_periodic_task(void);
-
-#endif
diff --git a/sw/airborne/csc/mercury_ap.c b/sw/airborne/csc/mercury_ap.c
deleted file mode 100644
index 2450122ea1..0000000000
--- a/sw/airborne/csc/mercury_ap.c
+++ /dev/null
@@ -1,189 +0,0 @@
-/*
- * Copyright (C) 2009 Joby Energy
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file csc_ap.c
- */
-
-#include <inttypes.h>
-#include "commands.h"
-#include "mercury_xsens.h"
-#include "firmwares/rotorcraft/autopilot.h"
-#include "firmwares/rotorcraft/stabilization.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
-#include "led.h"
-#include "math/pprz_algebra_float.h"
-#include "string.h"
-#include "subsystems/radio_control.h"
-#include "mercury_supervision.h"
-#include "firmwares/rotorcraft/actuators.h"
-#include "props_csc.h"
-#include "csc_booz2_guidance_v.h"
-
-static const int xsens_id = 0;
-
-// based on booz2_autopilot.c - 4c4112f044adeb48c5af7afbc070863839f697c9
-// -- mmt 6/15/09
-
-
-uint8_t booz2_autopilot_mode;
-uint8_t booz2_autopilot_mode_auto2;
-bool_t  booz2_autopilot_motors_on;
-bool_t  booz2_autopilot_in_flight;
-uint32_t booz2_autopilot_motors_on_counter;
-uint32_t booz2_autopilot_in_flight_counter;
-
-uint16_t mercury_yaw_servo_gain;
-
-uint8_t props_enable[PROPS_NB];
-uint8_t props_throttle_pass;
-
-
-#define BOOZ2_AUTOPILOT_MOTOR_ON_TIME     (512/4)
-#define BOOZ2_AUTOPILOT_IN_FLIGHT_TIME    40
-#define BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ * -14 / 20)
-#define BOOZ2_AUTOPILOT_YAW_TRESHOLD      (MAX_PPRZ *  14 / 20)
-
-
-void csc_ap_init(void) {
-  booz2_autopilot_mode = AP_MODE_FAILSAFE;
-  booz2_autopilot_motors_on = FALSE;
-  booz2_autopilot_in_flight = FALSE;
-  booz2_autopilot_motors_on_counter = 0;
-  booz2_autopilot_in_flight_counter = 0;
-  booz2_autopilot_mode_auto2 = MODE_AUTO2;
-
-  props_throttle_pass = 0;
-  for(uint8_t i = 0; i < PROPS_NB; i++){
-    props_enable[i] = 1;
-  }
-}
-
-
-
-#define BOOZ2_AUTOPILOT_CHECK_IN_FLIGHT() {				\
-    if (booz2_autopilot_in_flight) {					\
-      if (booz2_autopilot_in_flight_counter > 0) {			\
-	if (radio_control.values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \
-	  booz2_autopilot_in_flight_counter--;				\
-	  if (booz2_autopilot_in_flight_counter == 0) {			\
-	    booz2_autopilot_in_flight = FALSE;				\
-	  }								\
-	}								\
-	else {	/* rc throttle > threshold */				\
-	  booz2_autopilot_in_flight_counter = BOOZ2_AUTOPILOT_IN_FLIGHT_TIME; \
-        }								\
-      }									\
-    }									\
-    else { /* not in flight */						\
-      if (booz2_autopilot_in_flight_counter < BOOZ2_AUTOPILOT_IN_FLIGHT_TIME && \
-	  booz2_autopilot_motors_on) {					\
-	if (radio_control.values[RADIO_THROTTLE] > BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD) { \
-	  booz2_autopilot_in_flight_counter++;				\
-	  if (booz2_autopilot_in_flight_counter == BOOZ2_AUTOPILOT_IN_FLIGHT_TIME) \
-	    booz2_autopilot_in_flight = TRUE;				\
-	}								\
-	else { /*  rc throttle < threshold */				\
-	  booz2_autopilot_in_flight_counter = 0;			\
-	}								\
-      }									\
-    }									\
-  }
-
-#define BOOZ2_AUTOPILOT_CHECK_MOTORS_ON() {				\
-    if(!booz2_autopilot_motors_on){					\
-      if (radio_control.values[RADIO_THROTTLE] < BOOZ2_AUTOPILOT_THROTTLE_TRESHOLD && \
-	  (radio_control.values[RADIO_YAW] > BOOZ2_AUTOPILOT_YAW_TRESHOLD ||	\
-	   radio_control.values[RADIO_YAW] < -BOOZ2_AUTOPILOT_YAW_TRESHOLD)) {	\
-	  if ( booz2_autopilot_motors_on_counter <  BOOZ2_AUTOPILOT_MOTOR_ON_TIME) { \
-	    booz2_autopilot_motors_on_counter++;			\
-	    if (booz2_autopilot_motors_on_counter == BOOZ2_AUTOPILOT_MOTOR_ON_TIME){ \
-	      booz2_autopilot_motors_on = TRUE;				\
-	      booz2_autopilot_in_flight_counter += BOOZ2_AUTOPILOT_MOTOR_ON_TIME; \
-	    }								\
-	  }								\
-      } else {								\
-	booz2_autopilot_motors_on_counter = 0;				\
-      }									\
-    }									\
-  }
-
-
-// adapted from commands.h - cb6e74ae259a2384046f431c35735dc8018c0ecd
-// mmt -- 06/16/09
-const pprz_t csc_ap_commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
-
-
-#define CscSetCommands(_in_cmd, _in_flight, _motors_on) {			\
-    commands[COMMAND_PITCH]  = _in_cmd[COMMAND_PITCH];		\
-    commands[COMMAND_ROLL]   = _in_cmd[COMMAND_ROLL];		\
-    commands[COMMAND_YAW]    = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \
-    commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \
-  }
-
-pprz_t mixed_commands[PROPS_NB];
-
-
-void csc_ap_periodic(uint8_t _in_flight, uint8_t kill) {
-  //  RunOnceEvery(50, nav_periodic_task_10Hz())
-  BOOZ2_AUTOPILOT_CHECK_MOTORS_ON();
-  if(kill) booz2_autopilot_motors_on = FALSE;
-  booz2_autopilot_in_flight = _in_flight;
-
-  stabilization_attitude_read_rc(booz2_autopilot_in_flight);
-  stabilization_attitude_run(booz2_autopilot_in_flight);
-  booz2_guidance_v_run(booz2_autopilot_in_flight);
-
-  stabilization_cmd[COMMAND_THRUST] = (int32_t)radio_control.values[RADIO_THROTTLE] * 105 / 7200 + 95;
-
-
-  CscSetCommands(stabilization_cmd,
-		 booz2_autopilot_in_flight,booz2_autopilot_motors_on);
-
-
-  BOOZ2_SUPERVISION_RUN(mixed_commands, commands, booz2_autopilot_motors_on);
-
-
-  if(booz2_autopilot_motors_on && props_throttle_pass){
-    Bound(stabilization_cmd[COMMAND_THRUST],0,255);
-    for(uint8_t i = 0; i < PROPS_NB; i++)
-      mixed_commands[i] = stabilization_cmd[COMMAND_THRUST];
-
-  }
-
-  for(uint8_t i = 0; i < PROPS_NB; i++){
-    if(props_enable[i])
-      props_set(i,mixed_commands[i]);
-    else
-      props_set(i,0);
-  }
-
-  props_commit();
-
-
-  MERCURY_SURFACES_SUPERVISION_RUN(Actuator,
-				   stabilization_cmd,
-				   mixed_commands,
-				   (!booz2_autopilot_in_flight));
-  ActuatorsCommit();
-
-  SendCscFromActuators();
-}
diff --git a/sw/airborne/csc/mercury_csc_main.c b/sw/airborne/csc/mercury_csc_main.c
deleted file mode 100644
index 4c259524a9..0000000000
--- a/sw/airborne/csc/mercury_csc_main.c
+++ /dev/null
@@ -1,262 +0,0 @@
-/*
- * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $
- *
- * Copyright (C) 2008  Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include <inttypes.h>
-
-#include "csc_main.h"
-
-#include "std.h"
-
-#include "mcu.h"
-#include "sys_time.h"
-#include "led.h"
-
-#include "csc_vane.h"
-
-#ifdef USE_BUSS_TWI_BLMC_MOTOR
-#include "buss_twi_blmc_hw.h"
-#endif
-#include "i2c.h"
-
-#include "csc_servos.h"
-
-#include "airspeed.h"
-#include "baro_ets.h"
-#include "airspeed_ets.h"
-// #include "ams5812.h"
-
-#include "interrupt_hw.h"
-#include "mcu_periph/uart.h"
-#include "csc_telemetry.h"
-
-#include "generated/periodic.h"
-#include "downlink.h"
-
-#include "pwm_input.h"
-#include "csc_airspeed.h"
-#include "csc_baro.h"
-#include "csc_bat_monitor.h"
-
-#include "csc_adc.h"
-
-#include "csc_rc_spektrum.h"
-
-#include "csc_can.h"
-#include "csc_ap_link.h"
-
-static inline void on_servo_cmd(struct CscServoCmd *cmd);
-static inline void on_motor_cmd(struct CscMotorMsg *msg);
-static inline void on_prop_cmd(struct CscPropCmd *msg, int idx);
-
-#define SERVO_TIMEOUT (SYS_TICS_OF_SEC(0.1) / PERIODIC_TASK_PERIOD)
-#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
-#define AIRSPEED_TIMEOUT (SYS_TICS_OF_SEC(0.01) / PERIODIC_TASK_PERIOD)
-
-
-static uint32_t servo_cmd_timeout = 0;
-static uint32_t can_msg_count = 0;
-
-
-static void csc_main_init( void ) {
-
-	hw_init();
-	sys_time_init();
-	led_init();
-
-	actuators_init();
-
-
-#ifdef USE_UART0
-	Uart0Init();
-#endif
-
-#ifdef USE_UART1
-	Uart1Init();
-#endif
-
-#ifdef SPEKTRUM_LINK
-	spektrum_init();
-#endif
-
-#ifdef USE_PWM_INPUT
-	pwm_input_init();
-#endif
-
-
-// be sure to call servos_init after uart1 init since they are sharing pins
-	csc_servos_init();
-
-#ifdef USE_VANE_SENSOR
-#ifndef USE_PWM_INPUT
-	pwm_input_init();
-#endif
-#endif
-
-	csc_ap_link_init();
-	csc_ap_link_set_servo_cmd_cb(on_servo_cmd);
-	csc_ap_link_set_motor_cmd_cb(on_motor_cmd);
-	csc_ap_link_set_prop_cmd_cb(on_prop_cmd);
-
-#ifdef ADC
-	csc_adc_init();
-#endif
-
-
-#ifdef USE_I2C0
-	i2c0_init();
-#endif
-
-#ifdef USE_BUSS_TWI_BLMC_MOTOR
-	motors_init();
-#endif
-
-#ifdef USE_AIRSPEED
-	airspeed_init();
-#endif
-#ifdef USE_AMS5812
-		csc_ams5812_init();
-#endif
-#ifdef USE_BARO_SCP
-	baro_scp_init();
-#endif
-
-#ifdef USE_BAT_MONITOR
-	csc_bat_monitor_init();
-#endif
-
-	int_enable();
-
-}
-
-static void csc_main_periodic( void )
-{
-	static uint32_t zeros[4] = {0, 0, 0, 0};
-	static uint32_t csc_loops = 0;
-
-#ifdef DOWNLINK
-	PeriodicSendAp(DefaultChannel);
-#endif
-
-#ifdef USE_VANE_SENSOR
-	csc_vane_periodic();
-#endif
-
-	if (servo_cmd_timeout > SERVO_TIMEOUT) {
-		csc_servos_set(zeros);
-	} else {
-		servo_cmd_timeout++;
-	}
-
-	if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
-		csc_ap_link_send_status(csc_loops, can_msg_count);
-	}
-
-	csc_adc_periodic();
-
-	if ((csc_loops % AIRSPEED_TIMEOUT) == 0) {
-	} else if ((csc_loops % AIRSPEED_TIMEOUT) == 1) {
-#ifdef USE_BARO_ETS
-		baro_ets_periodic();
-#endif
-#ifdef USE_AIRSPEED
-		csc_airspeed_periodic();
-#endif
-#ifdef USE_AMS5812
-		csc_ams5812_periodic();
-		csc_ap_link_send_pressure(ams5812_pressure[0], ams5812_pressure[1]);
-#endif
-	}
-
-#ifdef USE_AIRSPEED
-	airspeed_update();
-#endif
-
-#ifdef USE_BARO_SCP
-	baro_scp_periodic();
-	csc_ap_link_send_baro(baro_scp_pressure, baro_scp_temperature, baro_scp_status);
-#endif
-
-#ifdef USE_BAT_MONITOR
-	csc_bat_monitor_periodic();
-#endif
-
-}
-
-static void csc_main_event( void ) {
-	csc_can_event();
-
-#ifdef USE_BUSS_TWI_BLMC_MOTOR
-	motors_event();
-#endif
-
-#ifdef SPEKTRUM_LINK
-	spektrum_event_task();
-#endif
-}
-
-#define MIN_SERVO SYS_TICS_OF_USEC(1000)
-#define MAX_SERVO SYS_TICS_OF_USEC(2000)
-
-#ifdef USE_BUSS_TWI_BLMC_MOTOR
-static void on_prop_cmd(struct CscPropCmd *cmd, int idx)
-{
-	for(uint8_t i = 0; i < 4; i++)
-		motors_set_motor(i + idx * 4, cmd->speeds[i]);
-
-	motors_commit(idx == 1);
-
-	++can_msg_count;
-}
-#else
-static void on_prop_cmd(struct CscPropCmd *cmd, int idx) {}
-#endif
-
-static void on_servo_cmd(struct CscServoCmd *cmd)
-{
-	uint16_t* servos = (uint16_t*)(cmd);
-	uint8_t i;
-
-	//	uint32_t servos_checked[4];
-	for(i = 0; i < 4; i++) {
-		csc_servo_normalized_set(i,servos[i]);
-	}
-	csc_servos_commit();
-	servo_cmd_timeout = 0;
-	++can_msg_count;
-}
-
-static void on_motor_cmd(struct CscMotorMsg *msg) {}
-
-int main( void ) {
-	csc_main_init();
-//	Uart0PrintString("Hello");
-	while(1) {
-		if (sys_time_periodic()) {
-			csc_main_periodic();
-		}
-		csc_main_event();
-	}
-	return 0;
-}
-
diff --git a/sw/airborne/csc/mercury_main.c b/sw/airborne/csc/mercury_main.c
deleted file mode 100644
index b6914fb70f..0000000000
--- a/sw/airborne/csc/mercury_main.c
+++ /dev/null
@@ -1,171 +0,0 @@
-/*
- * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $
- *
- * Copyright (C) 2008  Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include <inttypes.h>
-
-#include "csc_main.h"
-
-#include "std.h"
-
-#include "mcu.h"
-#include "sys_time.h"
-#include "led.h"
-#include "interrupt_hw.h"
-#include "mcu_periph/uart.h"
-#include "downlink.h"
-#include "generated/periodic.h"
-#include "generated/airframe.h"
-#include "commands.h"
-
-#include "csc_msg_def.h"
-#include ACTUATORS
-#include "subsystems/imu.h"
-#include "booz/ahrs/ahrs_aligner.h"
-#include "booz/ahrs.h"
-#include "mercury_xsens.h"
-#include "csc_telemetry.h"
-#include "csc_adc.h"
-#include "mercury_ap.h"
-#include "booz/booz2_autopilot.h"
-#include "booz/guidance/booz2_guidance_v.h"
-#include "csc_can.h"
-#include "csc_baro.h"
-#include "subsystems/radio_control.h"
-
-#include "booz/stabilization/stabilization_attitude.h"
-
-extern uint8_t vsupply;
-
-
-#define CSC_STATUS_TIMEOUT (SYS_TICS_OF_SEC(0.25) / PERIODIC_TASK_PERIOD)
-
-#define PPRZ_MODE_MOTORS_OFF 0
-#define PPRZ_MODE_MOTORS_ON  1
-#define PPRZ_MODE_IN_FLIGHT  2
-
-uint8_t pprz_mode = PPRZ_MODE_MOTORS_OFF;
-static uint16_t cpu_time = 0;
-
-static inline void csc_main_init( void );
-static inline void csc_main_periodic( void );
-static inline void csc_main_event( void );
-
-int main( void ) {
-  csc_main_init();
-  while(1) {
-    if (sys_time_periodic())
-      csc_main_periodic();
-    csc_main_event();
-  }
-  return 0;
-}
-
-static void nop(struct CscCanMsg *msg)
-{
-
-}
-
-static void on_rc_cmd(struct CscRCMsg *msg)
-{
-  uint16_t aux2_flag;
-
-  radio_control.values[RADIO_ROLL]  = CSC_RC_SCALE*(msg->right_stick_horizontal - CSC_RC_OFFSET);
-  radio_control.values[RADIO_PITCH] = -CSC_RC_SCALE*(msg->right_stick_vertical - CSC_RC_OFFSET);
-  radio_control.values[RADIO_YAW]   =  CSC_RC_SCALE*((msg->left_stick_horizontal_and_aux2 & ~(3 << 13)) - CSC_RC_OFFSET);
-  pprz_mode = (msg->left_stick_vertical_and_flap_mix & (3 << 13)) >> 13;
-  aux2_flag = (msg->left_stick_horizontal_and_aux2 >> 13) & 0x1;
-  radio_control.values[RADIO_MODE2] = (aux2_flag == 0) ? -7000 : ( (aux2_flag == 1) ? 0 : 7000);
-  radio_control.values[RADIO_MODE] = (pprz_mode == 0) ? -7000 : ( (pprz_mode == 1) ? 0 : 7000);
-  radio_control.values[RADIO_THROTTLE] = -CSC_RC_SCALE*((msg->left_stick_vertical_and_flap_mix & ~(3 << 13)) - CSC_RC_OFFSET);
-
-  radio_control.time_since_last_frame = 0;
-  radio_control.status = RC_OK;
-}
-
-static inline void csc_main_init( void ) {
-
-  mcu_init();
-  sys_time_init();
-  led_init();
-
-  Uart0Init();
-  Uart1Init();
-
-  imu_init();
-
-  ahrs_aligner_init();
-  ahrs_init();
-
-  xsens_init();
-
-  stabilization_attitude_init();
-  booz2_guidance_v_init();
-  booz_ins_init();
-
-  csc_adc_init();
-
-  baro_scp_init();
-
-  csc_ap_link_init();
-  csc_ap_link_set_servo_cmd_cb(&nop);
-  csc_ap_link_set_motor_cmd_cb(&nop);
-  csc_ap_link_set_rc_cmd_cb(on_rc_cmd);
-  actuators_init();
-
-  props_init();
-
-  csc_ap_init();
-  mcu_int_enable();
-
-  stabilization_attitude_enter();
-}
-
-
-static inline void csc_main_periodic( void )
-{
-  static uint32_t csc_loops = 0;
-
-  PeriodicSendAp();
-  radio_control_periodic();
-
-  cpu_time++;
-
-  if ((++csc_loops % CSC_STATUS_TIMEOUT) == 0) {
-    csc_adc_periodic();
-  }
-  xsens_periodic_task();
-
-  baro_scp_periodic();
-
-  csc_ap_periodic(pprz_mode == PPRZ_MODE_IN_FLIGHT,
-		  !(pprz_mode > PPRZ_MODE_MOTORS_OFF && ahrs_aligner.status == AHRS_ALIGNER_LOCKED));
-
-}
-
-static inline void csc_main_event( void )
-{
-  csc_can_event();
-  xsens_event_task();
-  DatalinkEvent();
-}
diff --git a/sw/airborne/csc/mercury_supervision.h b/sw/airborne/csc/mercury_supervision.h
deleted file mode 100644
index abb4691514..0000000000
--- a/sw/airborne/csc/mercury_supervision.h
+++ /dev/null
@@ -1,116 +0,0 @@
-#ifndef BOOZ2_SUPERVISION_H
-#define BOOZ2_SUPERVISION_H
-
-#include "generated/airframe.h"
-
-extern pprz_t mercury_supervision_yaw_servo_gain;
-extern pprz_t mercury_supervision_pitch_servo_gain;
-extern pprz_t mercury_supervision_yaw_motor_gain;
-extern pprz_t mercury_supervision_yaw_comp_slope;
-extern pprz_t mercury_supervision_yaw_comp_offset;
-
-/* #if defined SUPERVISION_FRONT_ROTOR_CW */
-/* #define TRIM_FRONT ( SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) */
-/* #define TRIM_RIGHT (-SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) */
-/* #define TRIM_BACK  (-SUPERVISION_TRIM_E-SUPERVISION_TRIM_R) */
-/* #define TRIM_LEFT  ( SUPERVISION_TRIM_A+SUPERVISION_TRIM_R) */
-/* #define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) {		\ */
-/*     _mot_cmd[PROP_FRONT] = _dt + _de - _dr + TRIM_FRONT;	\ */
-/*     _mot_cmd[PROP_RIGHT] = _dt - _da + _dr + TRIM_RIGHT;	\ */
-/*     _mot_cmd[PROP_BACK]  = _dt - _de - _dr + TRIM_BACK;	\ */
-/*     _mot_cmd[PROP_LEFT]  = _dt + _da + _dr + TRIM_LEFT;	\ */
-/*   } */
-/* #else */
-#define TRIM_UPPER_LEFT  ( SUPERVISION_TRIM_A-SUPERVISION_TRIM_E+SUPERVISION_TRIM_R)
-#define TRIM_LOWER_RIGHT (-SUPERVISION_TRIM_A+SUPERVISION_TRIM_E+SUPERVISION_TRIM_R)
-#define TRIM_LOWER_LEFT  ( SUPERVISION_TRIM_A+SUPERVISION_TRIM_E-SUPERVISION_TRIM_R)
-#define TRIM_UPPER_RIGHT (-SUPERVISION_TRIM_A-SUPERVISION_TRIM_E-SUPERVISION_TRIM_R)
-#define SUPERVISION_MIX(_mot_cmd, _da, _de, _dr, _dt) {			\
-    _mot_cmd[PROP_UPPER_LEFT]  = _dt + _da - _de  + _dr  + TRIM_UPPER_LEFT; \
-    _mot_cmd[PROP_LOWER_RIGHT] = _dt - _da + _de  + _dr  + TRIM_LOWER_RIGHT;	\
-    _mot_cmd[PROP_LOWER_LEFT]  = _dt + _da + _de  - _dr  + TRIM_LOWER_LEFT;	\
-    _mot_cmd[PROP_UPPER_RIGHT] = _dt - _da - _de  - _dr  + TRIM_UPPER_RIGHT;	\
-  }
-//#endif
-
-#define SUPERVISION_FIND_MAX_MOTOR(_mot_cmd, _max_mot) {	\
-    _max_mot = (-32767-1); /* INT16_MIN;*/			\
-    if (_mot_cmd[PROP_UPPER_LEFT] > _max_mot)			\
-      max_mot = _mot_cmd[PROP_UPPER_LEFT];				\
-    if (_mot_cmd[PROP_LOWER_RIGHT] > _max_mot)			\
-      max_mot = _mot_cmd[PROP_LOWER_RIGHT];				\
-    if (_mot_cmd[PROP_LOWER_LEFT] > _max_mot)			\
-      max_mot = _mot_cmd[PROP_LOWER_LEFT];				\
-    if (_mot_cmd[PROP_UPPER_RIGHT] > _max_mot)			\
-      max_mot = _mot_cmd[PROP_UPPER_RIGHT];				\
-  }
-
-#define SUPERVISION_FIND_MIN_MOTOR(_mot_cmd, _min_mot) {	\
-    _min_mot = (32767); /*INT16_MAX;*/				\
-    if (_mot_cmd[PROP_UPPER_LEFT] < _min_mot)			\
-      min_mot = _mot_cmd[PROP_UPPER_LEFT];				\
-    if (_mot_cmd[PROP_LOWER_RIGHT] < _min_mot)			\
-      min_mot = _mot_cmd[PROP_LOWER_RIGHT];				\
-    if (_mot_cmd[PROP_LOWER_LEFT] < _min_mot)			\
-      min_mot = _mot_cmd[PROP_LOWER_LEFT];				\
-    if (_mot_cmd[PROP_UPPER_RIGHT] < _min_mot)			\
-      min_mot = _mot_cmd[PROP_UPPER_RIGHT];				\
-  }
-
-#define SUPERVISION_OFFSET_MOTORS(_mot_cmd, _offset) {	\
-    _mot_cmd[PROP_UPPER_LEFT] += _offset;			\
-    _mot_cmd[PROP_LOWER_RIGHT] += _offset;			\
-    _mot_cmd[PROP_LOWER_LEFT]  += _offset;			\
-    _mot_cmd[PROP_UPPER_RIGHT]  += _offset;			\
-  }
-
-#define SUPERVISION_BOUND_MOTORS(_mot_cmd) {				\
-    Bound(_mot_cmd[PROP_UPPER_LEFT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
-    Bound(_mot_cmd[PROP_LOWER_RIGHT], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
-    Bound(_mot_cmd[PROP_LOWER_LEFT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
-    Bound(_mot_cmd[PROP_UPPER_RIGHT] , SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); \
-  }
-
-#define MERCURY_SURFACES_SUPERVISION_RUN(_actuators,_cmds,_props,_surfaces_manual) { \
-    if (_surfaces_manual) {						\
-      _actuators(SERVO_S1)  = (SERVO_S1_MAX+SERVO_S1_MIN)/2 +((SERVO_S1_MAX-SERVO_S1_MIN)*radio_control.values[RADIO_YAW])/2/7200; \
-      _actuators(SERVO_S2)  = (SERVO_S2_MAX+SERVO_S2_MIN)/2 +((SERVO_S2_MAX-SERVO_S2_MIN)*radio_control.values[RADIO_YAW])/2/7200; \
-    } else {								\
-      int32_t bndcmd = (mercury_supervision_yaw_servo_gain*_cmds[COMMAND_YAW]*mercury_supervision_yaw_comp_offset)/(mercury_supervision_yaw_comp_slope*((_props[PROP_UPPER_RIGHT]+_props[PROP_UPPER_LEFT])/2 - 105) + mercury_supervision_yaw_comp_offset); \
-      Bound(bndcmd,-400,400);						\
-      _actuators(SERVO_S1)  = (SERVO_S1_MAX+SERVO_S1_MIN)/2 -		\
-	((SERVO_S1_MAX-SERVO_S1_MIN)*bndcmd)/2/400; \
-      _actuators(SERVO_S2)  = (SERVO_S2_MAX+SERVO_S2_MIN)/2 - \
-	((SERVO_S2_MAX-SERVO_S2_MIN)*bndcmd)/2/400; \
-    } \
-  }
-
-#define BOOZ2_SUPERVISION_RUN(_out, _in,_motors_on) {			\
-    if (_motors_on) {							\
-      SUPERVISION_MIX(_out, _in[COMMAND_ROLL], _in[COMMAND_PITCH], (mercury_supervision_yaw_motor_gain*_in[COMMAND_YAW])/20, _in[COMMAND_THRUST]); \
-      pprz_t min_mot;							\
-      SUPERVISION_FIND_MIN_MOTOR(_out, min_mot);			\
-      if (min_mot < SUPERVISION_MIN_MOTOR) {				\
-	pprz_t offset = -(min_mot - SUPERVISION_MIN_MOTOR);		\
-	SUPERVISION_OFFSET_MOTORS(_out, offset) ;			\
-      }									\
-      pprz_t max_mot;							\
-      SUPERVISION_FIND_MAX_MOTOR(_out, max_mot);			\
-      if (max_mot > SUPERVISION_MAX_MOTOR) {				\
-	pprz_t offset = -(max_mot - SUPERVISION_MAX_MOTOR);		\
-	SUPERVISION_OFFSET_MOTORS(_out, offset) ;			\
-      }									\
-      SUPERVISION_BOUND_MOTORS(_out);					\
-    }									\
-    else {								\
-      _out[PROP_UPPER_LEFT] = 0;						\
-      _out[PROP_LOWER_RIGHT] = 0;						\
-      _out[PROP_LOWER_LEFT]  = 0;						\
-      _out[PROP_UPPER_RIGHT]  = 0;						\
-    }									\
-  }
-
-
-
-
-#endif /* BOOZ2_SUPERVISION_H */
diff --git a/sw/airborne/csc/mercury_xsens.c b/sw/airborne/csc/mercury_xsens.c
deleted file mode 100644
index 54f4c5e447..0000000000
--- a/sw/airborne/csc/mercury_xsens.c
+++ /dev/null
@@ -1,469 +0,0 @@
-/*
- * Paparazzi mcu0 $Id: quad_xsens.c,v 1.2 2008/05/07 12:54:23 gautier Exp $
- *
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file csc_xsens.c
- * \brief Parser for the Xsens protocol
- */
-
-#include "mercury_xsens.h"
-#include "subsystems/imu.h"
-#include "booz/ahrs.h"
-#include "booz/ahrs/ahrs_aligner.h"
-#include "subsystems/imu.h"
-#include "csc_booz2_ins.h"
-
-#include <inttypes.h>
-
-#include "led.h"
-
-#include "downlink.h"
-#include "messages.h"
-#include "mcu_periph/uart.h"
-//#include "com_stats.h"
-#include "math/pprz_algebra_float.h"
-#include "string.h"
-
-void parse_xsens_msg(uint8_t xsens_id, uint8_t c );
-
-#define __Xsens2Link(dev, _x) dev##_x
-#define _Xsens2Link(dev, _x)  __Xsens2Link(dev, _x)
-#define Xsens2Link(_x) _Xsens2Link(XSENS2_LINK, _x)
-
-#define Xsens2Buffer() Xsens2Link(ChAvailable())
-#define ReadXsens2Buffer() { while (Xsens2Link(ChAvailable())&&!xsens_msg_received[1]) parse_xsens_msg(1, Xsens2Link(Getch())); }
-
-#define Xsens2UartSend1(c) Xsens2Link(Transmit(c))
-#define Xsens2UartInitParam(_a,_b,_c) Xsens2Link(InitParam(_a,_b,_c))
-#define Xsens2UartRunning Xsens2Link(TxRunning)
-
-#define Xsens2InitCheksum() { send_ck[1] = 0; }
-#define Xsens2UpdateChecksum(c) { send_ck[1] += c; }
-
-#define Xsens2Send1(c) { uint8_t i8=c; XSens2UartSend1(i8); Xsens2UpdateChecksum(i8); }
-#define Xsens2Send1ByAddr(x) { Xsens2Send1(*x); }
-#define Xsens2Send2ByAddr(x) { Xsens2Send1(*(x+1)); Xsens2Send1(*x); }
-#define Xsens2Send4ByAddr(x) { Xsens2Send1(*(x+3)); Xsens2Send1(*(x+2)); Xsens2Send1(*(x+1)); Xsens2Send1(*x); }
-
-#define Xsens2Header(msg_id, len) { \
-  Xsens2UartSend1(XSENS_START); \
-  Xsens2InitCheksum(); \
-  Xsens2Send1(XSENS_BID); \
-  Xsens2Send1(msg_id); \
-  Xsens2Send1(len); \
-}
-#define Xsens2Trailer() { uint8_t i8=0x100-send_ck[1]; Xsens2UartSend1(i8); }
-
-#define __Xsens1Link(dev, _x) dev##_x
-#define _Xsens1Link(dev, _x)  __Xsens1Link(dev, _x)
-#define Xsens1Link(_x) _Xsens1Link(XSENS1_LINK, _x)
-
-#define Xsens1Buffer() Xsens1Link(ChAvailable())
-#define ReadXsens1Buffer() { while (Xsens1Link(ChAvailable())&&!xsens_msg_received[0]) parse_xsens_msg(0, Xsens1Link(Getch())); }
-
-#define Xsens1UartSend1(c) Xsens1Link(Transmit(c))
-#define Xsens1UartInitParam(_a,_b,_c) Xsens1Link(InitParam(_a,_b,_c))
-#define Xsens1UartRunning Xsens1Link(TxRunning)
-
-#define Xsens1InitCheksum() { send_ck[0] = 0; }
-#define Xsens1UpdateChecksum(c) { send_ck[0] += c; }
-
-#define Xsens1Send1(c) { uint8_t i8=c; XSens1UartSend1(i8); Xsens1UpdateChecksum(i8); }
-#define Xsens1Send1ByAddr(x) { Xsens1Send1(*x); }
-#define Xsens1Send2ByAddr(x) { Xsens1Send1(*(x+1)); Xsens1Send1(*x); }
-#define Xsens1Send4ByAddr(x) { Xsens1Send1(*(x+3)); Xsens1Send1(*(x+2)); Xsens1Send1(*(x+1)); Xsens1Send1(*x); }
-
-#define Xsens1Header(msg_id, len) { \
-  Xsens1UartSend1(XSENS_START); \
-  Xsens1InitCheksum(); \
-  Xsens1Send1(XSENS_BID); \
-  Xsens1Send1(msg_id); \
-  Xsens1Send1(len); \
-}
-#define Xsens1Trailer() { uint8_t i8=0x100-send_ck[0]; Xsens1UartSend1(i8); }
-
-
-/** Includes macros generated from xsens_MTi.xml */
-#include "xsens_protocol.h"
-
-uint8_t xsens_mode[XSENS_COUNT]; // Receiver status
-volatile uint8_t xsens_msg_received[XSENS_COUNT];
-
-float xsens_phi[XSENS_COUNT];
-float xsens_theta[XSENS_COUNT];
-float xsens_psi[XSENS_COUNT];
-
-float xsens_r_a[XSENS_COUNT];
-float xsens_r_b[XSENS_COUNT];
-float xsens_r_c[XSENS_COUNT];
-float xsens_r_d[XSENS_COUNT];
-float xsens_r_e[XSENS_COUNT];
-float xsens_r_f[XSENS_COUNT];
-float xsens_r_g[XSENS_COUNT];
-float xsens_r_h[XSENS_COUNT];
-float xsens_r_i[XSENS_COUNT];
-
-struct FloatRMat xsens_rmat[XSENS_COUNT];
-struct FloatRMat xsens_rmat_neutral[XSENS_COUNT];
-struct FloatRMat xsens_rmat_adj[XSENS_COUNT];
-
-float xsens_accel_x[XSENS_COUNT];
-float xsens_accel_y[XSENS_COUNT];
-float xsens_accel_z[XSENS_COUNT];
-
-float xsens_mag_x[XSENS_COUNT];
-float xsens_mag_y[XSENS_COUNT];
-float xsens_mag_z[XSENS_COUNT];
-
-float xsens_gyro_x[XSENS_COUNT];
-float xsens_gyro_y[XSENS_COUNT];
-float xsens_gyro_z[XSENS_COUNT];
-
-
-
-
-float xsens_mag_heading[XSENS_COUNT];
-
-int xsens_setzero = 0;
-
-#define XSENS_MSG_BUF 1
-#define XSENS_MAX_PAYLOAD 254
-uint8_t xsens_msg_buf[XSENS_COUNT][XSENS_MSG_BUF][XSENS_MAX_PAYLOAD];
-static volatile uint8_t xsens_msg_buf_count[XSENS_COUNT]; // buffer count
-static volatile uint8_t xsens_msg_buf_pi[XSENS_COUNT]; // produce index
-static volatile uint8_t xsens_msg_buf_ci[XSENS_COUNT]; // consume index
-
-#define XSENS_RAW_MODE
-
-// See Page 25 MT Low-Level Comm Doc
-#define XSENS_OUTPUT_CALIBRATED (1 << 1)
-#define XSENS_OUTPUT_ORIENTATION (1 << 2)
-#define XSENS_OUTPUT_AUXDATA (1 << 3)
-#define XSENS_OUTPUT_STATUS (1 << 11)
-#define XSENS_RAW_INERTIAL (1 << 14)
-#ifdef XSENS_RAW_MODE
-#define XSENS_DEFAULT_OUTPUT_MODE (XSENS_RAW_INERTIAL | XSENS_OUTPUT_STATUS)
-#else
-#define XSENS_DEFAULT_OUTPUT_MODE (XSENS_OUTPUT_ORIENTATION | XSENS_OUTPUT_STATUS | XSENS_OUTPUT_CALIBRATED)
-#endif
-
-// output settings : (Page 26 MT Low-Level Comm Doc)
-// Sample Counter, rotation matrix, floating point output
-#define XSENS_OUTPUT_AUX1 (1 << 10)
-#define XSENS_OUTPUT_AUX2 (1 << 11)
-#define XSENS_ORIENTATION (2 << 2)
-#define XSENS_ACCELS (1 << 4)
-#define XSENS_GYROS (1 << 5)
-#define XSENS_MAGS (1 << 6)
-#define XSENS_TIMESTAMP (1 << 0)
-#ifdef XSENS_RAW_MODE
-#define XSENS_DEFAULT_OUTPUT_SETTINGS (XSENS_ACCELS | XSENS_MAGS | XSENS_GYROS | XSENS_TIMESTAMP)
-#else
-#define XSENS_DEFAULT_OUTPUT_SETTINGS (XSENS_ORIENTATION | XSENS_ACCELS | XSENS_MAGS | XSENS_GYROS | XSENS_TIMESTAMP)
-#endif
-
-#define UNINIT        0
-#define GOT_START     1
-#define GOT_BID       2
-#define GOT_MID       3
-#define GOT_LEN       4
-#define GOT_DATA      5
-#define GOT_CHECKSUM  6
-
-uint8_t xsens_errorcode[XSENS_COUNT];
-uint8_t xsens_msg_status[XSENS_COUNT];
-uint16_t xsens_time_stamp[XSENS_COUNT];
-uint16_t xsens_output_mode[XSENS_COUNT];
-uint32_t xsens_output_settings[XSENS_COUNT];
-
-static uint8_t msg_id[XSENS_COUNT][XSENS_MSG_BUF];
-static uint8_t xsens_len[XSENS_COUNT][XSENS_MSG_BUF];
-static uint8_t xsens_msg_idx[XSENS_COUNT];
-static uint8_t ck[XSENS_COUNT];
-static uint8_t send_ck[XSENS_COUNT];
-
-void imu_impl_init( void  )
-{
-
-}
-
-void xsens_init( void )
-{
-  for (int i = 0; i < XSENS_COUNT; i++) {
-    send_ck[i] = 0;
-    xsens_msg_status[i] = 0;
-    xsens_time_stamp[i] = 0;
-    xsens_output_mode[i] = XSENS_DEFAULT_OUTPUT_MODE;
-    xsens_output_settings[i] = XSENS_DEFAULT_OUTPUT_SETTINGS;
-    xsens_msg_buf_count[i] = 0;
-    xsens_msg_buf_pi[i] = 0;
-    xsens_msg_buf_ci[i] = 0;
-    FLOAT_RMAT_ZERO(xsens_rmat_neutral[i]);
-  }
-  imu.accel_neutral.x = IMU_ACCEL_X_NEUTRAL;
-  imu.accel_neutral.y = IMU_ACCEL_Y_NEUTRAL;
-  imu.accel_neutral.z = IMU_ACCEL_Z_NEUTRAL;
-
-  imu.gyro_neutral.p = IMU_GYRO_P_NEUTRAL;
-  imu.gyro_neutral.q = IMU_GYRO_Q_NEUTRAL;
-  imu.gyro_neutral.r = IMU_GYRO_R_NEUTRAL;
-
-  imu.mag_neutral.x = IMU_MAG_X_NEUTRAL;
-  imu.mag_neutral.y = IMU_MAG_Y_NEUTRAL;
-  imu.mag_neutral.z = IMU_MAG_Z_NEUTRAL;
-  // Also TODO: set scenario to aerospace
-  // set magnetic declination angle
-  // Probably quicker to just set everything once via MT Manager software
-  // instead of setting via paparazzi (MT-G should store setting in flash)
-  //XSENS_GoToConfig();
-  //XSENS_SetOutputMode(xsens2_output_mode);
-  //XSENS_SetOutputSettings(xsens2_output_settings);
-  //XSENS_GoToMeasurment();
-}
-
-void xsens_periodic_task ( void )
-{
-  for (int i = 0; i < XSENS_COUNT; i++)
-  {
-    if (xsens_msg_buf_count[i] > 0) {
-      xsens_parse_msg(i);
-      xsens_msg_buf_count[i]--;
-      xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1) % XSENS_MSG_BUF;
-    }
-    if (xsens_setzero) {
-      memcpy(&xsens_rmat_neutral[i], &xsens_rmat[i], sizeof(struct FloatRMat));
-    }
-  }
-  xsens_setzero = 0;
-}
-
-void xsens_event_task( void )
-{
-  while (Xsens1Link(ChAvailable()) && !xsens_msg_received[0]) {
-    parse_xsens_msg(0, Xsens1Link(Getch()));
-  }
-
-/*
-  while (Xsens2Link(ChAvailable()) && !xsens_msg_received[1])
-    parse_xsens_msg(1, Xsens2Link(Getch()));
-*/
-
-  for (int i = 0; i < XSENS_COUNT; i++) {
-    if (xsens_msg_received[i]) {
-
-      // first make room
-      while (xsens_msg_buf_count[i] >= XSENS_MSG_BUF) {
-        // throwing away old stuff
-        xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1 ) % XSENS_MSG_BUF;
-        xsens_msg_buf_count[i]--;
-      }
-
-      xsens_msg_buf_pi[i] = (xsens_msg_buf_pi[i] + 1 ) % XSENS_MSG_BUF;
-      xsens_msg_buf_count[i]++;
-
-      //xsens_parse_msg(i);
-      xsens_msg_received[i] = FALSE;
-    }
-  }
-}
-
-// Called after receipt of valid message to
-void xsens_parse_msg( uint8_t xsens_id ) {
-  uint8_t buf_slot = xsens_msg_buf_ci[xsens_id];
-
-  if (msg_id[xsens_id][buf_slot] == XSENS_ReqOutputModeAck_ID) {
-    xsens_output_mode[xsens_id] = XSENS_ReqOutputModeAck_mode(xsens_msg_buf[xsens_id]);
-  }
-  else if (msg_id[xsens_id][buf_slot] == XSENS_Error_ID) {
-    xsens_errorcode[xsens_id] = XSENS_Error_errorcode(xsens_msg_buf[xsens_id]);
-  }
-  else if (msg_id[xsens_id][buf_slot] == XSENS_MTData_ID) {
-    uint8_t offset = 0;
-    // test RAW modes else calibrated modes
-      if (XSENS_MASK_RAWInertial(xsens_output_mode[xsens_id])){// || (XSENS_MASK_RAWGPS(xsens2_output_mode))) {
-	imu.accel_unscaled.x = XSENS_DATA_RAWInertial_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-	imu.accel_unscaled.y = XSENS_DATA_RAWInertial_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
-	imu.accel_unscaled.z = XSENS_DATA_RAWInertial_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
-	imu.gyro_unscaled.p = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-	imu.gyro_unscaled.q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
-	imu.gyro_unscaled.r = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
-        imu.mag_unscaled.x  = XSENS_DATA_RAWInertial_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-	imu.mag_unscaled.y  = XSENS_DATA_RAWInertial_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
-	imu.mag_unscaled.z  = XSENS_DATA_RAWInertial_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
-	ImuScaleGyro(imu);
-	ImuScaleAccel(imu);
-	ImuScaleMag(imu);
-
-	// Copied from booz2_main -- 5143134f060fcc57ce657e17d8b7fc2e72119fd7
-	// mmt 6/15/09
-	if (ahrs.status == AHRS_UNINIT) {
-	  // 150
-	  ahrs_aligner_run();
-	  if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
-	    ahrs_align();
-	}
-	else {
-	  ahrs_propagate();
-	  booz_ins_propagate();
-	}
-      }
-      if (XSENS_MASK_Temp(xsens_output_mode[xsens_id])) {
-        offset += XSENS_DATA_Temp_LENGTH;
-      }
-      if (XSENS_MASK_Calibrated(xsens_output_mode[xsens_id])) {
-        if (XSENS_MASK_AccOut(xsens_output_settings[xsens_id])) {
-          xsens_accel_x[xsens_id] = XSENS_DATA_Calibrated_accX(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_accel_y[xsens_id] = XSENS_DATA_Calibrated_accY(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_accel_z[xsens_id] = XSENS_DATA_Calibrated_accZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-        }
-        if (XSENS_MASK_GyrOut(xsens_output_settings[xsens_id])) {
-          xsens_gyro_x[xsens_id] = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_gyro_y[xsens_id] = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_gyro_z[xsens_id] = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-        }
-        if (XSENS_MASK_MagOut(xsens_output_settings[xsens_id])) {
-          xsens_mag_x[xsens_id] = XSENS_DATA_Calibrated_magX(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_mag_y[xsens_id] = XSENS_DATA_Calibrated_magY(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_mag_z[xsens_id] = XSENS_DATA_Calibrated_magZ(xsens_msg_buf[xsens_id][buf_slot],offset);
-          float pitch = xsens_phi[xsens_id];
-          float roll = -xsens_theta[xsens_id];
-          float tilt_comp_x = xsens_mag_x[xsens_id] * cos(pitch)
-                            + xsens_mag_y[xsens_id] * sin(roll) * sin(pitch)
-                            - xsens_mag_z[xsens_id] * cos(roll) * sin(pitch);
-          float tilt_comp_y = xsens_mag_y[xsens_id] * cos(roll)
-                            + xsens_mag_z[xsens_id] * sin(roll);
-          xsens_mag_heading[xsens_id] = -atan2( tilt_comp_y, tilt_comp_x);
-        }
-        offset += XSENS_DATA_Calibrated_LENGTH;
-      }
-      if (XSENS_MASK_Orientation(xsens_output_mode[xsens_id])) {
-        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x0) {
-          offset += XSENS_DATA_Quaternion_LENGTH;
-        }
-        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x1) {
-          xsens_phi[xsens_id] = XSENS_DATA_Euler_roll(xsens_msg_buf[xsens_id][buf_slot],offset)  * M_PI/180;
-          xsens_theta[xsens_id] =XSENS_DATA_Euler_pitch(xsens_msg_buf[xsens_id][buf_slot],offset) * M_PI/180;
-          xsens_psi[xsens_id] =  XSENS_DATA_Euler_yaw(xsens_msg_buf[xsens_id][buf_slot],offset)   * M_PI/180;
-          offset += XSENS_DATA_Euler_LENGTH;
-        }
-        if (XSENS_MASK_OrientationMode(xsens_output_settings[xsens_id]) == 0x2) {
-          xsens_rmat[xsens_id].m[0] = XSENS_DATA_Matrix_a(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[1] = XSENS_DATA_Matrix_b(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[2] = XSENS_DATA_Matrix_c(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[3] = XSENS_DATA_Matrix_d(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[4] = XSENS_DATA_Matrix_e(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[5] = XSENS_DATA_Matrix_f(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[6] = XSENS_DATA_Matrix_g(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[7] = XSENS_DATA_Matrix_h(xsens_msg_buf[xsens_id][buf_slot],offset);
-          xsens_rmat[xsens_id].m[8] = XSENS_DATA_Matrix_i(xsens_msg_buf[xsens_id][buf_slot],offset);
-
-	  FLOAT_RMAT_COMP_INV(xsens_rmat_adj[xsens_id], xsens_rmat_neutral[xsens_id], xsens_rmat[xsens_id]);
-
-          // Calculate roll, pitch, yaw from rotation matrix ( p 31-33 MTi-G USer Man and Tech Doc)
-          xsens_phi[xsens_id] = -atan2 (xsens_rmat_adj[xsens_id].m[7], xsens_rmat_adj[xsens_id].m[8]);
-          xsens_theta[xsens_id] = asin (xsens_rmat_adj[xsens_id].m[6]);
-          xsens_psi[xsens_id] = atan2 (xsens_rmat_adj[xsens_id].m[3], xsens_rmat_adj[xsens_id].m[0]);
-
-          offset += XSENS_DATA_Matrix_LENGTH;
-        }
-      }
-      if (XSENS_MASK_Status(xsens_output_mode[xsens_id])) {
-        xsens_msg_status[xsens_id] = XSENS_DATA_Status_status(xsens_msg_buf[xsens_id][buf_slot],offset);
-        xsens_mode[xsens_id] = xsens_msg_status[xsens_id];
-        offset += XSENS_DATA_Status_LENGTH;
-      }
-      if (XSENS_MASK_TimeStamp(xsens_output_settings[xsens_id])) {
-        uint16_t ts = XSENS_DATA_TimeStamp_ts(xsens_msg_buf[xsens_id][buf_slot],offset);
-        if (xsens_time_stamp[xsens_id] + 1 != ts)
-          //xsens_err_count[xsens_id]++;
-        xsens_time_stamp[xsens_id] = ts;
-        offset += XSENS_DATA_TimeStamp_LENGTH;
-      }
-  }
-}
-
-
-// Simple state machine parser for XSENS messages (MT Low-Level Comm Doc)
-// Passed serial bytes one per call and parses stream into message id, data length, and data buffer
-// for use at higher level
-void parse_xsens_msg( uint8_t xsens_id, uint8_t c ) {
-  static uint8_t xsens_status[XSENS_COUNT];
-  uint8_t buf_slot = xsens_msg_buf_pi[xsens_id]; // produce index
-
-  // keep track of checksum byte
-  ck[xsens_id] += c;
-  switch (xsens_status[xsens_id]) {
-  case UNINIT:
-    // Look for xsens start byte
-    if (c != XSENS_START)
-      goto error;
-    xsens_status[xsens_id]++;
-    ck[xsens_id] = 0; // Reset checksum
-    break;
-  case GOT_START:
-    // Look for xsens Bus ID
-    if (c != XSENS_BID)
-      goto error;
-    xsens_status[xsens_id]++;
-    break;
-  case GOT_BID:
-    // Save message ID
-    msg_id[xsens_id][buf_slot] = c;
-    xsens_status[xsens_id]++;
-    break;
-  case GOT_MID:
-    // Save message length
-    xsens_len[xsens_id][buf_slot] = c;
-    // check for valid message length
-    if (xsens_len[xsens_id][buf_slot] > XSENS_MAX_PAYLOAD)
-      goto error;
-    xsens_msg_idx[xsens_id] = 0; // Reset buffer index
-    xsens_status[xsens_id]++;
-    break;
-  case GOT_LEN:
-    // Read byte into data buffer
-    xsens_msg_buf[xsens_id][buf_slot][xsens_msg_idx[xsens_id]] = c;
-    xsens_msg_idx[xsens_id]++;
-    // Terminate reading at end of data field
-    if (xsens_msg_idx[xsens_id] >= xsens_len[xsens_id][buf_slot])
-      xsens_status[xsens_id]++;
-    break;
-  case GOT_DATA:
-    // Check for valid checksum
-    if (ck[xsens_id] != 0) {
-      //xsens_err_count[xsens_id]++;
-      goto error;
-    }
-    // Notification for valid message received
-    xsens_msg_received[xsens_id] = TRUE;
-    //xsens_recv_count[xsens_id]++;
-    goto restart;
-    break;
-  }
-  return;
- error:
- restart:
-  // Start over (Reset parser state)
-  xsens_status[xsens_id] = UNINIT;
-  return;
-}
diff --git a/sw/airborne/csc/mercury_xsens.h b/sw/airborne/csc/mercury_xsens.h
deleted file mode 100644
index a2f1857a97..0000000000
--- a/sw/airborne/csc/mercury_xsens.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
- * Copyright (C) 2008 Joby Energy
- *
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file xsens.h
- */
-
-#ifndef __MERCURY_XSENS_H__
-#define __MERCURY_XSENS_H__
-
-#include "types.h"
-
-#define XSENS_COUNT 1
-
-extern uint8_t xsens_mode[XSENS_COUNT]; // Receiver status
-extern volatile uint8_t xsens_msg_received[XSENS_COUNT];
-
-extern float xsens_phi[XSENS_COUNT];
-extern float xsens_theta[XSENS_COUNT];
-extern float xsens_psi[XSENS_COUNT];
-extern uint8_t xsens_msg_status[XSENS_COUNT];
-
-extern float xsens_r_a[XSENS_COUNT];
-extern float xsens_r_b[XSENS_COUNT];
-extern float xsens_r_c[XSENS_COUNT];
-extern float xsens_r_d[XSENS_COUNT];
-extern float xsens_r_e[XSENS_COUNT];
-extern float xsens_r_f[XSENS_COUNT];
-extern float xsens_r_g[XSENS_COUNT];
-extern float xsens_r_h[XSENS_COUNT];
-extern float xsens_r_i[XSENS_COUNT];
-
-extern float xsens_accel_x[XSENS_COUNT];
-extern float xsens_accel_y[XSENS_COUNT];
-extern float xsens_accel_z[XSENS_COUNT];
-extern float xsens_mag_x[XSENS_COUNT];
-extern float xsens_mag_y[XSENS_COUNT];
-extern float xsens_mag_z[XSENS_COUNT];
-extern float xsens_gyro_x[XSENS_COUNT];
-extern float xsens_gyro_y[XSENS_COUNT];
-extern float xsens_gyro_z[XSENS_COUNT];
-extern float xsens_mag_heading[XSENS_COUNT];
-
-extern uint16_t xsens_time_stamp[XSENS_COUNT];
-
-extern int xsens_setzero;
-
-#include "subsystems/ahrs.h"
-
-#define PERIODIC_SEND_IMU_GYRO_SCALED() {			\
-    DOWNLINK_SEND_IMU_GYRO_SCALED(&imu.gyro.p,		\
-			     &imu.gyro.q,		\
-			     &imu.gyro.r);		\
-  }
-
-#define PERIODIC_SEND_IMU_ACCEL_SCALED() {				\
-    DOWNLINK_SEND_IMU_ACCEL_SCALED(&imu.accel.x,		\
-			      &imu.accel.y,		\
-			      &imu.accel.z);		\
-  }
-
-#define PERIODIC_SEND_IMU_MAG_SCALED() {				\
-    DOWNLINK_SEND_IMU_MAG_SCALED(&imu.mag.x,			\
-			    &imu.mag.y,			\
-			    &imu.mag.z);			\
-  }
-
-#define PERIODIC_SEND_BOOZ2_AHRS_EULER() {			\
-    DOWNLINK_SEND_BOOZ2_AHRS_EULER(&ahrs.ltp_to_imu_euler.phi,	\
-				   &ahrs.ltp_to_imu_euler.theta,	\
-				   &ahrs.ltp_to_imu_euler.psi,	\
-				   &ahrs.ltp_to_body_euler.phi,	\
-				   &ahrs.ltp_to_body_euler.theta,	\
-				   &ahrs.ltp_to_body_euler.psi);	\
-  }
-
-
-
-#include "std.h"
-
-void xsens_init(void);
-void xsens_parse_msg(uint8_t xsens_id);
-void xsens_event_task(void);
-void xsens_periodic_task(void);
-
-#endif
diff --git a/sw/airborne/csc/ppm_bridge_main.c b/sw/airborne/csc/ppm_bridge_main.c
deleted file mode 100644
index 95961e9ef8..0000000000
--- a/sw/airborne/csc/ppm_bridge_main.c
+++ /dev/null
@@ -1,131 +0,0 @@
-/*
- * $Id: booz2_main.c 3049 2009-02-24 16:51:25Z poine $
- *
- * Copyright (C) 2008  Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include <inttypes.h>
-
-#include "csc_main.h"
-
-#include "std.h"
-
-#include "mcu.h"
-#include "sys_time.h"
-#include "led.h"
-#include "interrupt_hw.h"
-#include "mcu_periph/uart.h"
-#include "downlink.h"
-#include "generated/periodic.h"
-#include "generated/airframe.h"
-#include "commands.h"
-#include "subsystems/radio_control.h"
-
-#include "csc_telemetry.h"
-#include "led.h"
-
-#include "pprz_transport.h"
-
-#define RC_PROTOCOL_SYNC 13999
-
-extern uint8_t vsupply;
-
-static uint16_t cpu_time = 0;
-
-static void csc_main_init( void ) {
-
-  mcu_init();
-  sys_time_init();
-  led_init();
-
-  Uart0Init();
-  Uart1Init();
-
-  ppm_init();
-
-  // Configure P0.21 as GPIO, output and then pull high as we use it to drive ppm input transistor
-  PINSEL1 = PINSEL1 & ~(0x3 << 10);
-  IO0DIR = IO0DIR | (0x1 << 21);
-  IO0PIN = IO0DIR | (0x1 << 21);
-
-  mcu_int_enable();
-
-}
-
-
-static void csc_main_periodic( void )
-{
-  PeriodicSendAp(DefaultChannel);
-  radio_control_periodic_task();
-
-  cpu_time++;
-}
-
-static void send_short( int16_t s )
-{
-  Uart1Transmit(s >> 8);
-  Uart1Transmit(s & 0xFF);
-}
-
-static void send_channel ( int16_t c )
-{
-  send_short(c);
-  send_short(~c);
-}
-
-static void on_rc_event( void )
-{
-
-#ifdef SWAP_STICKS
-  int temp;
-  temp = rc_values[RADIO_YAW];
-  rc_values[RADIO_YAW] = rc_values[RADIO_ROLL];
-  rc_values[RADIO_ROLL] = temp;
-#endif
-
-  // 7 channels
-  // integers -9600 to +9600
-
-  // sync bytes
-  send_channel(RC_PROTOCOL_SYNC);
-
-  for (int i = 0; i < RADIO_CTL_NB; i++) {
-    send_channel(rc_values[i]);
-  }
-
-  LED_TOGGLE(3);
-}
-
-static void csc_main_event( void )
-{
-  RadioControlEventCheckAndHandle(on_rc_event);
-  DatalinkEvent();
-}
-
-int main( void ) {
-  csc_main_init();
-  while(1) {
-    if (sys_time_periodic())
-      csc_main_periodic();
-    csc_main_event();
-  }
-  return 0;
-}
diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h
deleted file mode 100644
index 26dcab2988..0000000000
--- a/sw/airborne/downlink.h
+++ /dev/null
@@ -1,123 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2003-2006  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file downlink.h
- *  \brief Common code for AP and FBW telemetry
- *
- */
-
-#ifndef DOWNLINK_H
-#define DOWNLINK_H
-
-#include <inttypes.h>
-
-#include "generated/modules.h"
-#include "messages.h"
-#include "generated/airframe.h" // AC_ID is required
-
-#if defined SITL
-
-#ifdef SIM_UART
-#include "sim_uart.h"
-#include "pprz_transport.h"
-#include "xbee.h"
-#else /* SIM_UART */
-/** Software In The Loop simulation uses IVY bus directly as the transport layer */
-#include "ivy_transport.h"
-#endif
-
-#else /** SITL */
-#include "pprz_transport.h"
-#include "modem.h"
-#include "xbee.h"
-#ifdef USE_USB_SERIAL
-#include "mcu_periph/usb_serial.h"
-#endif
-#endif /** !SITL */
-
-#ifndef DefaultChannel
-#define DefaultChannel DOWNLINK_TRANSPORT
-#endif
-
-#ifdef AP
-/** Telemetry mode for AP process: index in the telemetry.xml file */
-extern uint8_t telemetry_mode_Ap_DefaultChannel;
-#endif
-
-#ifdef FBW
-/** Telemetry mode for FBW process: index in the telemetry.xml file */
-extern uint8_t telemetry_mode_Fbw_DefaultChannel;
-#endif
-
-/** Counter of messages not sent because of unavailibity of the output buffer*/
-extern uint8_t downlink_nb_ovrn;
-extern uint16_t downlink_nb_bytes;
-extern uint16_t downlink_nb_msgs;
-
-
-#define __Transport(dev, _x) dev##_x
-#define _Transport(dev, _x) __Transport(dev, _x)
-#define Transport(_chan, _fun) _Transport(_chan, _fun)
-
-
-/** Set of macros for generated code (messages.h) from messages.xml */
-/** 2 = ac_id + msg_id */
-#define DownlinkIDsSize(_chan, _x) (_x+2)
-#define DownlinkSizeOf(_chan, _x) Transport(_chan, SizeOf(DownlinkIDsSize(_chan, _x)))
-
-#define DownlinkCheckFreeSpace(_chan, _x) Transport(_chan, CheckFreeSpace((uint8_t)(_x)))
-
-#define DownlinkPutUint8(_chan, _x) Transport(_chan, PutUint8(_x))
-
-#define DownlinkPutInt8ByAddr(_chan, _x) Transport(_chan, PutInt8ByAddr(_x))
-#define DownlinkPutUint8ByAddr(_chan, _x) Transport(_chan, PutUint8ByAddr(_x))
-#define DownlinkPutInt16ByAddr(_chan, _x) Transport(_chan, PutInt16ByAddr(_x))
-#define DownlinkPutUint16ByAddr(_chan, _x) Transport(_chan, PutUint16ByAddr(_x))
-#define DownlinkPutInt32ByAddr(_chan, _x) Transport(_chan, PutInt32ByAddr(_x))
-#define DownlinkPutUint32ByAddr(_chan, _x) Transport(_chan, PutUint32ByAddr(_x))
-#define DownlinkPutFloatByAddr(_chan, _x) Transport(_chan, PutFloatByAddr(_x))
-
-#define DownlinkPutDoubleByAddr(_chan, _x) Transport(_chan, PutDoubleByAddr(_x))
-
-#define DownlinkPutFloatArray(_chan, _n, _x) Transport(_chan, PutFloatArray(_n, _x))
-#define DownlinkPutDoubleArray(_chan, _n, _x) Transport(_chan, PutDoubleArray(_n, _x))
-#define DownlinkPutInt16Array(_chan, _n, _x) Transport(_chan, PutInt16Array(_n, _x))
-#define DownlinkPutUint16Array(_chan, _n, _x) Transport(_chan, PutUint16Array(_n, _x))
-#define DownlinkPutInt32Array(_chan, _n, _x) Transport(_chan, PutInt32Array(_n, _x))
-#define DownlinkPutUint32Array(_chan, _n, _x) Transport(_chan, PutUint32Array(_n, _x))
-#define DownlinkPutUint8Array(_chan, _n, _x) Transport(_chan, PutUint8Array(_n, _x))
-
-#define DownlinkOverrun(_chan) downlink_nb_ovrn++;
-#define DownlinkCountBytes(_chan, _n) downlink_nb_bytes += _n;
-
-#define DownlinkStartMessage(_chan, _name, msg_id, payload_len) { \
-  downlink_nb_msgs++; \
-  Transport(_chan, Header(DownlinkIDsSize(_chan, payload_len))); \
-  Transport(_chan, PutUint8(AC_ID)); \
-  Transport(_chan, PutNamedUint8(_name, msg_id)); \
-}
-
-#define DownlinkEndMessage(_chan) Transport(_chan, Trailer())
-
-#endif /* DOWNLINK_H */
diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c
index be4df02b68..98b1b7b120 100644
--- a/sw/airborne/estimator.c
+++ b/sw/airborne/estimator.c
@@ -99,7 +99,7 @@ void estimator_init( void ) {
 
   EstimatorSetRate(0., 0., 0.);
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
   EstimatorSetAirspeed( 0. );
 #endif
 
@@ -152,13 +152,13 @@ void alt_kalman(float gps_z) {
   float R;
   float SIGMA2;
 
-#ifdef USE_BARO_MS5534A
+#if USE_BARO_MS5534A
   if (alt_baro_enabled) {
     DT = BARO_DT;
     R = baro_MS5534A_r;
     SIGMA2 = baro_MS5534A_sigma2;
   } else
-#elif defined(USE_BARO_ETS)
+#elif USE_BARO_ETS
   if (baro_ets_enabled) {
     DT = BARO_ETS_DT;
     R = baro_ets_r;
@@ -204,7 +204,7 @@ void alt_kalman(float gps_z) {
   }
 
 #ifdef DEBUG_ALT_KALMAN
-  DOWNLINK_SEND_ALT_KALMAN(&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
+  DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
 #endif
 }
 
@@ -230,8 +230,5 @@ void estimator_update_state_gps( void ) {
 
   // Heading estimation now in ahrs_infrared
 
-#ifdef EXTRA_DOWNLINK_DEVICE
-    DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
-#endif
 }
 
diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h
index 358f416ee4..8748d43354 100644
--- a/sw/airborne/estimator.h
+++ b/sw/airborne/estimator.h
@@ -36,7 +36,7 @@
 #include "baro_MS5534A.h"
 #endif
 
-#ifdef USE_BARO_ETS
+#if USE_BARO_ETS
 #include "modules/sensors/baro_ets.h"
 #endif
 
@@ -94,10 +94,10 @@ extern void alt_kalman( float );
 #ifdef ALT_KALMAN
 #define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; }
 
-#if defined(USE_BARO_MS5534A) || defined(USE_BARO_ETS)
+#if USE_BARO_MS5534A || USE_BARO_ETS
 /* Kalman filter cannot be disabled in this mode (no z_dot) */
 #define EstimatorSetAlt(z) alt_kalman(z)
-#else /* USE_BARO_MS5534A */
+#else /* USE_BARO_x */
 #define EstimatorSetAlt(z) { \
   if (!alt_kalman_enabled) { \
     estimator_z = z; \
@@ -105,7 +105,7 @@ extern void alt_kalman( float );
     alt_kalman(z); \
   } \
 }
-#endif /* ! USE_BARO_MS5534A */
+#endif /* ! USE_BARO_x */
 
 #define EstimatorSetSpeedPol(vhmod, vhdir, vz) { \
   estimator_hspeed_mod = vhmod; \
diff --git a/sw/airborne/firmwares/beth/gps_c.patch b/sw/airborne/firmwares/beth/gps_c.patch
index 8fcbf9d486..dcf8f7f7c0 100644
--- a/sw/airborne/firmwares/beth/gps_c.patch
+++ b/sw/airborne/firmwares/beth/gps_c.patch
@@ -21,7 +21,7 @@
 +#define DefaultChannel gcs_com.udp_transport
 +#else
  #include "messages.h"
- #include "downlink.h"
+ #include "subsystems/datalink/downlink.h"
 +#endif
  
  uint16_t last_gps_msg_t;	/** cputime of the last gps message */
diff --git a/sw/airborne/firmwares/beth/main_beth.c b/sw/airborne/firmwares/beth/main_beth.c
index ff7ee9a0b9..e1964ba8b4 100644
--- a/sw/airborne/firmwares/beth/main_beth.c
+++ b/sw/airborne/firmwares/beth/main_beth.c
@@ -3,7 +3,7 @@
 #include "sys_time.h"
 #include "interrupt_hw.h"
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "beth/bench_sensors.h"
 
diff --git a/sw/airborne/firmwares/beth/main_coders.c b/sw/airborne/firmwares/beth/main_coders.c
index c0ea74c6fd..2e8d970b5d 100644
--- a/sw/airborne/firmwares/beth/main_coders.c
+++ b/sw/airborne/firmwares/beth/main_coders.c
@@ -2,7 +2,7 @@
 #include "mcu.h"
 #include "mcu_periph/can.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include <stm32/rcc.h>
 #include <stm32/gpio.h>
diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c
index 61978e26a0..551d43477e 100644
--- a/sw/airborne/firmwares/beth/main_stm32.c
+++ b/sw/airborne/firmwares/beth/main_stm32.c
@@ -27,7 +27,7 @@
 #include "mcu.h"
 #include "mcu_periph/can.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "firmwares/rotorcraft/commands.h"
 #include "firmwares/rotorcraft/actuators.h"
 //#include "booz/booz_radio_control.h"
diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c
index 60d871d922..388751fd55 100644
--- a/sw/airborne/firmwares/beth/overo_test_uart.c
+++ b/sw/airborne/firmwares/beth/overo_test_uart.c
@@ -43,7 +43,7 @@
 
 #include "overo_gcs_com.h"
 #include "uart_hw.h"
-#include "pprz_transport.h"
+#include "subsystems/datalink/pprz_transport.h"
 
 
 struct OveroController {
@@ -146,7 +146,7 @@ static void main_periodic(int my_sig_num) {
 
 }
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #if 0
 uint8_t downlink_nb_ovrn;
 uint16_t downlink_nb_bytes;
diff --git a/sw/airborne/firmwares/beth/overo_twist_controller.c b/sw/airborne/firmwares/beth/overo_twist_controller.c
index cd588f8d2c..845c5177a4 100644
--- a/sw/airborne/firmwares/beth/overo_twist_controller.c
+++ b/sw/airborne/firmwares/beth/overo_twist_controller.c
@@ -113,7 +113,7 @@ void control_run(void) {
   controller.elevation_ddot_ref = -2*controller.omega_elevation_ref*controller.xi_ref*controller.elevation_dot_ref
     - controller.omega_elevation_ref*controller.omega_elevation_ref*(controller.elevation_ref - controller.elevation_sp);
 
-#ifdef USE_AZIMUTH
+#if USE_AZIMUTH
   controller.azimuth_ref = controller.azimuth_ref + controller.azimuth_dot_ref * dt_ctl;
   controller.azimuth_dot_ref = controller.azimuth_dot_ref + controller.azimuth_ddot_ref * dt_ctl;
   controller.azimuth_ddot_ref = -2*controller.omega_azimuth_ref*controller.xi_ref*controller.azimuth_dot_ref
@@ -131,7 +131,7 @@ void control_run(void) {
   const float err_elevation = estimator.elevation - controller.elevation_ref;
   const float err_elevation_dot = estimator.elevation_dot - controller.elevation_dot_ref;
 
-#ifdef USE_AZIMUTH
+#if USE_AZIMUTH
   const float err_azimuth = estimator.azimuth - controller.azimuth_ref;
   const float err_azimuth_dot = estimator.azimuth_dot - controller.azimuth_dot_ref;
 #endif
@@ -151,7 +151,7 @@ void control_run(void) {
   controller.cmd_thrust_fb = -controller.mass * (2 * controller.xi_cl * controller.omega_cl * err_elevation_dot) -
   			controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation);
 
-#ifdef USE_AZIMUTH
+#if USE_AZIMUTH
   controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref;
   controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_azimuth_dot) +
                         controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth);
@@ -159,7 +159,7 @@ void control_run(void) {
 
   controller.cmd_pitch =  /*controller.cmd_pitch_ff*/ + controller.cmd_pitch_fb;
 
-#ifdef USE_AZIMUTH
+#if USE_AZIMUTH
   controller.tilt_sp = controller.azim_gain * (-controller.cmd_azimuth_fb );
 #endif
 
diff --git a/sw/airborne/firmwares/beth/rcv_telemetry.c b/sw/airborne/firmwares/beth/rcv_telemetry.c
index 7248878cba..e8a3c31350 100755
--- a/sw/airborne/firmwares/beth/rcv_telemetry.c
+++ b/sw/airborne/firmwares/beth/rcv_telemetry.c
@@ -31,7 +31,7 @@
 
 #include <inttypes.h>
 #include <string.h>
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 
 #include "generated/modules.h"
 
@@ -44,7 +44,7 @@
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 #include "mcu_periph/uart.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 
 
diff --git a/sw/airborne/csc/csc_telemetry.c b/sw/airborne/firmwares/fixedwing/ap_downlink.c
similarity index 72%
rename from sw/airborne/csc/csc_telemetry.c
rename to sw/airborne/firmwares/fixedwing/ap_downlink.c
index 70dc6a7166..2ca6a53dd5 100644
--- a/sw/airborne/csc/csc_telemetry.c
+++ b/sw/airborne/firmwares/fixedwing/ap_downlink.c
@@ -1,7 +1,7 @@
 /*
- * $Id: booz2_telemetry.c 3002 2009-02-10 11:36:07Z poine $
+ * Paparazzi $Id$
  *
- * Copyright (C) 2008  Antoine Drouin
+ * Copyright (C) 2011 Gautier Hattenberger
  *
  * This file is part of paparazzi.
  *
@@ -22,7 +22,14 @@
  *
  */
 
-#include "csc_telemetry.h"
+#include "firmwares/fixedwing/ap_downlink.h"
+#include "generated/airframe.h"
+
+#ifdef AP
+#ifndef TELEMETRY_MODE_AP
+#define TELEMETRY_MODE_AP 0
+#endif
+uint8_t telemetry_mode_Ap_DefaultChannel = TELEMETRY_MODE_AP;
+#endif /** AP */
 
 
-uint8_t telemetry_mode_Ap_DefaultChannel;
diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h
new file mode 100644
index 0000000000..d1b25251ed
--- /dev/null
+++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h
@@ -0,0 +1,267 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2006- Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file ap_downlink.h
+ *  \brief Set of macros defining the periodic telemetry messages of AP process
+ *
+ * The PeriodicSendAp() macro is generated from the telemetry description
+ * (named in conf.xml, usually in conf/telemetry directory). This macro
+ * is a sequence of calls to PERIODIC_SEND_message() which have to be defined
+ * in the present file.
+ *
+ */
+
+#ifndef AP_DOWNLINK_H
+#define AP_DOWNLINK_H
+
+#include <inttypes.h>
+
+#include "generated/airframe.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "subsystems/datalink/downlink.h"
+
+#include "messages.h"
+#include "generated/periodic.h"
+
+//#include "generated/modules.h"
+
+#if defined DOWNLINK
+#define Downlink(x) x
+#else
+#define Downlink(x) {}
+#endif
+
+#ifdef AP
+/** Telemetry mode for AP process: index in the telemetry.xml file */
+extern uint8_t telemetry_mode_Ap_DefaultChannel;
+#endif
+
+#define PERIODIC_SEND_ALIVE(_trans, _dev)  DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM);
+
+#define PERIODIC_SEND_BAT(_trans, _dev) {                      \
+    int16_t amps = (int16_t) (current/10);				\
+    Downlink({ int16_t e = energy;                      \
+        DOWNLINK_SEND_BAT(_trans, _dev,                        \
+                          &v_ctl_throttle_slewed,       \
+                          &vsupply,                     \
+                          &amps,                        \
+                          &estimator_flight_time,       \
+                          &kill_throttle,				\
+                          &block_time,					\
+                          &stage_time,					\
+                          &e);                          \
+      });                                               \
+  }
+
+#ifdef MCU_SPI_LINK
+#define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) DOWNLINK_SEND_DEBUG_MCU_LINK(_trans, _dev, &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt);
+#else
+#define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) {}
+#endif
+
+
+#define PERIODIC_SEND_DOWNLINK(_trans, _dev) { \
+  static uint16_t last; \
+  uint16_t rate = (downlink_nb_bytes - last) / PERIOD_DOWNLINK_Ap_DefaultChannel_0; \
+  last = downlink_nb_bytes; \
+  DOWNLINK_SEND_DOWNLINK(_trans, _dev, &downlink_nb_ovrn, &rate, &downlink_nb_msgs); \
+}
+
+
+#define PERIODIC_SEND_ATTITUDE(_trans, _dev) Downlink({ \
+      DOWNLINK_SEND_ATTITUDE(_trans, _dev, &estimator_phi, &estimator_psi, &estimator_theta); \
+})
+
+#define PERIODIC_SEND_PPRZ_MODE(_trans, _dev) DOWNLINK_SEND_PPRZ_MODE(_trans, _dev, &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status);
+#define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint);
+
+#if USE_INFRARED
+#define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &contrast); }
+#elif USE_IMU && USE_AHRS
+#define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint8_t mde = 3; if (ahrs.status == AHRS_UNINIT) mde = 2; if (ahrs_timeout_counter > 10) mde = 5; uint16_t val = 0; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &val); }
+#else
+#define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) {}
+#endif
+
+#define PERIODIC_SEND_NAVIGATION_REF(_trans, _dev)  DOWNLINK_SEND_NAVIGATION_REF(_trans, _dev, &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0);
+
+
+#define DownlinkSendWp(_trans, _dev, i) {	     \
+  float x = nav_utm_east0 +  waypoints[i].x; \
+  float y = nav_utm_north0 + waypoints[i].y; \
+  DOWNLINK_SEND_WP_MOVED(_trans, _dev, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \
+}
+
+
+#define PERIODIC_SEND_WP_MOVED(_trans, _dev) { \
+  static uint8_t i; \
+  i++; if (i >= nb_waypoint) i = 0; \
+  DownlinkSendWp(_trans, _dev, i);	    \
+}
+
+#ifdef RADIO_CONTROL_SETTINGS
+#define PERIODIC_SEND_SETTINGS(_trans, _dev) if (!RcSettingsOff()) DOWNLINK_SEND_SETTINGS(_trans, _dev, &slider_1_val, &slider_2_val);
+#else
+#define PERIODIC_SEND_SETTINGS(_trans, _dev) {}
+#endif
+
+#if USE_INFRARED || USE_INFRARED_TELEMETRY
+#include "subsystems/sensors/infrared.h"
+#define PERIODIC_SEND_IR_SENSORS(_trans, _dev) DOWNLINK_SEND_IR_SENSORS(_trans, _dev, &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
+#else
+#define PERIODIC_SEND_IR_SENSORS(_trans, _dev) ;
+#endif
+
+#define PERIODIC_SEND_ADC(_trans, _dev) {}
+
+
+#define PERIODIC_SEND_CALIBRATION(_trans, _dev) DOWNLINK_SEND_CALIBRATION(_trans, _dev, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode)
+
+#define PERIODIC_SEND_CIRCLE(_trans, _dev) if (nav_in_circle) { DOWNLINK_SEND_CIRCLE(_trans, _dev, &nav_circle_x, &nav_circle_y, &nav_circle_radius); }
+
+#define PERIODIC_SEND_SEGMENT(_trans, _dev) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_trans, _dev, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
+
+#ifdef IMU_TYPE_H
+#  ifdef INS_MODULE_H
+#  include "modules/ins/ins_module.h"
+#    define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {}
+#    define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {}
+#    define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {}
+#    define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &ins_p, &ins_q, &ins_r)}
+#    define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &ins_ax, &ins_ay, &ins_az)}
+#    define PERIODIC_SEND_IMU_MAG(_trans, _dev) { DOWNLINK_SEND_IMU_MAG(_trans, _dev, &ins_mx, &ins_my, &ins_mz)}
+#  else
+#    include "subsystems/imu.h"
+#    define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_ACCEL_RAW(_trans, _dev, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
+#    define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_GYRO_RAW(_trans, _dev, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
+#    define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) { DOWNLINK_SEND_IMU_MAG_RAW(_trans, _dev, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z)}
+#    define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); DOWNLINK_SEND_IMU_ACCEL(_trans, _dev, &accel_float.x, &accel_float.y, &accel_float.z)}
+#    define PERIODIC_SEND_IMU_GYRO(_trans, _dev) { struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); DOWNLINK_SEND_IMU_GYRO(_trans, _dev, &gyro_float.p, &gyro_float.q, &gyro_float.r)}
+#    ifdef USE_MAGNETOMETER
+#      define PERIODIC_SEND_IMU_MAG(_trans, _dev) { struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); DOWNLINK_SEND_IMU_MAG(_trans, _dev, &mag_float.x, &mag_float.y, &mag_float.z)}
+#    else
+#      define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
+#    endif
+#  endif
+#else
+#    define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {}
+#    define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {}
+#    define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {}
+#    define PERIODIC_SEND_IMU_ACCEL(_trans, _dev) {}
+#    define PERIODIC_SEND_IMU_GYRO(_trans, _dev) {}
+#    define PERIODIC_SEND_IMU_MAG(_trans, _dev) {}
+#endif
+
+#ifdef IMU_ANALOG
+#define PERIODIC_SEND_IMU(_trans, _dev) { int16_t dummy = 42; DOWNLINK_SEND_IMU(_trans, _dev, &(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); }
+#else
+#define PERIODIC_SEND_IMU(_trans, _dev) {}
+#endif
+
+#define PERIODIC_SEND_ESTIMATOR(_trans, _dev) DOWNLINK_SEND_ESTIMATOR(_trans, _dev, &estimator_z, &estimator_z_dot)
+
+#define SEND_NAVIGATION(_trans, _dev) Downlink({ uint8_t _circle_count = NavCircleCount(); DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &estimator_x, &estimator_y, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count);})
+
+#define PERIODIC_SEND_NAVIGATION(_trans, _dev) SEND_NAVIGATION(_trans, _dev)
+
+
+#if defined CAM || defined MOBILE_CAM
+#define SEND_CAM(_trans, _dev) Downlink({ int16_t x = cam_target_x; int16_t y = cam_target_y; int16_t phi = DegOfRad(cam_phi_c); int16_t theta = DegOfRad(cam_theta_c); DOWNLINK_SEND_CAM(_trans, _dev, &phi, &theta, &x, &y);})
+#define PERIODIC_SEND_CAM_POINT(_trans, _dev) DOWNLINK_SEND_CAM_POINT(_trans, _dev, &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon)
+#else
+#define SEND_CAM(_trans, _dev) {}
+#define PERIODIC_SEND_CAM_POINT(_trans, _dev) {}
+#endif
+
+#define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev) /** generated from the xml settings config in conf/settings */
+
+#define PERIODIC_SEND_SURVEY(_trans, _dev) { \
+  if (nav_survey_active) \
+    DOWNLINK_SEND_SURVEY(_trans, _dev, &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); \
+  }
+
+#define PERIODIC_SEND_RANGEFINDER(_trans, _dev) DOWNLINK_SEND_RANGEFINDER(_trans, _dev, &rangemeter, &ctl_grz_z_dot, &ctl_grz_z_dot_sum_err, &ctl_grz_z_dot_setpoint, &ctl_grz_z_sum_err, &ctl_grz_z_setpoint, &flying)
+
+#define PERIODIC_SEND_TUNE_ROLL(_trans, _dev) DOWNLINK_SEND_TUNE_ROLL(_trans, _dev, &estimator_p,&estimator_phi, &h_ctl_roll_setpoint);
+
+#if USE_GPS || USE_GPS_XSENS || defined SITL
+#define PERIODIC_SEND_GPS_SOL(_trans, _dev) DOWNLINK_SEND_GPS_SOL(_trans, _dev, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv)
+#endif
+
+#define PERIODIC_SEND_GPS(_trans, _dev) {                                      \
+    static uint8_t i;                                                   \
+    int16_t climb = -gps.ned_vel.z;                                     \
+    int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));             \
+    DOWNLINK_SEND_GPS(_trans, _dev, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.hmsl, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \
+    if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0;                                    \
+    if (i >= gps.nb_channels * 2) i = 0;                                    \
+    if (i < gps.nb_channels && gps.svinfos[i].cno > 0) { \
+      DOWNLINK_SEND_SVINFO(_trans, _dev, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
+    }                                                                   \
+    i++;                                                                \
+}
+
+#if USE_BARO_MS5534A
+//#include "baro_MS5534A.h"
+#define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) DOWNLINK_SEND_BARO_MS5534A(_trans, _dev, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z)
+#else
+#define PERIODIC_SEND_BARO_MS5534A(_trans, _dev) {}
+#endif
+
+#if USE_BARO_SCP
+#include "baro_scp.h"
+#define PERIODIC_SEND_SCP_STATUS(_trans, _dev) DOWNLINK_SEND_SCP_STATUS(_trans, _dev, &baro_scp_pressure, &baro_scp_temperature)
+#else
+#define PERIODIC_SEND_SCP_STATUS(_trans, _dev) {}
+#endif
+
+#ifdef BOARD_HAS_BARO
+#define PERIODIC_SEND_BARO_RAW(_trans, _dev) {  \
+    DOWNLINK_SEND_BARO_RAW(_trans, _dev,        \
+                           &baro.absolute,      \
+                           &baro.differential); \
+  }
+#else
+#define PERIODIC_SEND_BARO_RAW(_trans, _dev) {}
+#endif
+
+#ifdef MEASURE_AIRSPEED
+#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed, &estimator_airspeed)
+#elif USE_AIRSPEED
+#define PERIODIC_SEND_AIRSPEED(_trans, _dev) DOWNLINK_SEND_AIRSPEED (_trans, _dev, &estimator_airspeed, &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint)
+#else
+#define PERIODIC_SEND_AIRSPEED(_trans, _dev) {}
+#endif
+
+#define PERIODIC_SEND_ENERGY(_trans, _dev) Downlink({ const int16_t e = energy; const float vsup = ((float)vsupply) / 10.0f; const float curs = ((float) current)/1000.0f;  const float power = vsup * curs; DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &curs, &e, &power); })
+
+
+#include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
+#define PERIODIC_SEND_H_CTL_A(_trans, _dev) DOWNLINK_SEND_H_CTL_A(_trans, _dev, &h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, &h_ctl_ref_pitch_angle)
+
+
+#endif /* AP_DOWNLINK_H */
diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c
index 106156e564..553bb828df 100644
--- a/sw/airborne/firmwares/fixedwing/datalink.c
+++ b/sw/airborne/firmwares/fixedwing/datalink.c
@@ -31,7 +31,7 @@
 
 #include <inttypes.h>
 #include <string.h>
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 
 #include "generated/modules.h"
 
@@ -44,7 +44,7 @@
 #include "subsystems/nav.h"
 #endif
 
-#ifdef USE_JOYSTICK
+#if USE_JOYSTICK
 #include "joystick.h"
 #endif
 
@@ -61,7 +61,7 @@
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 #include "mcu_periph/uart.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "ap_downlink.h"
 
 #define MOfCm(_x) (((float)(_x))/100.)
@@ -93,7 +93,7 @@ void dl_parse_msg(void) {
 #endif
 
   if (msg_id == DL_PING) {
-    DOWNLINK_SEND_PONG(DefaultChannel);
+    DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice)
   } else
 #ifdef TRAFFIC_INFO
   if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) {
@@ -126,21 +126,21 @@ void dl_parse_msg(void) {
        coordinates */
     utm.east = waypoints[wp_id].x + nav_utm_east0;
     utm.north = waypoints[wp_id].y + nav_utm_north0;
-    DOWNLINK_SEND_WP_MOVED(DefaultChannel, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
+    DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0);
   } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) {
     nav_goto_block(DL_BLOCK_block_id(dl_buffer));
-    SEND_NAVIGATION(DefaultChannel);
+    SEND_NAVIGATION(DefaultChannel, DefaultDevice);
   } else
 #endif /** NAV */
 #ifdef WIND_INFO
   if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
     wind_east = DL_WIND_INFO_east(dl_buffer);
     wind_north = DL_WIND_INFO_north(dl_buffer);
-#ifndef USE_AIRSPEED
+#if !USE_AIRSPEED
     estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
 #endif
 #ifdef WIND_INFO_RET
-    DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, &wind_east, &wind_north, &estimator_airspeed);
+    DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind_east, &wind_north, &estimator_airspeed);
 #endif
   } else
 #endif /** WIND_INFO */
@@ -171,14 +171,14 @@ void dl_parse_msg(void) {
     uint8_t i = DL_SETTING_index(dl_buffer);
     float val = DL_SETTING_value(dl_buffer);
     DlSetting(i, val);
-    DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
+    DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
   } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
     uint8_t i = DL_GET_SETTING_index(dl_buffer);
     float val = settings_get_value(i);
-    DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
+    DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
   } else
 #endif /** Else there is no dl_settings section in the flight plan */
-#ifdef USE_JOYSTICK
+#if USE_JOYSTICK
     if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
       JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
 			    DL_JOYSTICK_RAW_pitch(dl_buffer),
@@ -187,14 +187,18 @@ void dl_parse_msg(void) {
 #endif // USE_JOYSTICK
 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
     if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
-LED_TOGGLE(3);
+#ifdef RADIO_CONTROL_DATALINK_LED
+      LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
+#endif
       parse_rc_3ch_datalink(
           DL_RC_3CH_throttle_mode(dl_buffer),
           DL_RC_3CH_roll(dl_buffer),
           DL_RC_3CH_pitch(dl_buffer));
     } else
     if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
-LED_TOGGLE(3);
+#ifdef RADIO_CONTROL_DATALINK_LED
+      LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
+#endif
       parse_rc_4ch_datalink(
           DL_RC_4CH_mode(dl_buffer),
           DL_RC_4CH_throttle(dl_buffer),
diff --git a/sw/airborne/modules/deploy_sonar_buoy.h b/sw/airborne/firmwares/fixedwing/fbw_downlink.c
similarity index 72%
rename from sw/airborne/modules/deploy_sonar_buoy.h
rename to sw/airborne/firmwares/fixedwing/fbw_downlink.c
index 09030ab6ee..46ba54f928 100644
--- a/sw/airborne/modules/deploy_sonar_buoy.h
+++ b/sw/airborne/firmwares/fixedwing/fbw_downlink.c
@@ -1,7 +1,7 @@
 /*
- * $Id: $
+ * Paparazzi $Id$
  *
- * Copyright (C) 2010 Eric Parsonage
+ * Copyright (C) 2011 Gautier Hattenberger
  *
  * This file is part of paparazzi.
  *
@@ -19,17 +19,17 @@
  * along with paparazzi; see the file COPYING.  If not, write to
  * the Free Software Foundation, 59 Temple Place - Suite 330,
  * Boston, MA 02111-1307, USA.
+ *
  */
 
+#include "firmwares/fixedwing/fbw_downlink.h"
+#include "generated/airframe.h"
 
-#ifndef DEPLOY_SONAR_BUOY_H
-#define DEPLOY_SONAR_BUOY_H
+#ifdef FBW
+#ifndef TELEMETRY_MODE_FBW
+#define TELEMETRY_MODE_FBW 0
+#endif
+uint8_t telemetry_mode_Fbw_DefaultChannel = TELEMETRY_MODE_FBW;
+#endif /** AP */
 
-#include "std.h"
 
-extern bool_t buoy_1;
-extern bool_t buoy_2;
-extern void deploy_sonar_buoy_init(void);
-extern void deploy_sonar_buoy_periodic(void);
-
-#endif  /* DEPLOY_SONAR_BUOY_H */
diff --git a/sw/airborne/fbw_downlink.h b/sw/airborne/firmwares/fixedwing/fbw_downlink.h
similarity index 57%
rename from sw/airborne/fbw_downlink.h
rename to sw/airborne/firmwares/fixedwing/fbw_downlink.h
index 61fe9228bb..7dc51984d7 100644
--- a/sw/airborne/fbw_downlink.h
+++ b/sw/airborne/firmwares/fixedwing/fbw_downlink.h
@@ -48,44 +48,51 @@
 #include "subsystems/electrical.h"
 #include "inter_mcu.h"
 
+#ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_FBW_DEVICE
-#include "downlink.h"
+#endif
+#include "subsystems/datalink/downlink.h"
 
-#define PERIODIC_SEND_COMMANDS(_chan) DOWNLINK_SEND_COMMANDS(_chan, COMMANDS_NB, commands)
+#ifdef FBW
+/** Telemetry mode for FBW process: index in the telemetry.xml file */
+extern uint8_t telemetry_mode_Fbw_DefaultChannel;
+#endif
+
+#define PERIODIC_SEND_COMMANDS(_trans, _dev) DOWNLINK_SEND_COMMANDS(_trans, _dev, COMMANDS_NB, commands)
 
 #ifdef RADIO_CONTROL
-#define PERIODIC_SEND_FBW_STATUS(_chan) DOWNLINK_SEND_FBW_STATUS(_chan, &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current)
+#define PERIODIC_SEND_FBW_STATUS(_trans, _dev) DOWNLINK_SEND_FBW_STATUS(_trans, _dev, &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current)
 #ifdef RADIO_CONTROL_TYPE_PPM
-#define PERIODIC_SEND_PPM(_chan) DOWNLINK_SEND_PPM(_chan, &(radio_control.frame_rate), PPM_NB_CHANNEL, ppm_pulses)
+#define PERIODIC_SEND_PPM(_trans, _dev) DOWNLINK_SEND_PPM(_trans, _dev, &(radio_control.frame_rate), PPM_NB_CHANNEL, ppm_pulses)
 #else
-#define PERIODIC_SEND_PPM(_chan) {}
+#define PERIODIC_SEND_PPM(_trans, _dev) {}
 #endif
-#define PERIODIC_SEND_RC(_chan) DOWNLINK_SEND_RC(_chan, RADIO_CONTROL_NB_CHANNEL, radio_control.values)
+#define PERIODIC_SEND_RC(_trans, _dev) DOWNLINK_SEND_RC(_trans, _dev, RADIO_CONTROL_NB_CHANNEL, radio_control.values)
 #else // RADIO_CONTROL
-#define PERIODIC_SEND_FBW_STATUS(_chan) { uint8_t dummy = 0; DOWNLINK_SEND_FBW_STATUS(_chan, &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); }
-#define PERIODIC_SEND_PPM(_chan) {}
-#define PERIODIC_SEND_RC(_chan) {}
+#define PERIODIC_SEND_FBW_STATUS(_trans, _dev) { uint8_t dummy = 0; DOWNLINK_SEND_FBW_STATUS(_trans, _dev, &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); }
+#define PERIODIC_SEND_PPM(_trans, _dev) {}
+#define PERIODIC_SEND_RC(_trans, _dev) {}
 #endif // RADIO_CONTROL
 
 #ifdef ACTUATORS
-#define PERIODIC_SEND_ACTUATORS(_chan) DOWNLINK_SEND_ACTUATORS(_chan, SERVOS_NB, actuators)
+#define PERIODIC_SEND_ACTUATORS(_trans, _dev) DOWNLINK_SEND_ACTUATORS(_trans, _dev, SERVOS_NB, actuators)
 #else
-#define PERIODIC_SEND_ACTUATORS(_chan) {}
+#define PERIODIC_SEND_ACTUATORS(_trans, _dev) {}
 #endif
 
 #ifdef BRICOLAGE_ADC
 extern uint16_t adc0_val[];
 
-#define PERIODIC_SEND_ADC(_chan) {			\
+#define PERIODIC_SEND_ADC(_trans, _dev) {			\
     static const uint8_t mcu = 0;			\
-    DOWNLINK_SEND_ADC(_chan, &mcu, 8, adc0_val);	\
+    DOWNLINK_SEND_ADC(_trans, _dev, &mcu, 8, adc0_val);	\
   }
 #else
-#define PERIODIC_SEND_ADC(_chan) {}
+#define PERIODIC_SEND_ADC(_trans, _dev) {}
 #endif
 
 static inline void fbw_downlink_periodic_task(void) {
-  PeriodicSendFbw(DefaultChannel)
+  PeriodicSendFbw(DefaultChannel,DefaultDevice)
 }
 
 
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
index 6ddd26a05d..cd7fecb7c0 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
@@ -92,7 +92,7 @@ inline static void v_ctl_climb_auto_throttle_loop( void );
 inline static void v_ctl_climb_auto_pitch_loop( void );
 #endif
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
 float v_ctl_auto_airspeed_setpoint;
 float v_ctl_auto_airspeed_controlled;
 float v_ctl_auto_airspeed_pgain;
@@ -142,7 +142,7 @@ void v_ctl_init( void ) {
   v_ctl_auto_pitch_sum_err = 0.;
 #endif
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
   v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
   v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
   v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN;
@@ -187,7 +187,7 @@ void v_ctl_init( void ) {
 void v_ctl_altitude_loop( void ) {
   float altitude_pgain_boost = 1.0;
 
-#if defined(USE_AIRSPEED) && defined(AGR_CLIMB)
+#if USE_AIRSPEED && defined(AGR_CLIMB)
   // Aggressive climb mode (boost gain of altitude loop)
   if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
     float dist = fabs(v_ctl_altitude_error);
@@ -236,7 +236,7 @@ void v_ctl_climb_loop ( void ) {
  * \brief
  */
 
-#ifndef USE_AIRSPEED
+#if !USE_AIRSPEED
 
 inline static void v_ctl_climb_auto_throttle_loop(void) {
   static float last_err;
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h
index dfcc49be94..66eeb82b45 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h
@@ -70,11 +70,6 @@ extern float v_ctl_auto_throttle_sum_err;
 extern float v_ctl_auto_throttle_pitch_of_vz_pgain;
 extern float v_ctl_auto_throttle_pitch_of_vz_dgain;
 
-#ifdef LOITER_TRIM
-extern float v_ctl_auto_throttle_loiter_trim;
-extern float v_ctl_auto_throttle_dash_trim;
-#endif
-
 /* agressive tuning */
 #ifdef TUNE_AGRESSIVE_CLIMB
 extern float agr_climb_throttle;
@@ -97,7 +92,7 @@ extern void v_ctl_init( void );
 extern void v_ctl_altitude_loop( void );
 extern void v_ctl_climb_loop ( void );
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
 /* "airspeed" inner loop parameters */
 extern float v_ctl_auto_airspeed_setpoint;
 extern float v_ctl_auto_airspeed_controlled;
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
index 0c94f3d8c5..ce55b59f14 100644
--- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -92,7 +92,7 @@ pprz_t v_ctl_throttle_slewed;
 
 uint8_t v_ctl_speed_mode;
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
 float v_ctl_auto_airspeed_setpoint;
 float v_ctl_auto_airspeed_controlled;
 float v_ctl_auto_airspeed_throttle_pgain;
@@ -146,7 +146,7 @@ void v_ctl_init( void ) {
   v_ctl_auto_pitch_igain = ABS(V_CTL_AUTO_PITCH_IGAIN);
   v_ctl_auto_pitch_sum_err = 0.;
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
   v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
   v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT;
   v_ctl_auto_airspeed_throttle_pgain = V_CTL_AUTO_AIRSPEED_THROTTLE_PGAIN;
@@ -250,7 +250,7 @@ static inline void v_ctl_set_throttle( void ) {
 
 }
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
 #define AIRSPEED_LOOP_PERIOD (1./60.)
 
 // Airspeed control loop (input: [airspeed controlled, climb_setpoint], output: [throttle controlled, pitch setpoint])
@@ -336,7 +336,7 @@ void v_ctl_climb_loop ( void ) {
     // Set throttle
     v_ctl_set_throttle();
     break;
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
   case V_CTL_SPEED_AIRSPEED:
     v_ctl_set_airspeed();
     break;
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 68610ffc7e..9d0743b212 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -47,9 +47,9 @@
 #include "link_mcu.h"
 #include "sys_time.h"
 #include "generated/flight_plan.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "subsystems/settings.h"
-#include "xbee.h"
+#include "subsystems/datalink/xbee.h"
 
 #include "gpio.h"
 
@@ -64,18 +64,18 @@
 #endif
 
 
-#ifdef USE_IMU
+#if USE_IMU
 #include "subsystems/imu.h"
 #endif
-#ifdef USE_AHRS
+#if USE_AHRS
 #include "subsystems/ahrs.h"
 #endif
-#ifdef USE_AHRS_ALIGNER
+#if USE_AHRS_ALIGNER
 #include "subsystems/ahrs/ahrs_aligner.h"
 #endif
 
-#ifdef USE_AHRS
-#ifdef USE_IMU
+#if USE_AHRS
+#if USE_IMU
 static inline void on_gyro_event( void );
 static inline void on_accel_event( void );
 static inline void on_mag_event( void );
@@ -85,7 +85,7 @@ static inline void on_ahrs_event(void);
 #endif // USE_IMU
 #endif // USE_AHRS
 
-#ifdef USE_GPS
+#if USE_GPS
 static inline void on_gps_solution( void );
 #endif
 
@@ -196,12 +196,12 @@ static inline void reporting_task( void ) {
 
   /** initialisation phase during boot */
   if (boot) {
-    DOWNLINK_SEND_BOOT(DefaultChannel, &version);
+    DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &version);
     boot = FALSE;
   }
   /** then report periodicly */
   else {
-    PeriodicSendAp(DefaultChannel);
+    PeriodicSendAp(DefaultChannel, DefaultDevice);
   }
 }
 
@@ -236,7 +236,7 @@ static inline void telecommand_task( void ) {
   }
   mode_changed |= mcu1_status_update();
   if ( mode_changed )
-    PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
+    PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
 
 #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
   /** In AUTO1 mode, compute roll setpoint and pitch setpoint from
@@ -293,7 +293,7 @@ static void navigation_task( void ) {
       if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
         last_pprz_mode = pprz_mode;
         pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
-        PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
+        PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
         gps_lost = TRUE;
       }
     } else if (gps_lost) { /* GPS is ok */
@@ -301,7 +301,7 @@ static void navigation_task( void ) {
       pprz_mode = last_pprz_mode;
       gps_lost = FALSE;
 
-      PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
+      PERIODIC_SEND_PPRZ_MODE(DefaultChannel, DefaultDevice);
     }
   }
 #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
@@ -319,10 +319,10 @@ static void navigation_task( void ) {
 #endif
 
 #ifndef PERIOD_NAVIGATION_DefaultChannel_0 // If not sent periodically (in default 0 mode)
-  SEND_NAVIGATION(DefaultChannel);
+  SEND_NAVIGATION(DefaultChannel, DefaultDevice);
 #endif
 
-  SEND_CAM(DefaultChannel);
+  SEND_CAM(DefaultChannel, DefaultDevice);
 
   /* The nav task computes only nav_altitude. However, we are interested
      by desired_altitude (= nav_alt+alt_shift) in any case.
@@ -391,7 +391,7 @@ static void navigation_task( void ) {
 
 static inline void attitude_loop( void ) {
 
-#ifdef USE_INFRARED
+#if USE_INFRARED
   ahrs_update_infrared();
 #endif /* USE_INFRARED */
 
@@ -411,7 +411,7 @@ static inline void attitude_loop( void ) {
 
 }
 
-#ifdef USE_AHRS
+#if USE_AHRS
 #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
 volatile uint8_t new_ins_attitude = 0;
 #endif
@@ -425,11 +425,11 @@ void periodic_task_ap( void ) {
   static uint8_t _4Hz  = 0;
   static uint8_t _1Hz  = 0;
 
-#ifdef USE_IMU
+#if USE_IMU
   // Run at PERIODIC_FREQUENCY (60Hz if not defined)
   imu_periodic();
 
-#ifdef USE_AHRS
+#if USE_AHRS
   if (ahrs_timeout_counter < 255)
     ahrs_timeout_counter ++;
 #endif
@@ -478,9 +478,6 @@ void periodic_task_ap( void ) {
   switch(_4Hz) {
   case 0:
     estimator_propagate_state();
-#ifdef EXTRA_DOWNLINK_DEVICE
-    DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta);
-#endif
     navigation_task();
     break;
   case 1:
@@ -488,7 +485,7 @@ void periodic_task_ap( void ) {
         estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) {
       estimator_flight_time = 1;
       launch = TRUE; /* Not set in non auto launch */
-      DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec);
+      DOWNLINK_SEND_TAKEOFF(DefaultChannel, DefaultDevice, &cpu_time_sec);
   default:
     break;
     }
@@ -535,7 +532,7 @@ void init_ap( void ) {
 #endif /* SINGLE_MCU */
 
   /************* Sensors initialization ***************/
-#ifdef USE_GPS
+#if USE_GPS
   gps_init();
 #endif
 
@@ -543,15 +540,15 @@ void init_ap( void ) {
   GpioInit();
 #endif
 
-#ifdef USE_IMU
+#if USE_IMU
   imu_init();
 #endif
 
-#ifdef USE_AHRS_ALIGNER
+#if USE_AHRS_ALIGNER
   ahrs_aligner_init();
 #endif
 
-#ifdef USE_AHRS
+#if USE_AHRS
   ahrs_init();
 #endif
 
@@ -559,8 +556,8 @@ void init_ap( void ) {
 #if defined MCU_SPI_LINK
   link_mcu_init();
 #endif
-#ifdef MODEM
-  modem_init();
+#if USE_AUDIO_TELEMETRY
+  audio_telemetry_init();
 #endif
 
   /************ Internal status ***************/
@@ -607,15 +604,15 @@ void init_ap( void ) {
 /*********** EVENT ***********************************************************/
 void event_task_ap( void ) {
 
-#if defined USE_AHRS
-#ifdef USE_IMU
+#if USE_AHRS
+#if USE_IMU
   ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
 #else
   AhrsEvent(on_ahrs_event);
 #endif // USE_IMU
 #endif // USE_AHRS
 
-#ifdef USE_GPS
+#if USE_GPS
   GpsEvent(on_gps_solution);
 #endif /** USE_GPS */
 
@@ -647,12 +644,10 @@ void event_task_ap( void ) {
 } /* event_task_ap() */
 
 
-#ifdef USE_GPS
+#if USE_GPS
 static inline void on_gps_solution( void ) {
   estimator_update_state_gps();
-#ifdef USE_INFRARED
   ahrs_update_gps();
-#endif
 #ifdef GPS_TRIGGERED_FUNCTION
   GPS_TRIGGERED_FUNCTION();
 #endif
@@ -660,8 +655,8 @@ static inline void on_gps_solution( void ) {
 #endif
 
 
-#ifdef USE_AHRS
-#ifdef USE_IMU
+#if USE_AHRS
+#if USE_IMU
 static inline void on_accel_event( void ) {
 }
 
@@ -673,7 +668,7 @@ static inline void on_gyro_event( void ) {
     LED_ON(AHRS_CPU_LED);
 #endif
 
-#ifdef USE_AHRS_ALIGNER
+#if USE_AHRS_ALIGNER
   // Run aligner on raw data as it also makes averages.
   if (ahrs.status == AHRS_UNINIT) {
     ImuScaleGyro(imu);
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 7989e57433..c8d642caef 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -145,7 +145,18 @@ void event_task_fbw( void) {
 #ifdef ACTUATORS
   if (fbw_new_actuators > 0)
   {
-    SetActuatorsFromCommands(commands);
+    pprz_t trimmed_commands[COMMANDS_NB];
+    int i;
+    for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i];
+
+    #ifdef COMMAND_ROLL
+    trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10);
+    #endif
+    #ifdef COMMAND_PITCH
+    trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10);
+    #endif
+
+    SetActuatorsFromCommands(trimmed_commands);
     fbw_new_actuators = 0;
   }
 #endif
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
index 76027e6153..a5348bc16c 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
@@ -185,7 +185,7 @@ void h_ctl_init( void ) {
   use_airspeed_ratio = FALSE;
   airspeed_ratio2 = 1.;
 
-#ifdef USE_PITCH_TRIM
+#if USE_PITCH_TRIM
   v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
   v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
 #else
@@ -220,7 +220,7 @@ void h_ctl_course_loop ( void ) {
   BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
 }
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
 static inline void compute_airspeed_ratio( void ) {
   if (use_airspeed_ratio) {
     // low pass airspeed
@@ -238,7 +238,7 @@ static inline void compute_airspeed_ratio( void ) {
 
 void h_ctl_attitude_loop ( void ) {
   if (!h_ctl_disabled) {
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
     compute_airspeed_ratio();
 #endif
     h_ctl_roll_loop();
@@ -254,7 +254,7 @@ inline static void h_ctl_roll_loop( void ) {
 
   static float cmd_fb = 0.;
 
-#ifdef USE_ANGLE_REF
+#if USE_ANGLE_REF
   // Update reference setpoints for roll
   h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
   h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
@@ -322,17 +322,11 @@ inline static void h_ctl_roll_loop( void ) {
 }
 
 
-// NOT USED
-#ifdef LOITER_TRIM
-float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
-float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
-#endif
-
-#ifdef USE_PITCH_TRIM
+#if USE_PITCH_TRIM
 inline static void loiter(void) {
   float pitch_trim;
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
   if (estimator_airspeed > NOMINAL_AIRSPEED) {
     pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1);
   } else {
@@ -364,11 +358,11 @@ inline static void h_ctl_pitch_loop( void ) {
     h_ctl_pitch_of_roll = 0.;
 
   h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi);
-#ifdef USE_PITCH_TRIM
+#if USE_PITCH_TRIM
   loiter();
 #endif
 
-#ifdef USE_ANGLE_REF
+#if USE_ANGLE_REF
   // Update reference setpoints for pitch
   h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT;
   h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
index c84f0ee3fc..f331067369 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
@@ -274,7 +274,7 @@ void h_ctl_course_loop ( void ) {
 
 
 
-#if defined(AGR_CLIMB) && !defined(USE_AIRSPEED)
+#if defined(AGR_CLIMB) && !USE_AIRSPEED
   /** limit navigation during extreme altitude changes */
   if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
     if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE || V_CTL_AUTO_THROTTLE_BLENDED) {
diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h
index 4b023ee203..ec0ad75fef 100644
--- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h
+++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h
@@ -44,6 +44,11 @@ extern float h_ctl_course_pgain;
 extern float h_ctl_course_dgain;
 extern float h_ctl_roll_max_setpoint;
 
+#ifdef LOITER_TRIM
+extern float v_ctl_auto_throttle_loiter_trim;
+extern float v_ctl_auto_throttle_dash_trim;
+#endif
+
 /* roll and pitch disabling */
 extern bool_t h_ctl_disabled;
 
diff --git a/sw/airborne/firmwares/logger/main_logger.c b/sw/airborne/firmwares/logger/main_logger.c
index 8f3eea1406..cc23a76b42 100644
--- a/sw/airborne/firmwares/logger/main_logger.c
+++ b/sw/airborne/firmwares/logger/main_logger.c
@@ -100,7 +100,7 @@
 #endif
 
 #ifndef LOG_STOP_KEY
-/* BUTTON that stops logging (BUTTON = P0.7, DTR = P0.13, INT1 = P0.14) */
+/* BUTTON that stops logging (PPM_IN = P0.6, BUTTON = P0.7, DTR = P0.13, INT1 = P0.14) */
 #define LOG_STOP_KEY 7
 #endif
 
diff --git a/sw/airborne/firmwares/motor_bench/main_motor_bench.c b/sw/airborne/firmwares/motor_bench/main_motor_bench.c
index 93c902282f..814bb443e0 100644
--- a/sw/airborne/firmwares/motor_bench/main_motor_bench.c
+++ b/sw/airborne/firmwares/motor_bench/main_motor_bench.c
@@ -14,7 +14,7 @@
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "generated/settings.h"
 
@@ -80,10 +80,10 @@ static inline void main_periodic_task( void ) {
 
 
   RunOnceEvery(125, {
-      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
       PeriodicSendDlValue(DefaultChannel);
     });
-  DOWNLINK_SEND_MOTOR_BENCH_STATUS(DefaultChannel, &cpu_time_ticks, &throttle, &rpm, &amps , &thrust, &torque, &cpu_time_sec, &mb_modes_mode);
+  DOWNLINK_SEND_MOTOR_BENCH_STATUS(DefaultChannel, DefaultDevice, &cpu_time_ticks, &throttle, &rpm, &amps , &thrust, &torque, &cpu_time_sec, &mb_modes_mode);
 
 
 
@@ -120,6 +120,6 @@ static inline void main_dl_parse_msg(void) {
     uint8_t i = DL_SETTING_index(dl_buffer);
     float var = DL_SETTING_value(dl_buffer);
     DlSetting(i, var);
-    DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &var);
+    DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
   }
 }
diff --git a/sw/airborne/firmwares/motor_bench/main_turntable.c b/sw/airborne/firmwares/motor_bench/main_turntable.c
index 8ea42385e6..2e00fd91cd 100644
--- a/sw/airborne/firmwares/motor_bench/main_turntable.c
+++ b/sw/airborne/firmwares/motor_bench/main_turntable.c
@@ -8,7 +8,7 @@
 #include "mcu_arch.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "armVIC.h"
 
@@ -46,11 +46,11 @@ static inline void main_periodic( void ) {
   RunOnceEvery(50, {
       const float tach_to_rpm = 15000000.*2*M_PI/(float)NB_STEP;
       omega_rad = tach_to_rpm / lp_pulse;
-      DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, &omega_rad);}
+      DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, DefaultDevice, &omega_rad);}
       //      float foo = nb_pulse;
-      //      DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, &foo);}
+      //      DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, DefaultDevice, &foo);}
     );
-  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel,  16, MD5SUM);});
+  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice,  16, MD5SUM);});
 
 }
 
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c
index c0b602915b..8b11977485 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_asctec.c
@@ -29,7 +29,7 @@ void actuators_init(void) {
 #endif
   actuators_asctec.nb_err = 0;
 
-#if defined BOOZ_START_DELAY && ! defined SITL
+#if defined ACTUATORS_START_DELAY && ! defined SITL
   actuators_delay_done = FALSE;
   SysTimeTimerStart(actuators_delay_time);
 #else
@@ -45,9 +45,9 @@ void actuators_init(void) {
 
 #ifndef ACTUATORS_ASCTEC_V2_PROTOCOL
 void actuators_set(bool_t motors_on) {
-#if defined BOOZ_START_DELAY && ! defined SITL
+#if defined ACTUATORS_START_DELAY && ! defined SITL
   if (!actuators_delay_done) {
-    if (SysTimeTimer(actuators_delay_time) < SYS_TICS_OF_SEC(BOOZ_START_DELAY)) return;
+    if (SysTimeTimer(actuators_delay_time) < SYS_TICS_OF_SEC(ACTUATORS_START_DELAY)) return;
     else actuators_delay_done = TRUE;
   }
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
index b9d7a04b98..b9c3dc8211 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_mkk.c
@@ -46,7 +46,7 @@ void actuators_init(void) {
     actuators_mkk.trans[i].status = I2CTransSuccess;
   }
 
-#if defined BOOZ_START_DELAY && ! defined SITL
+#if defined ACTUATORS_START_DELAY && ! defined SITL
   actuators_delay_done = FALSE;
   SysTimeTimerStart(actuators_delay_time);
 #else
@@ -58,9 +58,9 @@ void actuators_init(void) {
 
 
 void actuators_set(bool_t motors_on) {
-#if defined BOOZ_START_DELAY && ! defined SITL
+#if defined ACTUATORS_START_DELAY && ! defined SITL
   if (!actuators_delay_done) {
-    if (SysTimeTimer(actuators_delay_time) < SYS_TICS_OF_SEC(BOOZ_START_DELAY)) return;
+    if (SysTimeTimer(actuators_delay_time) < SYS_TICS_OF_SEC(ACTUATORS_START_DELAY)) return;
     else actuators_delay_done = TRUE;
   }
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_skiron.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_skiron.c
index fe6a806195..c46397e251 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_skiron.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_skiron.c
@@ -45,7 +45,7 @@ void actuators_init(void) {
     actuators_skiron.actuators_idx[i] = actuators_idx[i];
   }
 
-#if defined BOOZ_START_DELAY && ! defined SITL
+#if defined ACTUATORS_START_DELAY && ! defined SITL
   actuators_delay_done = FALSE;
   SysTimeTimerStart(actuators_delay_time);
 #else
@@ -56,9 +56,9 @@ void actuators_init(void) {
 }
 
 void actuators_set(bool_t motors_on) {
-#if defined BOOZ_START_DELAY && ! defined SITL
+#if defined ACTUATORS_START_DELAY && ! defined SITL
   if (!actuators_delay_done) {
-    if (SysTimeTimer(actuators_delay_time) < SYS_TICS_OF_SEC(BOOZ_START_DELAY)) return;
+    if (SysTimeTimer(actuators_delay_time) < SYS_TICS_OF_SEC(ACTUATORS_START_DELAY)) return;
     else actuators_delay_done = TRUE;
   }
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
index 4cefcf51ae..6007067d4b 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
+++ b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.c
@@ -60,7 +60,7 @@ void actuators_pwm_arch_init(void) {
   /* TIM3 and TIM4 clock enable */
   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
   RCC_APB1PeriphClockCmd(PWM_5AND6_RCC, ENABLE);
-#ifdef USE_SERVOS_7AND8
+#if USE_SERVOS_7AND8
   RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
 #endif
 
@@ -82,7 +82,7 @@ void actuators_pwm_arch_init(void) {
   GPIO_InitStructure.GPIO_Pin   = PWM5_Pin | PWM6_Pin;
   GPIO_Init(PWM_5AND6_GPIO, &GPIO_InitStructure);
 
-#ifdef USE_SERVOS_7AND8
+#if USE_SERVOS_7AND8
   GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_6 | GPIO_Pin_7;
   GPIO_Init(GPIOB, &GPIO_InitStructure);
 #endif
@@ -100,7 +100,7 @@ void actuators_pwm_arch_init(void) {
   TIM_TimeBaseStructure.TIM_Period = (ONE_MHZ_CLK / SERVO_HZ_SECONDARY) - 1;
 #endif
   TIM_TimeBaseInit(PWM_5AND6_TIMER, &TIM_TimeBaseStructure);
-#ifdef USE_SERVOS_7AND8
+#if USE_SERVOS_7AND8
   TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
 #endif
 
@@ -135,7 +135,7 @@ void actuators_pwm_arch_init(void) {
   TIM_OC_INIT(PWM6_OC)(PWM_5AND6_TIMER, &TIM_OCInitStructure);
   TIM_OC_PRELOADCONFIG(PWM6_OC)(PWM_5AND6_TIMER, TIM_OCPreload_Enable);
 
-#ifdef USE_SERVOS_7AND8
+#if USE_SERVOS_7AND8
   /* PWM7 Mode configuration: TIM4 Channel3 */
   TIM_OC1Init(TIM4, &TIM_OCInitStructure);
   TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
@@ -172,7 +172,7 @@ void actuators_pwm_commit(void) {
   TIM_SETCOMPARE(PWM5_OC)(PWM_5AND6_TIMER, actuators_pwm_values[4]);
   TIM_SETCOMPARE(PWM6_OC)(PWM_5AND6_TIMER, actuators_pwm_values[5]);
 
-#ifdef USE_SERVOS_7AND8
+#if USE_SERVOS_7AND8
   TIM_SetCompare1(TIM4, actuators_pwm_values[6]);
   TIM_SetCompare2(TIM4, actuators_pwm_values[7]);
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
index e47bad7512..867cb006e8 100644
--- a/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
+++ b/sw/airborne/firmwares/rotorcraft/actuators/arch/stm32/actuators_pwm_arch.h
@@ -29,7 +29,7 @@
 #ifndef ACTUATORS_PWM_ARCH_H
 #define ACTUATORS_PWM_ARCH_H
 
-#ifdef USE_SERVOS_7AND8
+#if USE_SERVOS_7AND8
 #define ACTUATORS_PWM_NB 8
 #else
 #define ACTUATORS_PWM_NB 6
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index 125937e453..74ce6df6ec 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -120,7 +120,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
       break;
 #endif
     case AP_MODE_KILL:
-      autopilot_motors_on = FALSE;
+      autopilot_set_motors_on(FALSE);
       guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
       break;
     case AP_MODE_RATE_DIRECT:
@@ -190,7 +190,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
    radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_TRESHOLD)
 
 
-static inline void autopilot_check_in_flight( void) {
+static inline void autopilot_check_in_flight( bool_t motors_on ) {
   if (autopilot_in_flight) {
     if (autopilot_in_flight_counter > 0) {
       if (THROTTLE_STICK_DOWN()) {
@@ -206,7 +206,7 @@ static inline void autopilot_check_in_flight( void) {
   }
   else { /* not in flight */
     if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
-        autopilot_motors_on) {
+        motors_on) {
       if (!THROTTLE_STICK_DOWN()) {
         autopilot_in_flight_counter++;
         if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
@@ -309,8 +309,19 @@ void autopilot_on_rc_frame(void) {
     autopilot_set_mode(AP_MODE_KILL);
 #endif
 
+#ifdef NO_ARMING_SEQUENCE
+#ifndef RADIO_KILL_SWITCH
+  // no arming sequence and no kill switch
+  // motors are turned on when "in_flight" is detected
+  // it can also be set by the flight plan
+  autopilot_check_in_flight(TRUE);
+  autopilot_motors_on = autopilot_in_flight;
+#endif
+#else
+  // an arming sequence is used to start motors
   autopilot_check_motors_on();
-  autopilot_check_in_flight();
+  autopilot_check_in_flight(autopilot_motors_on);
+#endif
   kill_throttle = !autopilot_motors_on;
 
   if (autopilot_mode > AP_MODE_FAILSAFE) {
diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c
index e4f6215e41..931663c427 100644
--- a/sw/airborne/firmwares/rotorcraft/datalink.c
+++ b/sw/airborne/firmwares/rotorcraft/datalink.c
@@ -24,12 +24,12 @@
 #define DATALINK_C
 #define MODULES_DATALINK_C
 
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 
 #include "generated/modules.h"
 
 #include "generated/settings.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "messages.h"
 #include "dl_protocol.h"
 #include "mcu_periph/uart.h"
@@ -54,7 +54,7 @@ void dl_parse_msg(void) {
 
   case  DL_PING:
     {
-      DOWNLINK_SEND_PONG(DefaultChannel);
+      DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
     }
     break;
 
@@ -64,7 +64,7 @@ void dl_parse_msg(void) {
       uint8_t i = DL_SETTING_index(dl_buffer);
       float var = DL_SETTING_value(dl_buffer);
       DlSetting(i, var);
-      DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &var);
+      DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
     }
     break;
 
@@ -73,7 +73,7 @@ void dl_parse_msg(void) {
       if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break;
       uint8_t i = DL_GET_SETTING_index(dl_buffer);
       float val = settings_get_value(i);
-      DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
+      DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
     }
     break;
 
@@ -101,7 +101,7 @@ void dl_parse_msg(void) {
       enu.y = POS_BFP_OF_REAL(enu.y)/100;
       enu.z = POS_BFP_OF_REAL(enu.z)/100;
       VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
-      DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp_id, &enu.x, &enu.y, &enu.z);
+      DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
     }
     break;
 #endif /* USE_NAVIGATION */
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index b9f7b9d439..0e45915486 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -22,7 +22,7 @@
  */
 
 #define GUIDANCE_H_C
-//#define GUIDANCE_H_USE_REF
+//#define GUIDANCE_H_USE_REF 1
 #include "firmwares/rotorcraft/guidance/guidance_h.h"
 
 #include "subsystems/ahrs.h"
@@ -193,7 +193,7 @@ void guidance_h_run(bool_t  in_flight) {
       }
       else {
         INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
-#ifdef GUIDANCE_H_USE_REF
+#if GUIDANCE_H_USE_REF
         b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp);
 #endif
 #ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
@@ -240,12 +240,14 @@ __attribute__ ((always_inline)) static inline void  guidance_h_hover_run(void) {
 
   /* run PID */
   // cmd_earth < 15.17
-  guidance_h_command_earth.x = (guidance_h_pgain<<1)  * guidance_h_pos_err.x +
-                                     guidance_h_dgain * (guidance_h_speed_err.x>>9) +
-                                      guidance_h_igain * (guidance_h_pos_err_sum.x >> 12);
-  guidance_h_command_earth.y = (guidance_h_pgain<<1)  * guidance_h_pos_err.y +
-                                     guidance_h_dgain *( guidance_h_speed_err.y>>9) +
-		                      guidance_h_igain * (guidance_h_pos_err_sum.y >> 12);
+  guidance_h_command_earth.x =
+    guidance_h_pgain * (guidance_h_pos_err.x << (10 - INT32_POS_FRAC)) +
+    guidance_h_dgain * (guidance_h_speed_err.x >> (INT32_SPEED_FRAC - 10)) +
+    guidance_h_igain * (guidance_h_pos_err_sum.x >> (12 + INT32_POS_FRAC - 10));
+  guidance_h_command_earth.y =
+    guidance_h_pgain * (guidance_h_pos_err.y << (10 - INT32_POS_FRAC)) +
+    guidance_h_dgain * (guidance_h_speed_err.y >> (INT32_SPEED_FRAC - 10)) +
+    guidance_h_igain * (guidance_h_pos_err_sum.y >> (12 + INT32_POS_FRAC - 10));
 
   VECT2_STRIM(guidance_h_command_earth, -MAX_BANK, MAX_BANK);
 
@@ -282,7 +284,7 @@ __attribute__ ((always_inline)) static inline void  guidance_h_hover_run(void) {
 __attribute__ ((always_inline)) static inline void  guidance_h_nav_run(bool_t in_flight) {
 
   /* convert our reference to generic representation */
-#ifdef GUIDANCE_H_USE_REF
+#if GUIDANCE_H_USE_REF
   INT32_VECT2_RSHIFT(guidance_h_pos_ref,   b2_gh_pos_ref,   (B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
   INT32_VECT2_LSHIFT(guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
   INT32_VECT2_LSHIFT(guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
@@ -310,6 +312,7 @@ __attribute__ ((always_inline)) static inline void  guidance_h_nav_run(bool_t in
     VECT2_ADD(guidance_h_pos_err_sum, guidance_h_pos_err);
     /* saturate it               */
     VECT2_STRIM(guidance_h_pos_err_sum, -MAX_POS_ERR_SUM, MAX_POS_ERR_SUM);
+    INT_VECT2_ZERO(guidance_h_nav_err);
   }
   else { // Tracking algorithm, no integral
     int32_t vect_prod = 0;
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
index 61613bd440..eba0bee30c 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
@@ -22,7 +22,7 @@
  */
 
 #define GUIDANCE_V_C
-#define GUIDANCE_V_USE_REF
+#define GUIDANCE_V_USE_REF 1
 #include "firmwares/rotorcraft/guidance/guidance_v.h"
 
 
@@ -186,7 +186,7 @@ void guidance_v_run(bool_t in_flight) {
     break;
 
   case GUIDANCE_V_MODE_CLIMB:
-#ifdef USE_FMS
+#if USE_FMS
     if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_CLIMB)
       guidance_v_zd_sp = fms.input.v_sp.climb;
 #endif
@@ -201,7 +201,7 @@ void guidance_v_run(bool_t in_flight) {
     break;
 
   case GUIDANCE_V_MODE_HOVER:
-#ifdef USE_FMS
+#if USE_FMS
     if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER)
       guidance_v_z_sp = fms.input.v_sp.height;
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
index 3e5f6d9235..e57bcbc721 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
@@ -63,7 +63,7 @@ int32_t gv_adapt_Xmeas;
 #define GV_ADAPT_SYS_NOISE_F 0.00005
 #define GV_ADAPT_SYS_NOISE  BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)
 
-#ifndef USE_ADAPT_HOVER
+#if !USE_ADAPT_HOVER
 
 #define GV_ADAPT_MEAS_NOISE_F 2.0
 #define GV_ADAPT_MEAS_NOISE BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_F, GV_ADAPT_P_FRAC)
@@ -81,7 +81,8 @@ int32_t gv_adapt_Xmeas;
 #define GV_ADAPT_MIN_CMD 20
 #define GV_ADAPT_HOVER_MAX_CMD 120
 #define GV_ADAPT_HOVER_MIN_CMD 60
-#endif
+
+#endif /* USE_ADAPT_HOVER */
 
 static inline void gv_adapt_init(void) {
   gv_adapt_X = GV_ADAPT_X0;
@@ -102,7 +103,7 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
   gv_adapt_P =  gv_adapt_P + GV_ADAPT_SYS_NOISE;
   /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */
   const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
-#ifdef USE_ADAPT_HOVER
+#if USE_ADAPT_HOVER
   /* Update only if accel and commands are in a valid range */
   if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD
       || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL)
@@ -116,7 +117,7 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
   int32_t residual = gv_adapt_Xmeas - gv_adapt_X;
   /* Covariance Error   */
   int32_t E = 0;
-#ifdef USE_ADAPT_HOVER
+#if USE_ADAPT_HOVER
   if ((thrust_applied > GV_ADAPT_HOVER_MIN_CMD && thrust_applied < GV_ADAPT_HOVER_MAX_CMD) ||
       (zdd_meas > -GV_ADAPT_HOVER_ACCEL && zdd_meas < GV_ADAPT_HOVER_ACCEL))
     E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER;
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index b0808d6202..1d8187c043 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -29,11 +29,11 @@
 #include "sys_time.h"
 #include "led.h"
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "firmwares/rotorcraft/telemetry.h"
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "subsystems/settings.h"
-#include "xbee.h"
+#include "subsystems/datalink/xbee.h"
 
 #include "firmwares/rotorcraft/commands.h"
 #include "firmwares/rotorcraft/actuators.h"
@@ -85,15 +85,6 @@ int main( void ) {
 
 STATIC_INLINE void main_init( void ) {
 
-#ifndef NO_FUCKING_STARTUP_DELAY
-#ifndef RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT
-  /* IF THIS IS NEEDED SOME PERHIPHERAL THEN PLEASE MOVE IT THERE */
-  for (uint32_t startup_counter=0; startup_counter<2000000; startup_counter++){
-    __asm("nop");
-  }
-#endif
-#endif
-
   mcu_init();
 
   sys_time_init();
@@ -119,7 +110,7 @@ STATIC_INLINE void main_init( void ) {
 
   ins_init();
 
-#ifdef USE_GPS
+#if USE_GPS
   gps_init();
 #endif
 
@@ -153,7 +144,7 @@ STATIC_INLINE void main_periodic( void ) {
       /* booz_fms_periodic(); FIXME */                      \
     },                                                      \
     {                                                       \
-      electrical_periodic();				    \
+      electrical_periodic();                                \
     },                                                      \
     {                                                       \
       LED_PERIODIC();                                       \
@@ -165,10 +156,10 @@ STATIC_INLINE void main_periodic( void ) {
     {},                                                     \
     {},                                                     \
     {                                                       \
-      Booz2TelemetryPeriodic();                             \
+      TelemetryPeriodic();                                  \
     } );
 
-#ifdef USE_GPS
+#if USE_GPS
   if (radio_control.status != RC_OK &&                  \
       autopilot_mode == AP_MODE_NAV && GpsIsLost())		\
     autopilot_set_mode(AP_MODE_FAILSAFE);
@@ -194,7 +185,7 @@ STATIC_INLINE void main_event( void ) {
 
   BaroEvent(on_baro_abs_event, on_baro_dif_event);
 
-#ifdef USE_GPS
+#if USE_GPS
   GpsEvent(on_gps_event);
 #endif
 
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 5b85ffac51..913fd5c67b 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -114,7 +114,7 @@ void nav_run(void) {
   int32_t dist_to_waypoint;
   INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
 
-#ifndef GUIDANCE_H_USE_REF
+#if GUIDANCE_H_USE_REF
   if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
     VECT2_COPY(navigation_carrot, navigation_target);
   }
@@ -255,7 +255,7 @@ unit_t nav_reset_reference( void ) {
 unit_t nav_reset_alt( void ) {
   ins_vf_realign = TRUE;
 
-#ifdef USE_GPS
+#if USE_GPS
   ins_ltp_def.lla.alt = gps.lla_pos.alt;
   ins_ltp_def.hmsl = gps.hmsl;
 #endif
@@ -283,13 +283,13 @@ void nav_periodic_task() {
   ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 1000.);
 }
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "messages.h"
 #include "mcu_periph/uart.h"
 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
   if (wp_id < nb_waypoint) {
     INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
-    DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z));
+    DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z));
   }
 }
 
@@ -310,7 +310,7 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int
   nav_heading += delta_heading;
 
   INT32_COURSE_NORMALIZE(nav_heading);
-  RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp, &(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z)));
+  RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z)));
 }
 
 bool_t nav_detect_ground(void) {
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c
index 0c286356d7..20008096c1 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/quat_setpoint.c
@@ -17,7 +17,7 @@
 
 #include "messages.h"
 #include "mcu_periph/uart.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #define QUAT_SETPOINT_HOVER_PITCH RadOfDeg(90)
 
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
index 803d30adf1..1e5a845b81 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
@@ -144,25 +144,21 @@ void stabilization_attitude_run(bool_t  in_flight) {
     stabilization_gains.d.z    * rate_err.r +
     OFFSET_AND_ROUND2((stabilization_gains.i.z  * stabilization_att_sum_err.psi), 10);
 
-  /* sum feedforward and feedback */
+
 #ifdef USE_HELI
+#define CMD_SHIFT 12
+#else
+#define CMD_SHIFT 16
+#endif
+
+  /* sum feedforward and feedback */
   stabilization_cmd[COMMAND_ROLL] =
-    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]), 12);
+    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]), CMD_SHIFT);
 
   stabilization_cmd[COMMAND_PITCH] =
-    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]), 12);
+    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]), CMD_SHIFT);
 
   stabilization_cmd[COMMAND_YAW] =
-    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), 12);
- #else
-   stabilization_cmd[COMMAND_ROLL] =
-    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]), 16);
-
-  stabilization_cmd[COMMAND_PITCH] =
-    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]), 16);
-
-  stabilization_cmd[COMMAND_YAW] =
-    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), 16);
- #endif
+    OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), CMD_SHIFT);
 
 }
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
index f324cac0bf..3e52134344 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c
@@ -42,7 +42,7 @@ void stabilization_attitude_ref_init(void) {
 
 void stabilization_attitude_ref_update() {
 
-#ifdef USE_REF
+#if USE_REF
 
     /* dumb integrate reference attitude        */
     struct FloatRates delta_rate;
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
index 73886a315e..bb83dd82c4 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
@@ -138,7 +138,7 @@ void stabilization_attitude_ref_enter()
 
 #include "messages.h"
 #include "mcu_periph/uart.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 void stabilization_attitude_ref_update() {
 
diff --git a/sw/airborne/firmwares/rotorcraft/telemetry.h b/sw/airborne/firmwares/rotorcraft/telemetry.h
index 59cc90d163..5129673f9c 100644
--- a/sw/airborne/firmwares/rotorcraft/telemetry.h
+++ b/sw/airborne/firmwares/rotorcraft/telemetry.h
@@ -28,7 +28,7 @@
 #include "messages.h"
 #include "mcu_periph/uart.h"
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifdef RADIO_CONTROL
 #include "subsystems/radio_control.h"
@@ -43,11 +43,11 @@
 #define TELEMETRY_STARTUP_DELAY 0
 #endif
 
-#define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM)
+#define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM)
 
 #include "subsystems/electrical.h"
 #include "subsystems/imu.h"
-#ifdef USE_GPS
+#if USE_GPS
 #include "subsystems/gps.h"
 #endif
 #include "subsystems/ins.h"
@@ -57,11 +57,11 @@
 
 extern uint8_t telemetry_mode_Main_DefaultChannel;
 
-#ifdef USE_GPS
-#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) {			\
+#if USE_GPS
+#define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) {			\
     uint32_t imu_nb_err = 0;						\
     uint8_t _twi_blmc_nb_err = 0;					\
-    DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan,				\
+    DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev,				\
 				    &imu_nb_err,			\
 				    &_twi_blmc_nb_err,			\
 				    &radio_control.status,		\
@@ -77,11 +77,11 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				    );					\
   }
 #else /* !USE_GPS */
-#define PERIODIC_SEND_ROTORCRAFT_STATUS(_chan) {			\
+#define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) {			\
     uint32_t imu_nb_err = 0;						\
     uint8_t twi_blmc_nb_err = 0;					\
     uint8_t  fix = GPS_FIX_NONE;					\
-    DOWNLINK_SEND_ROTORCRAFT_STATUS(_chan,					\
+    DOWNLINK_SEND_ROTORCRAFT_STATUS(_trans, _dev,					\
 			      &imu_nb_err,				\
 			      &twi_blmc_nb_err,				\
 			      &radio_control.status,			\
@@ -99,17 +99,17 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 #endif /* USE_GPS */
 
 #ifdef RADIO_CONTROL
-#define PERIODIC_SEND_RC(_chan) DOWNLINK_SEND_RC(_chan, RADIO_CONTROL_NB_CHANNEL, radio_control.values)
+#define PERIODIC_SEND_RC(_trans, _dev) DOWNLINK_SEND_RC(_trans, _dev, RADIO_CONTROL_NB_CHANNEL, radio_control.values)
 #if defined RADIO_KILL_SWITCH
-#define PERIODIC_SEND_BOOZ2_RADIO_CONTROL(_chan) SEND_BOOZ2_RADIO_CONTROL( _chan, &radio_control.values[RADIO_KILL_SWITCH])
+#define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) SEND_ROTORCRAFT_RADIO_CONTROL( _trans, _dev, &radio_control.values[RADIO_KILL_SWITCH])
 #else /* ! RADIO_KILL_SWITCH */
-#define PERIODIC_SEND_BOOZ2_RADIO_CONTROL(_chan) {			                    \
+#define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) {			                    \
     int16_t foo = -42;							                    \
-    SEND_BOOZ2_RADIO_CONTROL( _chan, &foo)				                    \
+    SEND_ROTORCRAFT_RADIO_CONTROL( _trans, _dev, &foo)				                    \
 }
 #endif /* !RADIO_KILL_SWITCH */
-#define SEND_BOOZ2_RADIO_CONTROL(_chan, _kill_switch) {			                    \
-    DOWNLINK_SEND_BOOZ2_RADIO_CONTROL(_chan,				                    \
+#define SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev, _kill_switch) {			                    \
+    DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev,				                    \
 				      &radio_control.values[RADIO_ROLL],            \
 				      &radio_control.values[RADIO_PITCH],           \
 				      &radio_control.values[RADIO_YAW],	            \
@@ -118,83 +118,73 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				      _kill_switch,                                        \
 				      &radio_control.status);}
 #else /* ! RADIO_CONTROL */
-#define PERIODIC_SEND_RC(_chan) {}
-#define PERIODIC_SEND_BOOZ2_RADIO_CONTROL(_chan) {}
+#define PERIODIC_SEND_RC(_trans, _dev) {}
+#define PERIODIC_SEND_ROTORCRAFT_RADIO_CONTROL(_trans, _dev) {}
 #endif
 
 #ifdef RADIO_CONTROL_TYPE_PPM
-#define PERIODIC_SEND_PPM(_chan)                    \
-  DOWNLINK_SEND_PPM(_chan,                          \
+#define PERIODIC_SEND_PPM(_trans, _dev)                    \
+  DOWNLINK_SEND_PPM(_trans, _dev,                          \
                     &radio_control.frame_rate,      \
                     PPM_NB_CHANNEL,                 \
                     ppm_pulses)
 #else
-#define PERIODIC_SEND_PPM(_chan) {}
+#define PERIODIC_SEND_PPM(_trans, _dev) {}
 #endif
 
-#define PERIODIC_SEND_IMU_GYRO_SCALED(_chan) {		\
-    DOWNLINK_SEND_IMU_GYRO_SCALED(_chan,			\
+#define PERIODIC_SEND_IMU_GYRO_SCALED(_trans, _dev) {		\
+    DOWNLINK_SEND_IMU_GYRO_SCALED(_trans, _dev,			\
 			     &imu.gyro.p,		\
 			     &imu.gyro.q,		\
 			     &imu.gyro.r);		\
   }
 
-#define PERIODIC_SEND_IMU_ACCEL_SCALED(_chan) {			\
-    DOWNLINK_SEND_IMU_ACCEL_SCALED(_chan,				\
+#define PERIODIC_SEND_IMU_ACCEL_SCALED(_trans, _dev) {			\
+    DOWNLINK_SEND_IMU_ACCEL_SCALED(_trans, _dev,				\
 			      &imu.accel.x,		\
 			      &imu.accel.y,		\
 			      &imu.accel.z);		\
   }
 
-#define PERIODIC_SEND_IMU_MAG_SCALED(_chan) {			\
-    DOWNLINK_SEND_IMU_MAG_SCALED(_chan,				\
+#define PERIODIC_SEND_IMU_MAG_SCALED(_trans, _dev) {			\
+    DOWNLINK_SEND_IMU_MAG_SCALED(_trans, _dev,				\
 			    &imu.mag.x,			\
 			    &imu.mag.y,			\
 			    &imu.mag.z);			\
   }
 
-#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {				\
-    DOWNLINK_SEND_IMU_GYRO_RAW(_chan,					\
+#define PERIODIC_SEND_IMU_GYRO_RAW(_trans, _dev) {				\
+    DOWNLINK_SEND_IMU_GYRO_RAW(_trans, _dev,					\
 			       &imu.gyro_unscaled.p,		\
 			       &imu.gyro_unscaled.q,		\
 			       &imu.gyro_unscaled.r);		\
   }
 
-#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {				\
-    DOWNLINK_SEND_IMU_ACCEL_RAW(_chan,					\
+#define PERIODIC_SEND_IMU_ACCEL_RAW(_trans, _dev) {				\
+    DOWNLINK_SEND_IMU_ACCEL_RAW(_trans, _dev,					\
 				&imu.accel_unscaled.x,		\
 				&imu.accel_unscaled.y,		\
 				&imu.accel_unscaled.z);		\
   }
 
-#define PERIODIC_SEND_IMU_MAG_RAW(_chan) {				\
-    DOWNLINK_SEND_IMU_MAG_RAW(_chan,					\
+#define PERIODIC_SEND_IMU_MAG_RAW(_trans, _dev) {				\
+    DOWNLINK_SEND_IMU_MAG_RAW(_trans, _dev,					\
 			      &imu.mag_unscaled.x,			\
 			      &imu.mag_unscaled.y,			\
 			      &imu.mag_unscaled.z);		\
   }
 
-/* FIXME: make that depend on board */
-#define PERIODIC_SEND_BOOZ_BARO_RAW(_chan) {				\
-    DOWNLINK_SEND_BOOZ2_BARO_RAW(_chan,					\
-				 &baro_board.offset,			\
-			         &baro.absolute,			\
-			         &baro_board.value_filtered);		\
+#define PERIODIC_SEND_BARO_RAW(_trans, _dev) {         \
+    DOWNLINK_SEND_BARO_RAW(_trans, _dev,               \
+                           &baro.absolute,      \
+                           &baro.differential); \
   }
 
 
-#define PERIODIC_SEND_BARO_RAW(_chan) {					\
-    DOWNLINK_SEND_BARO_RAW(_chan,					\
-			   &baro.absolute,				\
-			   &baro.differential);				\
-  }
-
-
-
 
 #include "firmwares/rotorcraft/stabilization.h"
-#define PERIODIC_SEND_RATE_LOOP(_chan) {                          \
-    DOWNLINK_SEND_RATE_LOOP(_chan,                                \
+#define PERIODIC_SEND_RATE_LOOP(_trans, _dev) {                          \
+    DOWNLINK_SEND_RATE_LOOP(_trans, _dev,                                \
                                   &stabilization_rate_sp.p,        \
                                   &stabilization_rate_sp.q,        \
                                   &stabilization_rate_sp.r,        \
@@ -217,8 +207,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
   }
 
 #ifdef STABILISATION_ATTITUDE_TYPE_INT
-#define PERIODIC_SEND_STAB_ATTITUDE(_chan) {			\
-    DOWNLINK_SEND_STAB_ATTITUDE_INT(_chan,			\
+#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) {			\
+    DOWNLINK_SEND_STAB_ATTITUDE_INT(_trans, _dev,			\
 					  &ahrs.body_rate.p,	\
 					  &ahrs.body_rate.q,	\
 					  &ahrs.body_rate.r,	\
@@ -243,8 +233,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
   }
 
 
-#define PERIODIC_SEND_STAB_ATTITUDE_REF(_chan) {			\
-    DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(_chan,			\
+#define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) {			\
+    DOWNLINK_SEND_STAB_ATTITUDE_REF_INT(_trans, _dev,			\
 					      &stab_att_sp_euler.phi, \
 					      &stab_att_sp_euler.theta, \
 					      &stab_att_sp_euler.psi, \
@@ -261,8 +251,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 #endif /* STABILISATION_ATTITUDE_TYPE_INT */
 
 #ifdef STABILISATION_ATTITUDE_TYPE_FLOAT
-#define PERIODIC_SEND_STAB_ATTITUDE(_chan) {			\
-    DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_chan,			\
+#define PERIODIC_SEND_STAB_ATTITUDE(_trans, _dev) {			\
+    DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(_trans, _dev,			\
 					    &ahrs_float.body_rate.p,	\
 					    &ahrs_float.body_rate.q,	\
 					    &ahrs_float.body_rate.r,	\
@@ -286,8 +276,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 					    &stabilization_cmd[COMMAND_YAW]); \
   }
 
-#define PERIODIC_SEND_STAB_ATTITUDE_REF(_chan) {			\
-    DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(_chan,			\
+#define PERIODIC_SEND_STAB_ATTITUDE_REF(_trans, _dev) {			\
+    DOWNLINK_SEND_STAB_ATTITUDE_REF_FLOAT(_trans, _dev,			\
 						&stab_att_sp_euler.phi, \
 						&stab_att_sp_euler.theta, \
 						&stab_att_sp_euler.psi, \
@@ -306,8 +296,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 
 
 #include "subsystems/ahrs/ahrs_aligner.h"
-#define PERIODIC_SEND_FILTER_ALIGNER(_chan) {			\
-    DOWNLINK_SEND_FILTER_ALIGNER(_chan,				\
+#define PERIODIC_SEND_FILTER_ALIGNER(_trans, _dev) {			\
+    DOWNLINK_SEND_FILTER_ALIGNER(_trans, _dev,				\
 				       &ahrs_aligner.lp_gyro.p,	\
 				       &ahrs_aligner.lp_gyro.q,	\
 				       &ahrs_aligner.lp_gyro.r,	\
@@ -319,8 +309,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
   }
 
 
-#define PERIODIC_SEND_BOOZ2_CMD(_chan) {				\
-    DOWNLINK_SEND_BOOZ2_CMD(_chan,					\
+#define PERIODIC_SEND_BOOZ2_CMD(_trans, _dev) {				\
+    DOWNLINK_SEND_BOOZ2_CMD(_trans, _dev,					\
 			    &stabilization_cmd[COMMAND_ROLL],	\
 			    &stabilization_cmd[COMMAND_PITCH],	\
 			    &stabilization_cmd[COMMAND_YAW],	\
@@ -328,10 +318,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
   }
 
 
-#ifdef USE_AHRS_CMPL
+#if USE_AHRS_CMPL
 #include "subsystems/ahrs/ahrs_int_cmpl_euler.h"
-#define PERIODIC_SEND_FILTER(_chan) {					\
-    DOWNLINK_SEND_FILTER(_chan,						\
+#define PERIODIC_SEND_FILTER(_trans, _dev) {					\
+    DOWNLINK_SEND_FILTER(_trans, _dev,						\
 			 &ahrs.ltp_to_imu_euler.phi,			\
 			 &ahrs.ltp_to_imu_euler.theta,			\
 			 &ahrs.ltp_to_imu_euler.psi,			\
@@ -349,15 +339,15 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 			 &ahrs_impl.gyro_bias.r);			\
   }
 #else
-#define PERIODIC_SEND_FILTER(_chan) {}
+#define PERIODIC_SEND_FILTER(_trans, _dev) {}
 #endif
 
-#ifdef USE_AHRS_LKF
+#if USE_AHRS_LKF
 #include "subsystems/ahrs.h"
 #include "ahrs/ahrs_float_lkf.h"
-#define PERIODIC_SEND_AHRS_LKF(_chan) {				\
+#define PERIODIC_SEND_AHRS_LKF(_trans, _dev) {				\
     DOWNLINK_SEND_AHRS_LKF(&bafl_eulers.phi,			\
-				_chan,					\
+				_trans, _dev,					\
 				&bafl_eulers.theta,			\
 				&bafl_eulers.psi,			\
 				&bafl_quat.qi,				\
@@ -374,8 +364,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				&bafl_mag.y,				\
 				&bafl_mag.z);				\
   }
-#define PERIODIC_SEND_AHRS_LKF_DEBUG(_chan) {		   \
-    DOWNLINK_SEND_AHRS_LKF_DEBUG(_chan,			   \
+#define PERIODIC_SEND_AHRS_LKF_DEBUG(_trans, _dev) {		   \
+    DOWNLINK_SEND_AHRS_LKF_DEBUG(_trans, _dev,			   \
 				      &bafl_X[0],		   \
 				      &bafl_X[1],		   \
 				      &bafl_X[2],		   \
@@ -392,8 +382,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				      &bafl_P[4][4],		   \
 				      &bafl_P[5][5]);		   \
   }
-#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_chan) {		    \
-    DOWNLINK_SEND_AHRS_LKF_ACC_DBG(_chan,			    \
+#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_trans, _dev) {		    \
+    DOWNLINK_SEND_AHRS_LKF_ACC_DBG(_trans, _dev,			    \
 					&bafl_q_a_err.qi,	    \
 					&bafl_q_a_err.qx,	    \
 					&bafl_q_a_err.qy,	    \
@@ -402,8 +392,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 					&bafl_b_a_err.q,	    \
 					&bafl_b_a_err.r);	    \
   }
-#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_chan) {	    \
-    DOWNLINK_SEND_AHRS_LKF_MAG_DBG(_chan,		    \
+#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_trans, _dev) {	    \
+    DOWNLINK_SEND_AHRS_LKF_MAG_DBG(_trans, _dev,		    \
 					&bafl_q_m_err.qi,   \
 					&bafl_q_m_err.qx,   \
 					&bafl_q_m_err.qy,   \
@@ -413,15 +403,15 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 					&bafl_b_m_err.r);   \
   }
 #else
-#define PERIODIC_SEND_AHRS_LKF(_chan) {}
-#define PERIODIC_SEND_AHRS_LKF_DEBUG(_chan) {}
-#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_chan) {}
-#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_chan) {}
+#define PERIODIC_SEND_AHRS_LKF(_trans, _dev) {}
+#define PERIODIC_SEND_AHRS_LKF_DEBUG(_trans, _dev) {}
+#define PERIODIC_SEND_AHRS_LKF_MAG_DBG(_trans, _dev) {}
+#define PERIODIC_SEND_AHRS_LKF_ACC_DBG(_trans, _dev) {}
 #endif
 
 
-#define PERIODIC_SEND_AHRS_REF_QUAT(_chan) {				\
-    DOWNLINK_SEND_AHRS_REF_QUAT(_chan,				\
+#define PERIODIC_SEND_AHRS_REF_QUAT(_trans, _dev) {				\
+    DOWNLINK_SEND_AHRS_REF_QUAT(_trans, _dev,				\
 				  &stab_att_ref_quat.qi,	\
 				  &stab_att_ref_quat.qx,	\
 				  &stab_att_ref_quat.qy,	\
@@ -432,8 +422,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				  &ahrs.ltp_to_body_quat.qz);	\
   }
 
-#define PERIODIC_SEND_BOOZ2_AHRS_QUAT(_chan) {				\
-    DOWNLINK_SEND_BOOZ2_AHRS_QUAT(_chan,				\
+#define PERIODIC_SEND_AHRS_QUAT_INT(_trans, _dev) {				\
+    DOWNLINK_SEND_AHRS_QUAT_INT(_trans, _dev,				\
 				  &ahrs.ltp_to_imu_quat.qi,	\
 				  &ahrs.ltp_to_imu_quat.qx,	\
 				  &ahrs.ltp_to_imu_quat.qy,	\
@@ -444,8 +434,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				  &ahrs.ltp_to_body_quat.qz);	\
   }
 
-#define PERIODIC_SEND_BOOZ2_AHRS_EULER(_chan) {				\
-    DOWNLINK_SEND_BOOZ2_AHRS_EULER(_chan,				\
+#define PERIODIC_SEND_AHRS_EULER_INT(_trans, _dev) {				\
+    DOWNLINK_SEND_AHRS_EULER_INT(_trans, _dev,				\
 				   &ahrs.ltp_to_imu_euler.phi,	\
 				   &ahrs.ltp_to_imu_euler.theta,	\
 				   &ahrs.ltp_to_imu_euler.psi,	\
@@ -454,8 +444,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				   &ahrs.ltp_to_body_euler.psi);	\
   }
 
-#define PERIODIC_SEND_BOOZ2_AHRS_RMAT(_chan) {				\
-    DOWNLINK_SEND_BOOZ2_AHRS_RMAT(_chan,				\
+#define PERIODIC_SEND_AHRS_RMAT_INT(_trans, _dev) {      \
+    DOWNLINK_SEND_AHRS_RMAT(_trans, _dev,				\
 				  &ahrs.ltp_to_imu_rmat.m[0],	\
 				  &ahrs.ltp_to_imu_rmat.m[1],	\
 				  &ahrs.ltp_to_imu_rmat.m[2],	\
@@ -478,19 +468,10 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 
 
 
-
-#define PERIODIC_SEND_FILTER_Q(_chan) {				\
-    DOWNLINK_SEND_FILTER_Q(_chan,					\
-				 &booz2_filter_attitude_quat.qi,	\
-				 &booz2_filter_attitude_quat.qx,	\
-				 &booz2_filter_attitude_quat.qy,	\
-				 &booz2_filter_attitude_quat.qz);	\
-  }
-
-#ifdef USE_VFF
+#if USE_VFF
 #include "subsystems/ins/vf_float.h"
-#define PERIODIC_SEND_VFF(_chan) {		\
-    DOWNLINK_SEND_VFF(_chan,			\
+#define PERIODIC_SEND_VFF(_trans, _dev) {		\
+    DOWNLINK_SEND_VFF(_trans, _dev,			\
 			    &vff_z_meas,		\
 			    &vff_z,			\
 			    &vff_zdot,		\
@@ -500,13 +481,13 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 			    & vff_P[2][2]);		\
   }
 #else
-#define PERIODIC_SEND_VFF(_chan) {}
+#define PERIODIC_SEND_VFF(_trans, _dev) {}
 #endif
 
-#ifdef USE_HFF
+#if USE_HFF
 #include  "subsystems/ins/hf_float.h"
-#define PERIODIC_SEND_HFF(_chan) {	\
-    DOWNLINK_SEND_HFF(_chan,		\
+#define PERIODIC_SEND_HFF(_trans, _dev) {	\
+    DOWNLINK_SEND_HFF(_trans, _dev,		\
                             &b2_hff_state.x,			\
                             &b2_hff_state.y,			\
                             &b2_hff_state.xdot,         \
@@ -514,8 +495,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
                             &b2_hff_state.xdotdot,      \
                             &b2_hff_state.ydotdot);     \
   }
-#define PERIODIC_SEND_HFF_DBG(_chan) {                \
-	DOWNLINK_SEND_HFF_DBG(_chan,                      \
+#define PERIODIC_SEND_HFF_DBG(_trans, _dev) {                \
+	DOWNLINK_SEND_HFF_DBG(_trans, _dev,                      \
                                 &b2_hff_x_meas,             \
                                 &b2_hff_y_meas,             \
                                 &b2_hff_xd_meas,            \
@@ -526,39 +507,39 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
                                 &b2_hff_state.yP[1][1]);    \
   }
 #ifdef GPS_LAG
-#define PERIODIC_SEND_HFF_GPS(_chan) {	\
-    DOWNLINK_SEND_HFF_GPS(_chan,			\
+#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {	\
+    DOWNLINK_SEND_HFF_GPS(_trans, _dev,			\
 							  &b2_hff_rb_last->lag_counter,		\
 							  &lag_counter_err,	\
 							  &save_counter);	\
   }
 #else
-#define PERIODIC_SEND_HFF_GPS(_chan) {}
+#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {}
 #endif
 #else
-#define PERIODIC_SEND_HFF(_chan) {}
-#define PERIODIC_SEND_HFF_DBG(_chan) {}
-#define PERIODIC_SEND_HFF_GPS(_chan) {}
+#define PERIODIC_SEND_HFF(_trans, _dev) {}
+#define PERIODIC_SEND_HFF_DBG(_trans, _dev) {}
+#define PERIODIC_SEND_HFF_GPS(_trans, _dev) {}
 #endif
 
-#define PERIODIC_SEND_GUIDANCE(_chan) {				\
-    DOWNLINK_SEND_GUIDANCE(_chan,					\
+#define PERIODIC_SEND_GUIDANCE(_trans, _dev) {				\
+    DOWNLINK_SEND_GUIDANCE(_trans, _dev,					\
 				 &guidance_h_cur_pos.x,		\
 				 &guidance_h_cur_pos.y,		\
 				 &guidance_h_held_pos.x,		\
 				 &guidance_h_held_pos.y);		\
   }
 
-#define PERIODIC_SEND_INS_Z(_chan) {				\
-    DOWNLINK_SEND_INS_Z(_chan,					\
+#define PERIODIC_SEND_INS_Z(_trans, _dev) {				\
+    DOWNLINK_SEND_INS_Z(_trans, _dev,					\
 			    &ins_baro_alt,				\
 			    &ins_ltp_pos.z,			\
 			    &ins_ltp_speed.z,			\
 			    &ins_ltp_accel.z);			\
   }
 
-#define PERIODIC_SEND_INS(_chan) {			\
-    DOWNLINK_SEND_INS(_chan,				\
+#define PERIODIC_SEND_INS(_trans, _dev) {			\
+    DOWNLINK_SEND_INS(_trans, _dev,				\
 					   &ins_ltp_pos.x,		\
 					   &ins_ltp_pos.y,	    \
 					   &ins_ltp_pos.z,		\
@@ -570,8 +551,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 					   &ins_ltp_accel.z);	\
   }
 
-#define PERIODIC_SEND_INS_REF(_chan) {				\
-    DOWNLINK_SEND_INS_REF(_chan,					\
+#define PERIODIC_SEND_INS_REF(_trans, _dev) {				\
+    DOWNLINK_SEND_INS_REF(_trans, _dev,					\
 				&ins_ltp_def.ecef.x,		\
 				&ins_ltp_def.ecef.y,		\
 				&ins_ltp_def.ecef.z,		\
@@ -582,8 +563,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				&ins_qfe);				\
   }
 
-#define PERIODIC_SEND_VERT_LOOP(_chan) {				\
-    DOWNLINK_SEND_VERT_LOOP(_chan,				\
+#define PERIODIC_SEND_VERT_LOOP(_trans, _dev) {				\
+    DOWNLINK_SEND_VERT_LOOP(_trans, _dev,				\
 				  &guidance_v_z_sp,		\
 				  &guidance_v_zd_sp,		\
 				  &ins_ltp_pos.z,			\
@@ -601,8 +582,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				  &guidance_v_delta_t);		\
   }
 
-#define PERIODIC_SEND_HOVER_LOOP(_chan) {				\
-    DOWNLINK_SEND_HOVER_LOOP(_chan,				\
+#define PERIODIC_SEND_HOVER_LOOP(_trans, _dev) {				\
+    DOWNLINK_SEND_HOVER_LOOP(_trans, _dev,				\
 				   &guidance_h_pos_sp.x,		\
 				   &guidance_h_pos_sp.y,		\
 				   &ins_ltp_pos.x,			\
@@ -626,8 +607,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				   &guidance_h_command_body.psi);	\
   }
 
-#define PERIODIC_SEND_GUIDANCE_H_REF(_chan) { \
-  DOWNLINK_SEND_GUIDANCE_H_REF_INT(_chan, \
+#define PERIODIC_SEND_GUIDANCE_H_REF(_trans, _dev) { \
+  DOWNLINK_SEND_GUIDANCE_H_REF_INT(_trans, _dev, \
       &guidance_h_pos_sp.x, \
       &guidance_h_pos_ref.x, \
       &guidance_h_speed_ref.x, \
@@ -639,9 +620,9 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 }
 
 #include "firmwares/rotorcraft/navigation.h"
-#define PERIODIC_SEND_ROTORCRAFT_FP(_chan) {					\
+#define PERIODIC_SEND_ROTORCRAFT_FP(_trans, _dev) {					\
     int32_t carrot_up = -guidance_v_z_sp;				\
-    DOWNLINK_SEND_ROTORCRAFT_FP( _chan,					\
+    DOWNLINK_SEND_ROTORCRAFT_FP( _trans, _dev,					\
 			    &ins_enu_pos.x,			\
 			    &ins_enu_pos.y,			\
 			    &ins_enu_pos.z,			\
@@ -659,9 +640,9 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
           &autopilot_flight_time);	\
   }
 
-#ifdef USE_GPS
-#define PERIODIC_SEND_GPS_INT(_chan) {				\
-    DOWNLINK_SEND_GPS_INT( _chan,                   \
+#if USE_GPS
+#define PERIODIC_SEND_GPS_INT(_trans, _dev) {				\
+    DOWNLINK_SEND_GPS_INT( _trans, _dev,                   \
                            &gps.ecef_pos.x,         \
                            &gps.ecef_pos.y,         \
                            &gps.ecef_pos.z,         \
@@ -682,18 +663,18 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
     static uint8_t last_cnos[GPS_NB_CHANNELS];      \
     if (i == gps.nb_channels) i = 0;                \
     if (i < gps.nb_channels && gps.svinfos[i].cno > 0 && gps.svinfos[i].cno != last_cnos[i]) { \
-      DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
+      DOWNLINK_SEND_SVINFO(DefaultChannel, DefaultDevice, &i, &gps.svinfos[i].svid, &gps.svinfos[i].flags, &gps.svinfos[i].qi, &gps.svinfos[i].cno, &gps.svinfos[i].elev, &gps.svinfos[i].azim); \
       last_cnos[i] = gps.svinfos[i].cno;                                \
     }                                                                   \
     i++;                                                                \
   }
 #else
-#define PERIODIC_SEND_GPS_INT(_chan) {}
+#define PERIODIC_SEND_GPS_INT(_trans, _dev) {}
 #endif
 
 #include "firmwares/rotorcraft/navigation.h"
-#define PERIODIC_SEND_ROTORCRAFT_NAV_STATUS(_chan) {				\
-    DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(_chan,                      \
+#define PERIODIC_SEND_ROTORCRAFT_NAV_STATUS(_trans, _dev) {				\
+    DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(_trans, _dev,                      \
 				   &block_time,				\
 				   &stage_time,				\
 				   &nav_block,				\
@@ -704,34 +685,34 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
       float sy = POS_FLOAT_OF_BFP(waypoints[nav_segment_start].y);	\
       float ex = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].x);	\
       float ey = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].y);	\
-      DOWNLINK_SEND_SEGMENT(_chan, &sx, &sy, &ex, &ey);			\
+      DOWNLINK_SEND_SEGMENT(_trans, _dev, &sx, &sy, &ex, &ey);			\
     }									\
     else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {			\
       float cx = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].x);	\
       float cy = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].y);	\
       float r = POS_FLOAT_OF_BFP(nav_circle_radius); \
-      DOWNLINK_SEND_CIRCLE(_chan, &cx, &cy, &r);			\
+      DOWNLINK_SEND_CIRCLE(_trans, _dev, &cx, &cy, &r);			\
     }									\
   }
 
-#define PERIODIC_SEND_WP_MOVED(_chan) {					\
+#define PERIODIC_SEND_WP_MOVED(_trans, _dev) {					\
     static uint8_t i;							\
     i++; if (i >= nb_waypoint) i = 0;					\
-    DOWNLINK_SEND_WP_MOVED_ENU(_chan,					\
+    DOWNLINK_SEND_WP_MOVED_ENU(_trans, _dev,					\
 			       &i,					\
 			       &(waypoints[i].x),			\
 			       &(waypoints[i].y),			\
 			       &(waypoints[i].z));			\
   }
 
-#ifdef USE_CAM
-#define PERIODIC_SEND_BOOZ2_CAM(_chan) DOWNLINK_SEND_BOOZ2_CAM(_chan,&booz_cam_tilt,&booz_cam_pan);
+#if USE_CAM
+#define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) DOWNLINK_SEND_BOOZ2_CAM(_trans, _dev,&booz_cam_tilt,&booz_cam_pan);
 #else
-#define PERIODIC_SEND_BOOZ2_CAM(_chan) {}
+#define PERIODIC_SEND_BOOZ2_CAM(_trans, _dev) {}
 #endif
 
-#define PERIODIC_SEND_BOOZ2_TUNE_HOVER(_chan) {				       \
-    DOWNLINK_SEND_BOOZ2_TUNE_HOVER(_chan,				       \
+#define PERIODIC_SEND_BOOZ2_TUNE_HOVER(_trans, _dev) {				       \
+    DOWNLINK_SEND_BOOZ2_TUNE_HOVER(_trans, _dev,				       \
 				   &radio_control.values[RADIO_ROLL],  \
 				   &radio_control.values[RADIO_PITCH], \
 				   &radio_control.values[RADIO_YAW],   \
@@ -748,8 +729,8 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 				   );					       \
   }
 
-#define PERIODIC_SEND_I2C_ERRORS(_chan) {				       \
-    DOWNLINK_SEND_I2C_ERRORS(_chan,				       \
+#define PERIODIC_SEND_I2C_ERRORS(_trans, _dev) {				       \
+    DOWNLINK_SEND_I2C_ERRORS(_trans, _dev,				       \
 				   &i2c_errc_ack_fail,  \
 				   &i2c_errc_miss_start_stop,  \
 				   &i2c_errc_arb_lost,  \
@@ -762,14 +743,14 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
 
 //TODO replace by BOOZ_EXTRA_ADC
 #ifdef BOOZ2_SONAR
-#define PERIODIC_SEND_BOOZ2_SONAR(_chan) DOWNLINK_SEND_BOOZ2_SONAR(_chan,&booz2_adc_1,&booz2_adc_2,&booz2_adc_3,&booz2_adc_4);
+#define PERIODIC_SEND_BOOZ2_SONAR(_trans, _dev) DOWNLINK_SEND_BOOZ2_SONAR(_trans, _dev,&booz2_adc_1,&booz2_adc_2,&booz2_adc_3,&booz2_adc_4);
 #else
-#define PERIODIC_SEND_BOOZ2_SONAR(_chan) {}
+#define PERIODIC_SEND_BOOZ2_SONAR(_trans, _dev) {}
 #endif
 
 #ifdef BOOZ2_TRACK_CAM
 #include "cam_track.h"
-#define PERIODIC_SEND_CAM_TRACK(_chan) DOWNLINK_SEND_BOOZ_SIM_SPEED_POS(_chan, \
+#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) DOWNLINK_SEND_BOOZ_SIM_SPEED_POS(_trans, _dev, \
     &target_accel_ned.x, \
     &target_accel_ned.y, \
     &target_accel_ned.z, \
@@ -780,15 +761,15 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
     &target_pos_ned.y, \
     &target_pos_ned.z)
 #else
-#define PERIODIC_SEND_CAM_TRACK(_chan) {}
+#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) {}
 #endif
 
 #include "generated/settings.h"
-#define PERIODIC_SEND_DL_VALUE(_chan) PeriodicSendDlValue(_chan)
+#define PERIODIC_SEND_DL_VALUE(_trans, _dev) PeriodicSendDlValue(_trans, _dev)
 
 #include "generated/periodic.h"
-#define Booz2TelemetryPeriodic() {			\
-    PeriodicSendMain(DefaultChannel);			\
+#define TelemetryPeriodic() {			\
+    PeriodicSendMain(DefaultChannel,DefaultDevice);			\
   }
 
 
diff --git a/sw/airborne/firmwares/setup/setup_actuators.c b/sw/airborne/firmwares/setup/setup_actuators.c
index 7404d02b18..0d73eb8007 100644
--- a/sw/airborne/firmwares/setup/setup_actuators.c
+++ b/sw/airborne/firmwares/setup/setup_actuators.c
@@ -6,11 +6,11 @@
 //#include "actuators.h"
 #include "generated/airframe.h"
 #define DATALINK_C
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "mcu_periph/uart.h"
-#include "pprz_transport.h"
+#include "subsystems/datalink/pprz_transport.h"
 #include "firmwares/fixedwing/main_fbw.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "generated/settings.h"
 
 
@@ -35,11 +35,11 @@ void dl_parse_msg( void ) {
     for (int j=0 ; j<8 ; j++) {
       SetServo(j,actuators[j]);
     }
-    DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
+    DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
   } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
     uint8_t i = DL_GET_SETTING_index(dl_buffer);
     float val = settings_get_value(i);
-    DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
+    DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
   }
 #endif
 }
@@ -71,8 +71,8 @@ void periodic_task_fbw(void) {
    /* uint16_t servo_value = 1500+ 500*sin(t); */
    /* SetServo(SERVO_THROTTLE, servo_value); */
 
-  RunOnceEvery(300, DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM));
-  RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, SERVOS_NB, actuators ));
+  RunOnceEvery(300, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM));
+  RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, SERVOS_NB, actuators ));
 }
 
 void event_task_fbw(void) {
diff --git a/sw/airborne/firmwares/tutorial/main_demo4.c b/sw/airborne/firmwares/tutorial/main_demo4.c
index 83352b5faf..8f61cd312f 100644
--- a/sw/airborne/firmwares/tutorial/main_demo4.c
+++ b/sw/airborne/firmwares/tutorial/main_demo4.c
@@ -5,7 +5,7 @@
 #include "mcu_periph/uart.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
diff --git a/sw/airborne/firmwares/tutorial/main_demo5.c b/sw/airborne/firmwares/tutorial/main_demo5.c
index 5cb2b2c430..52754d2849 100644
--- a/sw/airborne/firmwares/tutorial/main_demo5.c
+++ b/sw/airborne/firmwares/tutorial/main_demo5.c
@@ -5,7 +5,7 @@
 #include "mcu_periph/uart.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
diff --git a/sw/airborne/firmwares/tutorial/main_demo6.c b/sw/airborne/firmwares/tutorial/main_demo6.c
index dd312de7a2..68b18be2cc 100644
--- a/sw/airborne/firmwares/tutorial/main_demo6.c
+++ b/sw/airborne/firmwares/tutorial/main_demo6.c
@@ -6,7 +6,7 @@
 #include "mcu_periph/usb_serial.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
diff --git a/sw/airborne/firmwares/vor/lpc_vor_main.c b/sw/airborne/firmwares/vor/lpc_vor_main.c
index 818f23e394..418e5dbc7e 100644
--- a/sw/airborne/firmwares/vor/lpc_vor_main.c
+++ b/sw/airborne/firmwares/vor/lpc_vor_main.c
@@ -6,7 +6,7 @@
 #include "mcu_periph/uart.h"
 #include "print.h"
 //#include "messages.h"
-//#include "downlink.h"
+//#include "subsystems/datalink/downlink.h"
 
 #include "lpc_vor_convertions.h"
 #include "vor_int_demod_decim.h"
diff --git a/sw/airborne/firmwares/wind_tunnel/main.c b/sw/airborne/firmwares/wind_tunnel/main.c
index e32d4f3441..1d40c231ba 100644
--- a/sw/airborne/firmwares/wind_tunnel/main.c
+++ b/sw/airborne/firmwares/wind_tunnel/main.c
@@ -6,9 +6,9 @@
 #include "mcu_periph/uart.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "generated/settings.h"
 #include "dl_protocol.h"
 
diff --git a/sw/airborne/firmwares/wind_tunnel/main_mb.c b/sw/airborne/firmwares/wind_tunnel/main_mb.c
index 437e0fab32..f9b221842a 100644
--- a/sw/airborne/firmwares/wind_tunnel/main_mb.c
+++ b/sw/airborne/firmwares/wind_tunnel/main_mb.c
@@ -6,9 +6,9 @@
 #include "mcu_periph/uart.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "generated/settings.h"
 #include "dl_protocol.h"
 
diff --git a/sw/airborne/fms/fms_test_datalink.c b/sw/airborne/fms/fms_test_datalink.c
index 13ff9f7e4f..d93fe1956e 100644
--- a/sw/airborne/fms/fms_test_datalink.c
+++ b/sw/airborne/fms/fms_test_datalink.c
@@ -11,7 +11,7 @@
 #define DATALINK_PORT 4243
 #include "udp_transport.h"
 #include "fms_network.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #define PERIODIC_SEC       0
 #define PERIODIC_USEC   50000
diff --git a/sw/airborne/fms/overo_test_periodic.c b/sw/airborne/fms/overo_test_periodic.c
index 3ce6637739..e3c5028992 100644
--- a/sw/airborne/fms/overo_test_periodic.c
+++ b/sw/airborne/fms/overo_test_periodic.c
@@ -15,7 +15,7 @@
 #include "generated/airframe.h"
 
 #include "fms_periodic.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "udp_transport.h"
 #include "fms_network.h"
 #include "fms_spi_link.h"
diff --git a/sw/airborne/fms/overo_test_telemetry.c b/sw/airborne/fms/overo_test_telemetry.c
index dd20b8b1c8..482a21bce2 100644
--- a/sw/airborne/fms/overo_test_telemetry.c
+++ b/sw/airborne/fms/overo_test_telemetry.c
@@ -6,7 +6,7 @@
 #include <event.h>
 #include <evutil.h>
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "udp_transport.h"
 #include "fms_network.h"
 
diff --git a/sw/airborne/fms/test_telemetry_2.c b/sw/airborne/fms/test_telemetry_2.c
index 42665c0a50..2bed7c5ec3 100644
--- a/sw/airborne/fms/test_telemetry_2.c
+++ b/sw/airborne/fms/test_telemetry_2.c
@@ -7,7 +7,7 @@
 #include <evutil.h>
 
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "udp_transport.h"
 #include "fms_network.h"
 
diff --git a/sw/airborne/led.h b/sw/airborne/led.h
index 55a9c0493e..802056f216 100644
--- a/sw/airborne/led.h
+++ b/sw/airborne/led.h
@@ -23,7 +23,7 @@
  */
 
 /** \file led.h
- *  \brief arch independant LED (Light Emitting Diodes) API
+ *  \brief arch independent LED (Light Emitting Diodes) API
  *
  *
  */
diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c
index 96a5c5e13c..0b2ead0157 100644
--- a/sw/airborne/lisa/lisa_stm_passthrough_main.c
+++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c
@@ -25,7 +25,7 @@
 #include "mcu.h"
 #include "mcu_periph/uart.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "firmwares/rotorcraft/commands.h"
 #include "actuators.h"
 #include "actuators/actuators_pwm.h"
@@ -165,10 +165,10 @@ static inline void main_periodic(void) {
 
 	RunOnceEvery(10, {
 			LED_PERIODIC();
-			DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+			DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
 			radio_control_periodic();
 			check_radio_lost();
-			DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);
+			DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential);
 		});
 
 	RunOnceEvery(2, {baro_periodic();});
@@ -179,7 +179,7 @@ static inline void main_periodic(void) {
 		v1 = adc0_buf.sum / adc0_buf.av_nb_sample;
 		v2 = adc1_buf.values[0];
 
-		RunOnceEvery(10, { DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &v1, &v2) });
+		RunOnceEvery(10, { DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &v1, &v2) });
 	}
 }
 
@@ -318,7 +318,7 @@ static inline void on_mag_event(void) {
 static inline void on_vane_msg(void *data) {
 	new_vane = TRUE;
 	int zero = 0;
-	DOWNLINK_SEND_VANE_SENSOR(DefaultChannel,
+	DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, DefaultDevice,
 				&(csc_vane_msg.vane_angle1),
 				&zero,
 				&zero,
diff --git a/sw/airborne/lisa/test/hs_gyro.c b/sw/airborne/lisa/test/hs_gyro.c
index 544e4579c2..81edad1f59 100644
--- a/sw/airborne/lisa/test/hs_gyro.c
+++ b/sw/airborne/lisa/test/hs_gyro.c
@@ -29,7 +29,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/imu.h"
 
@@ -70,7 +70,7 @@ static inline void main_init( void ) {
 static inline void main_periodic_task( void ) {
   RunOnceEvery(100, {
       LED_TOGGLE(3);
-      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
     });
   imu_periodic();
   RunOnceEvery(10, { LED_PERIODIC();});
@@ -101,11 +101,11 @@ static inline void on_gyro_accel_event(void) {
   if (cnt > NB_SAMPLES) cnt = 0;
   samples[cnt] = imu.MEASURED_SENSOR;
   if (cnt == NB_SAMPLES-1) {
-    DOWNLINK_SEND_IMU_HS_GYRO(DefaultChannel, &axis, NB_SAMPLES, samples);
+    DOWNLINK_SEND_IMU_HS_GYRO(DefaultChannel, DefaultDevice, &axis, NB_SAMPLES, samples);
   }
 
   if (cnt == 10) {
-    DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
+    DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
 			       &imu.gyro_unscaled.p,
 			       &imu.gyro_unscaled.q,
 			       &imu.gyro_unscaled.r);
diff --git a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
index 50ad516aad..54fe479a4f 100644
--- a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
+++ b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
@@ -26,7 +26,7 @@
 #include "sys_time.h"
 #include "firmwares/rotorcraft/commands.h"
 #include "actuators.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "actuators/actuators_asctec.h"
 
@@ -62,7 +62,7 @@ static inline void main_periodic_task( void ) {
 
   RunOnceEvery(100, {
       LED_TOGGLE(3);
-      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
     });
 
 
diff --git a/sw/airborne/lisa/test/lisa_test_adxl345.c b/sw/airborne/lisa/test/lisa_test_adxl345.c
index 6b8c65ca58..b76739b778 100644
--- a/sw/airborne/lisa/test/lisa_test_adxl345.c
+++ b/sw/airborne/lisa/test/lisa_test_adxl345.c
@@ -37,7 +37,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "peripherals/adxl345.h"
 #include "my_debug_servo.h"
@@ -148,7 +148,7 @@ static inline void main_periodic_task( void ) {
 
   RunOnceEvery(10,
     {
-      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
       LED_PERIODIC();
     });
 
@@ -197,7 +197,7 @@ static inline void main_event_task( void ) {
     int32_t iax = *((int16_t*)&values[0]);
     int32_t iay = *((int16_t*)&values[2]);
     int32_t iaz = *((int16_t*)&values[4]);
-    RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, &iax, &iay, &iaz);});
+    RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &iax, &iay, &iaz);});
   }
 
 }
diff --git a/sw/airborne/lisa/test/lisa_test_adxl345_dma.c b/sw/airborne/lisa/test/lisa_test_adxl345_dma.c
index fa74e5018a..002e6f776b 100644
--- a/sw/airborne/lisa/test/lisa_test_adxl345_dma.c
+++ b/sw/airborne/lisa/test/lisa_test_adxl345_dma.c
@@ -31,7 +31,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "peripherals/adxl345.h"
 #include "my_debug_servo.h"
@@ -161,7 +161,7 @@ static inline void main_periodic_task( void ) {
 
   RunOnceEvery(10,
     {
-      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
       LED_PERIODIC();
     });
 
@@ -197,7 +197,7 @@ static inline void main_event_task( void ) {
     int32_t iax = ax;
     int32_t iay = ay;
     int32_t iaz = az;
-    RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, &iax, &iay, &iaz);});
+    RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &iax, &iay, &iaz);});
   }
 
 }
diff --git a/sw/airborne/lisa/test/lisa_test_aspirin.c b/sw/airborne/lisa/test/lisa_test_aspirin.c
index 2641273fe2..e845219576 100644
--- a/sw/airborne/lisa/test/lisa_test_aspirin.c
+++ b/sw/airborne/lisa/test/lisa_test_aspirin.c
@@ -30,7 +30,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "peripherals/itg3200.h"
 #include "peripherals/hmc5843.h"
@@ -80,7 +80,7 @@ static inline void main_periodic_task( void ) {
   //  LED_TOGGLE(6);
   RunOnceEvery(10,
 	       {
-		 DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+		 DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
 		 LED_PERIODIC();
 	       });
 
diff --git a/sw/airborne/lisa/test/lisa_test_hmc5843.c b/sw/airborne/lisa/test/lisa_test_hmc5843.c
index b21d0c84e9..959e42aece 100644
--- a/sw/airborne/lisa/test/lisa_test_hmc5843.c
+++ b/sw/airborne/lisa/test/lisa_test_hmc5843.c
@@ -36,7 +36,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "std.h"
 
 #include "mcu_periph/uart.h"
@@ -84,12 +84,12 @@ static inline void main_periodic_task( void ) {
   //  LED_TOGGLE(6);
   RunOnceEvery(10,
   {
-    DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+    DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
     LED_PERIODIC();
   });
   RunOnceEvery(256,
     {
-      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
 			       &i2c2_errors.ack_fail_cnt,
 			       &i2c2_errors.miss_start_stop_cnt,
 			       &i2c2_errors.arb_lost_cnt,
@@ -165,10 +165,10 @@ static inline void main_event_task( void ) {
       int16_t mz   = i2c_trans.buf[4]<<8 | i2c_trans.buf[5];
       struct Int32Vect3 m;
       VECT3_ASSIGN(m, mx, my, mz);
-      DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, &m.x, &m.y, &m.z);
+      DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &m.x, &m.y, &m.z);
       //      uint8_t tmp[8];
       //      memcpy(tmp, i2c2.buf, 8);
-      //      DOWNLINK_SEND_DEBUG(DefaultChannel, 8, tmp);
+      //      DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 8, tmp);
     }
 		 );
     reading_mag = FALSE;
diff --git a/sw/airborne/lisa/test/lisa_test_itg3200.c b/sw/airborne/lisa/test/lisa_test_itg3200.c
index 66848cb408..bc6138507f 100644
--- a/sw/airborne/lisa/test/lisa_test_itg3200.c
+++ b/sw/airborne/lisa/test/lisa_test_itg3200.c
@@ -39,7 +39,7 @@
 #include "mcu_periph/uart.h"
 #include "mcu_periph/i2c.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "std.h"
 #include "math/pprz_algebra_int.h"
 
@@ -82,11 +82,11 @@ static inline void main_periodic_task( void ) {
   //  LED_TOGGLE(6);
   RunOnceEvery(10,
   {
-    DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+    DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
     LED_PERIODIC();
   });
   RunOnceEvery(256, {
-   DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+   DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
 			    &i2c2_errors.ack_fail_cnt,
 			    &i2c2_errors.miss_start_stop_cnt,
 			    &i2c2_errors.arb_lost_cnt,
@@ -205,11 +205,11 @@ static inline void main_event_task( void ) {
     RATES_ASSIGN(g, tgp, tgq, tgr);
     RunOnceEvery(10,
     {
-      DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, &g.p, &g.q, &g.r);
+      DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &g.p, &g.q, &g.r);
 
       uint8_t tmp[8];
       memcpy(tmp, i2c_trans.buf, 8);
-      DOWNLINK_SEND_DEBUG(DefaultChannel, 8, tmp);
+      DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 8, tmp);
 
 
     });
diff --git a/sw/airborne/lisa/test/lisa_test_max1168.c b/sw/airborne/lisa/test/lisa_test_max1168.c
index 38a6cc67bc..82b2d161dc 100644
--- a/sw/airborne/lisa/test/lisa_test_max1168.c
+++ b/sw/airborne/lisa/test/lisa_test_max1168.c
@@ -31,7 +31,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "peripherals/max1168.h"
 
 static inline void main_init( void );
@@ -65,7 +65,7 @@ static inline void main_periodic_task( void ) {
   max1168_read();
   RunOnceEvery(10,
 	       {
-		 DOWNLINK_SEND_BOOT(DefaultChannel, &cpu_time_sec);
+		 DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &cpu_time_sec);
 		 LED_PERIODIC();
 	       });
 
@@ -74,9 +74,9 @@ static inline void main_periodic_task( void ) {
 static inline void main_event_task( void ) {
   if (max1168_status == STA_MAX1168_DATA_AVAILABLE) {
     RunOnceEvery(10, {
-	DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, &max1168_values[0], &max1168_values[1], &max1168_values[2]);
-	DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, &max1168_values[3], &max1168_values[4], &max1168_values[6]);
-	//	DOWNLINK_SEND_BOOT(DefaultChannel, &max1168_values[7]); });
+	DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &max1168_values[0], &max1168_values[1], &max1168_values[2]);
+	DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &max1168_values[3], &max1168_values[4], &max1168_values[6]);
+	//	DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &max1168_values[7]); });
       });
     max1168_status = STA_MAX1168_IDLE;
   }
diff --git a/sw/airborne/lisa/test/lisa_test_ms2100.c b/sw/airborne/lisa/test/lisa_test_ms2100.c
index 1bf869b3fa..7e3089be96 100644
--- a/sw/airborne/lisa/test/lisa_test_ms2100.c
+++ b/sw/airborne/lisa/test/lisa_test_ms2100.c
@@ -31,7 +31,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "peripherals/ms2100.h"
 
 static inline void main_init( void );
@@ -63,7 +63,7 @@ static inline void main_init( void ) {
 static inline void main_periodic_task( void ) {
   RunOnceEvery(10,
 	       {
-		 DOWNLINK_SEND_BOOT(DefaultChannel, &cpu_time_sec);
+		 DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &cpu_time_sec);
 		 LED_PERIODIC();
 	       });
 
@@ -82,7 +82,7 @@ static inline void main_periodic_task( void ) {
 static inline void main_event_task( void ) {
   if (ms2100_status == MS2100_DATA_AVAILABLE) {
     RunOnceEvery(10, {
-	DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+	DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
 				  &ms2100_values[0],
 				  &ms2100_values[1],
 				  &ms2100_values[2]);
diff --git a/sw/airborne/lisa/test/lisa_test_sc18is600.c b/sw/airborne/lisa/test/lisa_test_sc18is600.c
index 846e4cab96..385b3cc792 100644
--- a/sw/airborne/lisa/test/lisa_test_sc18is600.c
+++ b/sw/airborne/lisa/test/lisa_test_sc18is600.c
@@ -28,7 +28,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "peripherals/sc18is600.h"
 
 static inline void main_init( void );
@@ -61,7 +61,7 @@ static inline void main_periodic_task( void ) {
   //  LED_TOGGLE(6);
   RunOnceEvery(10,
 	       {
-		 DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+		 DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
 		 LED_PERIODIC();
 	       });
 
diff --git a/sw/airborne/lisa/test/test_board.c b/sw/airborne/lisa/test/test_board.c
index d1fab3a701..32ac998173 100644
--- a/sw/airborne/lisa/test/test_board.c
+++ b/sw/airborne/lisa/test/test_board.c
@@ -32,10 +32,10 @@
 #include "mcu.h"
 #include "mcu_periph/uart.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "led.h"
 
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "generated/settings.h"
 
 #include "lisa/lisa_baro.h"
@@ -113,7 +113,7 @@ static inline void main_init( void ) {
 static inline void main_periodic_task( void ) {
 
   LED_PERIODIC();
-  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
 
   tests[cur_test]._periodic();
 
@@ -156,7 +156,7 @@ static void test_baro_start(void) {all_led_green();}
 static void test_baro_periodic(void) {
   RunOnceEvery(2, {baro_periodic();});
   RunOnceEvery(100,{
-      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
                    &i2c2_errors.ack_fail_cnt,
                    &i2c2_errors.miss_start_stop_cnt,
                    &i2c2_errors.arb_lost_cnt,
@@ -170,10 +170,10 @@ static void test_baro_periodic(void) {
 }
 static void test_baro_event(void) {BaroEvent(test_baro_on_baro_abs, test_baro_on_baro_diff);}
 static inline void test_baro_on_baro_abs(void) {
-  RunOnceEvery(5,{DOWNLINK_SEND_BOOZ_BARO2_RAW(DefaultChannel, &baro.abs_raw, &baro.diff_raw);});
+  RunOnceEvery(5,{DOWNLINK_SEND_BOOZ_BARO2_RAW(DefaultChannel, DefaultDevice, &baro.abs_raw, &baro.diff_raw);});
 }
 static inline void test_baro_on_baro_diff(void) {
-  RunOnceEvery(5,{DOWNLINK_SEND_BOOZ_BARO2_RAW(DefaultChannel, &baro.abs_raw, &baro.diff_raw);});
+  RunOnceEvery(5,{DOWNLINK_SEND_BOOZ_BARO2_RAW(DefaultChannel, DefaultDevice, &baro.abs_raw, &baro.diff_raw);});
 }
 
 
@@ -190,7 +190,7 @@ static void test_bldc_periodic(void) {
   i2c1_transmit(0x58, 1, NULL);
 
   RunOnceEvery(100,{
-      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
                    &i2c1_errors.ack_fail_cnt,
                    &i2c1_errors.miss_start_stop_cnt,
                    &i2c1_errors.arb_lost_cnt,
@@ -260,7 +260,7 @@ static void test_uart_event(void) {
   if (Uart3ChAvailable()) {
     buf_dest[idx_rx] = Uart3Getch();
     if (idx_rx<sizeof(buf_src)) {
-      DOWNLINK_SEND_DEBUG(DefaultChannel, sizeof(buf_src), buf_dest);
+      DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, sizeof(buf_src), buf_dest);
       idx_rx++;
       if (idx_rx == sizeof(buf_src)) {
     if ( memcmp(buf_dest, buf_src, sizeof(buf_src)) ) {
@@ -278,7 +278,7 @@ static void test_uart_event(void) {
   if (Uart1ChAvailable()) {
     buf_dest[idx_rx] = Uart1Getch();
     if (idx_rx<sizeof(buf_src)) {
-      DOWNLINK_SEND_DEBUG(DefaultChannel, sizeof(buf_src), buf_dest);
+      DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, sizeof(buf_src), buf_dest);
       idx_rx++;
       if (idx_rx == sizeof(buf_src)) {
     if ( memcmp(buf_dest, buf_src, sizeof(buf_src)) ) {
@@ -330,7 +330,7 @@ void dl_parse_msg(void) {
 
   case  DL_PING:
     {
-      DOWNLINK_SEND_PONG(DefaultChannel);
+      DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
     }
     break;
 
@@ -340,7 +340,7 @@ void dl_parse_msg(void) {
       uint8_t i = DL_SETTING_index(dl_buffer);
       float var = DL_SETTING_value(dl_buffer);
       DlSetting(i, var);
-      DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &var);
+      DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
     }
     break;
 
diff --git a/sw/airborne/lisa/test/test_mc_asctec_v1_simple.c b/sw/airborne/lisa/test/test_mc_asctec_v1_simple.c
index b83160e704..f8dad77388 100644
--- a/sw/airborne/lisa/test/test_mc_asctec_v1_simple.c
+++ b/sw/airborne/lisa/test/test_mc_asctec_v1_simple.c
@@ -30,7 +30,7 @@
 
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
@@ -60,10 +60,10 @@ static inline void main_init( void ) {
 static inline void main_periodic_task( void ) {
 
 
-  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
   RunOnceEvery(256,
     {
-      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
 			       &i2c1_errors.ack_fail_cnt,
 			       &i2c1_errors.miss_start_stop_cnt,
 			       &i2c1_errors.arb_lost_cnt,
diff --git a/sw/airborne/lisa/test_adc.c b/sw/airborne/lisa/test_adc.c
index 2666e8f0bb..f7299bd9e5 100644
--- a/sw/airborne/lisa/test_adc.c
+++ b/sw/airborne/lisa/test_adc.c
@@ -31,7 +31,7 @@
 #include "led.h"
 #include "mcu_periph/adc.h"
 #include "mcu_periph/uart.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 int main_periodic(void);
 static inline void main_init( void );
@@ -68,8 +68,8 @@ int main( void ) {
 }
 
 static inline void main_periodic_task( void ) {
-  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
-  RunOnceEvery(100, {/*LED_TOGGLE(7);*/ DOWNLINK_SEND_TIME(DefaultChannel, &cpu_time_sec);});
+  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
+  RunOnceEvery(100, {/*LED_TOGGLE(7);*/ DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &cpu_time_sec);});
   LED_PERIODIC();
 }
 
@@ -82,7 +82,7 @@ static inline void main_event_task( void ) {
     //    v1 = (((adc0_buf.values[0])));
     v1 = adc0_buf.sum/adc0_buf.av_nb_sample;
     v2 = (((adc3_buf.values[0])));
-    RunOnceEvery(100, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &v1, &v2)});
+    RunOnceEvery(100, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &v1, &v2)});
   }
 
 }
diff --git a/sw/airborne/lisa/test_csc_servo.c b/sw/airborne/lisa/test_csc_servo.c
deleted file mode 100644
index 7249d89ec4..0000000000
--- a/sw/airborne/lisa/test_csc_servo.c
+++ /dev/null
@@ -1,133 +0,0 @@
-/*
- * $Id:$
- *
- * Copyright (C) 2010 Piotr Esden-Tempski <piotr@esden.net>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-
-#include "mcu.h"
-#include "sys_time.h"
-#include "csc_msg_def.h"
-#include "csc_protocol.h"
-#include "stm32/can.h"
-#include "downlink.h"
-
-static inline void main_init( void );
-static inline void main_periodic_task( void );
-static inline void main_event_task( void );
-
-uint16_t servos[4];
-
-FlagStatus can_error_warning = RESET;
-FlagStatus can_error_passive = RESET;
-FlagStatus can_bus_off = RESET;
-
-struct CscVaneMsg csc_vane_msg;
-
-void main_on_vane_msg(void *data);
-
-int main(void) {
-  main_init();
-
-  servos[0] = 1;
-  servos[1] = 2;
-  servos[2] = 3;
-  servos[3] = 4;
-
-  while(1) {
-	  if (sys_time_periodic())
-		  main_periodic_task();
-	  main_event_task();
-  }
-
-  return 0;
-}
-
-static inline void main_init( void ) {
-	hw_init();
-	sys_time_init();
-	cscp_init();
-	cscp_register_callback(CSC_VANE_MSG_ID, main_on_vane_msg, (void *)&csc_vane_msg);
-}
-
-static inline void main_periodic_task( void ) {
-	servos[0]+=10;
-	servos[1]+=10;
-	servos[2]+=10;
-	servos[3]+=10;
-
-	if ((can_error_warning = CAN_GetFlagStatus(CAN1, CAN_FLAG_EWG)) == SET) {
-		LED_ON(2);
-	} else {
-		LED_OFF(2);
-	}
-	if ((can_error_passive = CAN_GetFlagStatus(CAN1, CAN_FLAG_EPV)) == SET) {
-		LED_ON(3);
-	} else {
-		LED_OFF(3);
-	}
-	if ((can_bus_off = CAN_GetFlagStatus(CAN1, CAN_FLAG_BOF)) == SET) {
-		LED_ON(0);
-	} else {
-		LED_OFF(0);
-	}
-
-	cscp_transmit(0, 0, (uint8_t *)servos, 8);
-
-	LED_PERIODIC();
-	DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
-}
-
-
-
-static inline void main_event_task( void ) {
-	cscp_event();
-
-	LED_OFF(0);
-	LED_OFF(1);
-	LED_OFF(2);
-	LED_OFF(3);
-
-//	DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
-}
-
-void main_on_vane_msg(void *data)
-{
-	int zero = 0;
-
-//	DOWNLINK_SEND_PONG(DefaultChannel);
-
-//	RunOnceEvery(10, {
-			DOWNLINK_SEND_VANE_SENSOR(DefaultChannel,
-																&(csc_vane_msg.vane_angle1),
-																&zero,
-																&zero,
-																&zero,
-																&zero,
-																&csc_vane_msg.vane_angle2,
-																&zero,
-																&zero,
-																&zero,
-																&zero);
-//	});
-}
-
-
-
diff --git a/sw/airborne/lisa/test_datalink.c b/sw/airborne/lisa/test_datalink.c
index ce5a02f314..92e321d7c1 100644
--- a/sw/airborne/lisa/test_datalink.c
+++ b/sw/airborne/lisa/test_datalink.c
@@ -26,9 +26,9 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 
 static inline void main_init( void );
 static inline void main_periodic( void );
@@ -52,7 +52,7 @@ static inline void main_init( void ) {
 }
 
 static inline void main_periodic( void ) {
-  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel,  16, MD5SUM);});
+  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice,  16, MD5SUM);});
 }
 
 static inline void main_event( void ) {
@@ -67,7 +67,7 @@ void dl_parse_msg(void) {
 
   case  DL_PING:
     {
-      DOWNLINK_SEND_PONG(DefaultChannel);
+      DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
     }
     break;
   }
diff --git a/sw/airborne/lisa/test_float.c b/sw/airborne/lisa/test_float.c
index 0339888cac..9e60970686 100644
--- a/sw/airborne/lisa/test_float.c
+++ b/sw/airborne/lisa/test_float.c
@@ -26,7 +26,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 static inline void main_init( void );
 static inline void main_periodic( void );
 
@@ -58,11 +58,11 @@ static inline void main_periodic( void ) {
   float i = sqrt(f);  // ok
   //float i = powf(f1, f1); // nok
   //float i = atan2(f, f); // ok
-  RunOnceEvery(10, {DOWNLINK_SEND_TEST_FORMAT(DefaultChannel, &d1, &i);});
+  RunOnceEvery(10, {DOWNLINK_SEND_TEST_FORMAT(DefaultChannel, DefaultDevice, &d1, &i);});
 
   uint16_t  blaaa = f+d;
 
-  RunOnceEvery(10, {DOWNLINK_SEND_BOOT(DefaultChannel, &blaaa);});
+  RunOnceEvery(10, {DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &blaaa);});
   LED_PERIODIC();
 }
 
diff --git a/sw/airborne/lisa/test_spi_slave.c b/sw/airborne/lisa/test_spi_slave.c
index d78e32ccdd..4c5ef4c3b8 100644
--- a/sw/airborne/lisa/test_spi_slave.c
+++ b/sw/airborne/lisa/test_spi_slave.c
@@ -28,7 +28,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
@@ -65,7 +65,7 @@ static inline void main_periodic_task( void ) {
     GPIOC->BSRR = GPIO_Pin_4;
   foo = !foo;
 #endif
-  RunOnceEvery(10, {DOWNLINK_SEND_BOOT(DefaultChannel, &cpu_time_sec);});
+  RunOnceEvery(10, {DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &cpu_time_sec);});
   LED_PERIODIC();
 }
 
@@ -129,7 +129,7 @@ void spi1_irq_handler(void) {
   SPI_I2S_SendData(SPI1, cnt);
   cnt++;
   LED_TOGGLE(3);
-  DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, &foo, &foo, &cnt);
+  DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, DefaultDevice, &foo, &foo, &cnt);
 
 
 }
diff --git a/sw/airborne/lisa/test_spi_slave2.c b/sw/airborne/lisa/test_spi_slave2.c
index 09bdee54e5..4ff988259a 100644
--- a/sw/airborne/lisa/test_spi_slave2.c
+++ b/sw/airborne/lisa/test_spi_slave2.c
@@ -29,7 +29,7 @@
 #include BOARD_CONFIG
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "fms/fms_autopilot_msg.h"
 
 static inline void main_init( void );
@@ -67,7 +67,7 @@ static inline void main_init( void ) {
 static inline void main_periodic_task( void ) {
   RunOnceEvery(10,
 	       {
-		 DOWNLINK_SEND_BOOT(DefaultChannel, &cpu_time_sec);
+		 DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &cpu_time_sec);
 		 LED_PERIODIC();
 	       });
 
@@ -78,7 +78,7 @@ static inline void main_event_task( void ) {
 #ifdef USE_DMA
   if (DMA_GetFlagStatus(DMA1_FLAG_TC2)) {
     LED_TOGGLE(3);
-    RunOnceEvery(10, {DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, &SPI_SLAVE_Buffer_Rx[0],
+    RunOnceEvery(10, {DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, DefaultDevice, &SPI_SLAVE_Buffer_Rx[0],
 						    &SPI_SLAVE_Buffer_Rx[1], &SPI_SLAVE_Buffer_Rx[2]);});
     memcpy(SPI_SLAVE_Buffer_Tx, SPI_SLAVE_Buffer_Rx, BufferSize);
     main_setup_dma();
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
index 1e935fe12b..9bc9b54c8a 100644
--- a/sw/airborne/math/pprz_algebra.h
+++ b/sw/airborne/math/pprz_algebra.h
@@ -214,6 +214,20 @@
     (_vo).z = ABS((_vi).z);   \
   }
 
+#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2) {        \
+    (_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y;    \
+    (_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z;    \
+    (_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x;    \
+  }
+
+#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) {    \
+    (_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y;    \
+    (_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z;    \
+    (_vo).z = (_r1).p*(_v2).y - (_r1).q*(_v2).x;    \
+  }
+
+
+
 
 /*
  * Euler angles
@@ -434,7 +448,12 @@
     }									\
   }
 
-
+/* set _row of _mat with _vin multiplied by scalar _s */
+#define MAT33_ROW_VECT3_SMUL(_mat, _row, _vin, _s) {         \
+    MAT33_ELMT((_mat), _row, 0) = (_vin).x * (_s);           \
+    MAT33_ELMT((_mat), _row, 1) = (_vin).y * (_s);           \
+    MAT33_ELMT((_mat), _row, 2) = (_vin).z * (_s);           \
+  }
 
 
 //
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index 5a31345f6c..af0d7c3fd6 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -455,21 +455,26 @@ static inline float renorm_factor(float n) {
 
 static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
 
-  struct FloatVect3* r0 = (struct FloatVect3*)(&RMAT_ELMT(*rm, 0,0));
-  struct FloatVect3* r1 = (struct FloatVect3*)(&RMAT_ELMT(*rm, 1,0));
-  struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(*rm, 2,0));
-  float _err = -0.5*FLOAT_VECT3_DOT_PRODUCT(*r0, *r1);
+  const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0,0),
+                                RMAT_ELMT(*rm, 0,1),
+                                RMAT_ELMT(*rm, 0,2)};
+  const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1,0),
+                                RMAT_ELMT(*rm, 1,1),
+                                RMAT_ELMT(*rm, 1,2)};
+  float _err = -0.5*FLOAT_VECT3_DOT_PRODUCT(r0, r1);
   struct FloatVect3 r0_t;
-  VECT3_SUM_SCALED(r0_t, *r0, *r1, _err);
-  VECT3_SUM_SCALED(*r1,  *r1, *r0, _err);
-  VECT3_COPY(*r0, r0_t);
-  FLOAT_VECT3_CROSS_PRODUCT(*r2, *r0, *r1);
-  float s = renorm_factor(FLOAT_VECT3_NORM2(*r0));
-  FLOAT_VECT3_SMUL(*r0, *r0, s);
-  s = renorm_factor(FLOAT_VECT3_NORM2(*r1));
-  FLOAT_VECT3_SMUL(*r1, *r1, s);
-  s = renorm_factor(FLOAT_VECT3_NORM2(*r2));
-  FLOAT_VECT3_SMUL(*r2, *r2, s);
+  VECT3_SUM_SCALED(r0_t, r0, r1, _err);
+  struct FloatVect3 r1_t;
+  VECT3_SUM_SCALED(r1_t,  r1, r0, _err);
+  struct FloatVect3 r2_t;
+  FLOAT_VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
+  float s = renorm_factor(FLOAT_VECT3_NORM2(r0_t));
+  MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
+  s = renorm_factor(FLOAT_VECT3_NORM2(r1_t));
+  MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
+  s = renorm_factor(FLOAT_VECT3_NORM2(r2_t));
+  MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
+
   return _err;
 
 }
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 0e1cff56dd..1a4c3ae322 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -311,10 +311,10 @@ struct Int64Vect3 {
   }
 
 
-#define INT32_MAT33_VECT3_MULT(_o, _m, _v, _f) {			\
-    (_o).x = ((_m)[0]*(_v).x + (_m)[1]*(_v).y + (_m)[2]*(_v).z)>>(_f);	\
-    (_o).y = ((_m)[3]*(_v).x + (_m)[4]*(_v).y + (_m)[5]*(_v).z)>>(_f);	\
-    (_o).z = ((_m)[6]*(_v).x + (_m)[7]*(_v).y + (_m)[8]*(_v).z)>>(_f);	\
+#define INT32_MAT33_VECT3_MUL(_o, _m, _v, _f) {			\
+    (_o).x = ((_m).m[0]*(_v).x + (_m).m[1]*(_v).y + (_m).m[2]*(_v).z)>>(_f);	\
+    (_o).y = ((_m).m[3]*(_v).x + (_m).m[4]*(_v).y + (_m).m[5]*(_v).z)>>(_f);	\
+    (_o).z = ((_m).m[6]*(_v).x + (_m).m[7]*(_v).y + (_m).m[8]*(_v).z)>>(_f);	\
   }
 
 /*
diff --git a/sw/airborne/mcu.h b/sw/airborne/mcu.h
index 0372433374..e4fdaf5aee 100644
--- a/sw/airborne/mcu.h
+++ b/sw/airborne/mcu.h
@@ -23,7 +23,7 @@
  */
 
 /** \file mcu.h
- *  \brief arch independant mcu ( Micro Controller Unit ) utilities
+ *  \brief arch independent mcu ( Micro Controller Unit ) utilities
  */
 
 #ifndef MCU_H
diff --git a/sw/airborne/mcu_periph/adc.h b/sw/airborne/mcu_periph/adc.h
index bb2f9362c2..feb7cf4c47 100644
--- a/sw/airborne/mcu_periph/adc.h
+++ b/sw/airborne/mcu_periph/adc.h
@@ -23,7 +23,7 @@
  */
 
 /** \file adc.h
- *  \brief arch independant ADC (Analog to Digital Converter) API
+ *  \brief arch independent ADC (Analog to Digital Converter) API
  *
  * Facility to store last values in a circular buffer for a specific
  *   channel:
diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h
index 9cdd614a4b..c2fbf7c0c6 100644
--- a/sw/airborne/mcu_periph/spi.h
+++ b/sw/airborne/mcu_periph/spi.h
@@ -23,7 +23,7 @@
  */
 
 /** \file mcu_periph/spi.h
- * \brief arch independant SPI (Serial Peripheral Interface) API */
+ * \brief arch independent SPI (Serial Peripheral Interface) API */
 
 
 #ifndef SPI_H
diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h
index d9ebf3b32b..d2bd283d7e 100644
--- a/sw/airborne/mcu_periph/uart.h
+++ b/sw/airborne/mcu_periph/uart.h
@@ -23,7 +23,7 @@
  */
 
 /** \file mcu_periph/uart.h
- *  \brief arch independant UART (Universal Asynchronous Receiver/Transmitter) API
+ *  \brief arch independent UART (Universal Asynchronous Receiver/Transmitter) API
  *
  */
 
diff --git a/sw/airborne/mcu_periph/usb_serial.h b/sw/airborne/mcu_periph/usb_serial.h
index fa080e407e..0ce1a825d1 100644
--- a/sw/airborne/mcu_periph/usb_serial.h
+++ b/sw/airborne/mcu_periph/usb_serial.h
@@ -23,7 +23,7 @@
  */
 
 /** \file usb_serial.h
- *  \brief arch independant USB API
+ *  \brief arch independent USB API
  *
  */
 
diff --git a/sw/airborne/modules/MPPT/MPPT.c b/sw/airborne/modules/MPPT/MPPT.c
index 2df9df684f..9eeabf29bb 100644
--- a/sw/airborne/modules/MPPT/MPPT.c
+++ b/sw/airborne/modules/MPPT/MPPT.c
@@ -55,7 +55,7 @@ struct i2c_transaction mppt_trans;
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 uint8_t MPPT_mode;
 /** A value different from 0 is a request from this mode */
@@ -83,7 +83,7 @@ static void MPPT_ask( void ) {
     fbw_current_milliamp = MPPT_data[MPPT_IBAT_INDEX];
 
     MPPT_data[MPPT_ITOTAL_INDEX] = MPPT_data[MPPT_IBAT_INDEX] + MPPT_data[MPPT_ICONV_INDEX];
-    DOWNLINK_SEND_MPPT(DefaultChannel, NB_DATA, MPPT_data);
+    DOWNLINK_SEND_MPPT(DefaultChannel, DefaultDevice, NB_DATA, MPPT_data);
     data_index = 0;
   }
 
diff --git a/sw/airborne/modules/MPPT/sim_MPPT.c b/sw/airborne/modules/MPPT/sim_MPPT.c
index 29a463328c..f8e3485361 100644
--- a/sw/airborne/modules/MPPT/sim_MPPT.c
+++ b/sw/airborne/modules/MPPT/sim_MPPT.c
@@ -28,7 +28,7 @@
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 
 
@@ -46,5 +46,5 @@ void MPPT_init( void ) {
 void MPPT_periodic( void ) {
   MPPT_data[MPPT_ITOTAL_INDEX] = MPPT_data[MPPT_IBAT_INDEX] + MPPT_data[MPPT_ICONV_INDEX];
 
-  RunOnceEvery(8, DOWNLINK_SEND_MPPT(DefaultChannel, NB_DATA, MPPT_data));
+  RunOnceEvery(8, DOWNLINK_SEND_MPPT(DefaultChannel, DefaultDevice, NB_DATA, MPPT_data));
 }
diff --git a/sw/airborne/modules/adcs/adc_generic.c b/sw/airborne/modules/adcs/adc_generic.c
index c2d6419b09..327c894056 100644
--- a/sw/airborne/modules/adcs/adc_generic.c
+++ b/sw/airborne/modules/adcs/adc_generic.c
@@ -2,7 +2,7 @@
 #include "mcu_periph/adc.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include BOARD_CONFIG
 
 uint16_t adc_generic_val1;
@@ -47,6 +47,6 @@ void adc_generic_periodic( void ) {
   adc_generic_val2 = buf_generic2.sum / buf_generic2.av_nb_sample;
 #endif
 
-  DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &adc_generic_val1, &adc_generic_val2);
+  DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &adc_generic_val1, &adc_generic_val2);
 }
 
diff --git a/sw/airborne/modules/adcs/max11040.c b/sw/airborne/modules/adcs/max11040.c
index 0cafe4993e..a867c191e6 100644
--- a/sw/airborne/modules/adcs/max11040.c
+++ b/sw/airborne/modules/adcs/max11040.c
@@ -30,7 +30,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "max11040.h"
 #include "adcs/max11040_hw.h"
 
@@ -66,7 +66,7 @@ void max11040_periodic(void) {
     }
 
     DOWNLINK_SEND_TURB_PRESSURE_VOLTAGE(
-        DefaultChannel,
+        DefaultChannel, DefaultDevice,
         &max11040_values_f[0],
         &max11040_values_f[1],
         &max11040_values_f[2],
diff --git a/sw/airborne/modules/benchmark/flight_benchmark.c b/sw/airborne/modules/benchmark/flight_benchmark.c
index 6c891a363f..be1c0700cc 100644
--- a/sw/airborne/modules/benchmark/flight_benchmark.c
+++ b/sw/airborne/modules/benchmark/flight_benchmark.c
@@ -9,7 +9,7 @@
 #include "firmwares/fixedwing/guidance/guidance_v.h"
 #include "estimator.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "mcu_periph/uart.h"
 #include "generated/airframe.h"
 #include "subsystems/nav.h"
@@ -59,7 +59,7 @@ void flight_benchmark_periodic( void ) {
 	}
 
 	if (benchm_go){
-	#if defined(USE_AIRSPEED) && defined(BENCHMARK_AIRSPEED)
+	#if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED)
 		Err_airspeed = fabs(estimator_airspeed - v_ctl_auto_airspeed_setpoint);
 		if (Err_airspeed>ToleranceAispeed){
 			Err_airspeed = Err_airspeed-ToleranceAispeed;
@@ -113,7 +113,7 @@ void flight_benchmark_periodic( void ) {
 	#endif
 	}
 
-	DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position)
+	DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, DefaultDevice, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position)
 
 }
 
diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c
index 0290a4d471..6e5c1877c8 100644
--- a/sw/airborne/modules/cam_control/cam.c
+++ b/sw/airborne/modules/cam_control/cam.c
@@ -46,6 +46,7 @@ float test_cam_estimator_theta;
 float test_cam_estimator_hspeed_dir;
 #endif // TEST_CAM
 
+//FIXME: use radians
 #ifdef CAM_PAN_NEUTRAL
 #if (CAM_PAN_MAX == CAM_PAN_NEUTRAL)
 #error CAM_PAN_MAX has to be different from CAM_PAN_NEUTRAL
@@ -55,6 +56,7 @@ float test_cam_estimator_hspeed_dir;
 #endif
 #endif
 
+//FIXME: use radians
 #ifdef CAM_TILT_NEUTRAL
 #if ((CAM_TILT_MAX) == (CAM_TILT_NEUTRAL))
 #error CAM_TILT_MAX has to be different from CAM_TILT_NEUTRAL
@@ -64,20 +66,17 @@ float test_cam_estimator_hspeed_dir;
 #endif
 #endif
 
-#define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
-#define DELTA_ALPHA 0.2
-
-#ifdef CAM_PAN0
-float cam_pan_c = RadOfDeg(CAM_PAN0);
-#else
+//FIXME: use radians
+#ifndef CAM_PAN0
+#define CAM_PAN0  RadOfDeg(0)
+#endif
 float cam_pan_c;
-#endif
 
-#ifdef CAM_TILT0
-float cam_tilt_c = RadOfDeg(CAM_TILT0);
-#else
-float cam_tilt_c;
+//FIXME: use radians
+#ifndef CAM_TILT0
+#define CAM_TILT0  RadOfDeg(0)
 #endif
+float cam_tilt_c;
 
 float cam_phi_c;
 float cam_theta_c;
@@ -86,6 +85,9 @@ float cam_target_x, cam_target_y, cam_target_alt;
 uint8_t cam_target_wp;
 uint8_t cam_target_ac;
 
+#ifndef CAM_MODE0
+#define CAM_MODE0 CAM_MODE_OFF
+#endif
 uint8_t cam_mode;
 bool_t cam_lock;
 
@@ -99,11 +101,7 @@ void cam_waypoint_target(void);
 void cam_ac_target(void);
 
 void cam_init( void ) {
-#ifdef CAM_MODE0
   cam_mode = CAM_MODE0;
-#else
-  cam_mode = CAM_MODE_OFF;
-#endif
 }
 
 void cam_periodic( void ) {
@@ -113,16 +111,8 @@ void cam_periodic( void ) {
 #endif
     switch (cam_mode) {
     case CAM_MODE_OFF:
-#if defined(CAM_PAN0)
       cam_pan_c = RadOfDeg(CAM_PAN0);
-#else
-      cam_pan_c = RadOfDeg(0);
-#endif
-#if defined(CAM_TILT0)
       cam_tilt_c = RadOfDeg(CAM_TILT0);
-#else
-      cam_tilt_c = RadOfDeg(90);
-#endif
       cam_angles();
       break;
     case CAM_MODE_ANGLES:
@@ -188,19 +178,19 @@ void cam_periodic( void ) {
 void cam_angles( void ) {
   float cam_pan = 0;
   float cam_tilt = 0;
-  if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)){
-     cam_pan_c = RadOfDeg(CAM_PAN_MAX);
-
-  }else{
-          if(cam_pan_c < RadOfDeg(CAM_PAN_MIN)){ cam_pan_c = RadOfDeg(CAM_PAN_MIN); }
-       }
+  if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) {
+    cam_pan_c = RadOfDeg(CAM_PAN_MAX);
+  } else {
+    if (cam_pan_c < RadOfDeg(CAM_PAN_MIN))
+      cam_pan_c = RadOfDeg(CAM_PAN_MIN);
+  }
 
   if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)){
-     cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
-
-  }else{
-          if(cam_tilt_c < RadOfDeg(CAM_TILT_MIN)){ cam_tilt_c = RadOfDeg(CAM_TILT_MIN); }
-       }
+    cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
+  } else {
+    if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN))
+      cam_tilt_c = RadOfDeg(CAM_TILT_MIN);
+  }
 
 #ifdef CAM_PAN_NEUTRAL
   float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
@@ -240,14 +230,14 @@ void cam_angles( void ) {
 void cam_target( void ) {
 #ifdef TEST_CAM
   vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z,
-     test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
-     cam_target_x, cam_target_y, cam_target_alt,
-     &cam_pan_c, &cam_tilt_c);
+         test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir,
+         cam_target_x, cam_target_y, cam_target_alt,
+         &cam_pan_c, &cam_tilt_c);
 #else
   vPoint(estimator_x, estimator_y, estimator_z,
-     estimator_phi, estimator_theta, estimator_hspeed_dir,
-     cam_target_x, cam_target_y, cam_target_alt,
-     &cam_pan_c, &cam_tilt_c);
+         estimator_phi, estimator_theta, estimator_hspeed_dir,
+         cam_target_x, cam_target_y, cam_target_alt,
+         &cam_pan_c, &cam_tilt_c);
 #endif
   cam_angles();
 }
diff --git a/sw/airborne/modules/cam_control/cam.h b/sw/airborne/modules/cam_control/cam.h
index 5a829e1498..818c8e5363 100644
--- a/sw/airborne/modules/cam_control/cam.h
+++ b/sw/airborne/modules/cam_control/cam.h
@@ -41,6 +41,7 @@
 #define CAM_MODE_STABILIZED    6   // Stabilized mode, input: camera angles from the pan and tilt radio channels, output pointing coordinates.
 #define CAM_MODE_RC            7   // Manual mode, input: camera angles from the pan and tilt radio channels, output servo positions.
 
+//FIXME: use radians
 #ifndef CAM_PAN_MAX
 #define CAM_PAN_MAX 90
 #endif
diff --git a/sw/airborne/modules/cam_control/cam_roll.c b/sw/airborne/modules/cam_control/cam_roll.c
index 60c0beae7c..39cd30bbac 100644
--- a/sw/airborne/modules/cam_control/cam_roll.c
+++ b/sw/airborne/modules/cam_control/cam_roll.c
@@ -1,7 +1,6 @@
 /*
- * $Id$
  *
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2003-2011 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
  *
@@ -33,12 +32,10 @@
 #include "generated/flight_plan.h"
 #include "estimator.h"
 #include "inter_mcu.h"
-#include "subsystems/nav.h"
 
-#define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
-#define DELTA_ALPHA 0.2
-
-#define MAX_CAM_ROLL M_PI/2
+#ifndef CAM_PHI_MAX
+#define CAM_PHI_MAX RadOfDeg(45)
+#endif
 
 float cam_roll_phi; /* radian */
 float phi_c; /* radian */
@@ -73,7 +70,7 @@ void cam_periodic( void ) {
   default:
     phi_c = 0;
   }
-  ap_state->commands[COMMAND_CAM_ROLL] = TRIM_PPRZ(phi_c * MAX_PPRZ / RadOfDeg(CAM_PHI_MAX_DEG));
+  ap_state->commands[COMMAND_CAM_ROLL] = TRIM_PPRZ(phi_c * MAX_PPRZ / CAM_PHI_MAX);
 }
 
 #endif // MOBILE_CAM
diff --git a/sw/airborne/csc/mercury_ap.h b/sw/airborne/modules/cam_control/cam_roll.h
similarity index 60%
rename from sw/airborne/csc/mercury_ap.h
rename to sw/airborne/modules/cam_control/cam_roll.h
index de0ef82d3d..21d02eee18 100644
--- a/sw/airborne/csc/mercury_ap.h
+++ b/sw/airborne/modules/cam_control/cam_roll.h
@@ -1,6 +1,7 @@
 /*
- * Copyright (C) 2008 Joby Energy
+ * Copyright (C) 2003-2011 Pascal Brisset, Antoine Drouin
  *
+ * This file is part of paparazzi.
  *
  * paparazzi is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -19,20 +20,18 @@
  *
  */
 
-/** \file mercury_ap.h
- */
-
-#ifndef __MERCURY_AP_H__
-#define __MERCURY_AP_H__
-
-#include "generated/airframe.h"
-
-extern pprz_t mixed_commands[PROPS_NB];
-extern uint8_t props_enable[PROPS_NB];
-extern uint8_t props_throttle_pass;
-
-void csc_ap_init( void );
-void csc_ap_periodic (uint8_t _in_flight, uint8_t kill);
+#ifndef CAM_ROLL_H
+#define CAM_ROLL_H
 
+extern uint8_t cam_roll_mode;
+extern float cam_roll_phi;
+extern bool_t cam_roll_switch;
 
+#if defined VIDEO_SWITCH_PIN && !(defined SITL)
+#define cam_roll_Switch(_x) { cam_roll_switch = _x; if (_x) IO0SET = _BV(VIDEO_SWITCH_PIN); else IO0CLR = _BV(VIDEO_SWITCH_PIN); }
+#else
+#define cam_roll_Switch(_x) { cam_roll_switch = _x; }
 #endif
+
+
+#endif /* CAM_ROLL_H */
diff --git a/sw/airborne/modules/cam_control/cam_track.c b/sw/airborne/modules/cam_control/cam_track.c
index 8e68801d9f..78167c838a 100644
--- a/sw/airborne/modules/cam_control/cam_track.c
+++ b/sw/airborne/modules/cam_control/cam_track.c
@@ -27,7 +27,7 @@
 #include "subsystems/ins.h"
 #include "subsystems/ahrs.h"
 
-#ifdef USE_HFF
+#if USE_HFF
 #include "subsystems/ins/hf_float.h"
 #endif
 
@@ -50,7 +50,7 @@ struct FloatVect3 last_pos_ned;
 #define GOT_END       5
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 volatile uint8_t cam_msg_received;
 uint8_t cam_status;
@@ -115,7 +115,7 @@ void track_periodic_task(void) {
   for (i = 0; i < c; i++) {
     CamUartSend1(cmd_msg[i]);
   }
-  //DOWNLINK_SEND_DEBUG(DefaultChannel,c,cmd_msg);
+  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,c,cmd_msg);
 
 }
 
@@ -125,7 +125,7 @@ void track_event(void) {
     ins_hf_realign = TRUE;
   }
 
-#ifdef USE_HFF
+#if USE_HFF
   if (ins_hf_realign) {
     ins_hf_realign = FALSE;
     struct FloatVect2 pos, zero;
@@ -188,13 +188,13 @@ void parse_cam_msg( void ) {
   ptr++;
   *ptr = cam_data_buf[11];
 
-  //DOWNLINK_SEND_DEBUG(DefaultChannel,12,cam_data_buf);
+  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,12,cam_data_buf);
 }
 
 void parse_cam_buffer( uint8_t c ) {
   char bla[1];
   bla[1] = c;
-  //DOWNLINK_SEND_DEBUG(DefaultChannel,1,bla);
+  //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,1,bla);
   switch (cam_status) {
   case UNINIT:
     if (c != CAM_START_1)
diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c
index ab2dd938ff..1fa3aa897f 100644
--- a/sw/airborne/modules/cartography/cartography.c
+++ b/sw/airborne/modules/cartography/cartography.c
@@ -87,9 +87,9 @@ uint16_t railnumberSinceBoot=1; //used to count the number of rails the plane ha
 //the number 1 is reserved for snapshot fonctions that take only one image, the 2-65535 numbers are used to number the following sequences
 
 ////////////////////////////////////////////////////////////////////////////////////////////////
-#define USE_ONBOARD_CAMERA
+#define USE_ONBOARD_CAMERA 1
 
-#ifdef USE_ONBOARD_CAMERA
+#if USE_ONBOARD_CAMERA
 uint16_t camera_snapshot_image_number=0;
 #endif
 
@@ -141,7 +141,7 @@ bool_t ProjectionInsideLimitOfRail;
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "mcu_periph/uart.h"
 #include "std.h"
 
@@ -152,7 +152,7 @@ void init_carto(void) {
 }
 
 void periodic_downlink_carto(void) {
-	DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel,&camera_snapshot_image_number);
+	DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,&camera_snapshot_image_number);
 }
 
 void start_carto(void) {
diff --git a/sw/airborne/modules/cartography/cartography.h b/sw/airborne/modules/cartography/cartography.h
index a7ccde3d75..ab0312560b 100644
--- a/sw/airborne/modules/cartography/cartography.h
+++ b/sw/airborne/modules/cartography/cartography.h
@@ -16,7 +16,7 @@ void stop_carto(void);
  */
 
 
-#ifdef USE_ONBOARD_CAMERA
+#if USE_ONBOARD_CAMERA
 extern bool_t CAMERA_SNAPSHOT_REQUIERED;
 extern uint16_t camera_snapshot_image_number;
 #endif
diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.c b/sw/airborne/modules/cartography/photogrammetry_calculator.c
index 7afeea45e9..8f722a39b3 100644
--- a/sw/airborne/modules/cartography/photogrammetry_calculator.c
+++ b/sw/airborne/modules/cartography/photogrammetry_calculator.c
@@ -42,16 +42,16 @@
 
 
 // Flightplan Paramters
-int photogrammetry_sweep_angle = 0;
+int photogrammetry_sweep_angle = 0; // in rad
 
 int photogrammetry_sidestep = 0;
 int photogrammetry_triggerstep = 0;
 int photogrammetry_height = 0;
 
 // Photogrammetry Goals
-int photogrammetry_sidelap;	// Percent 0 - 100
-int photogrammetry_overlap;	// Percent 0 - 100
-int photogrammetry_resolution;	// Millimeter per pixel
+int photogrammetry_sidelap; // Percent 0 - 100
+int photogrammetry_overlap; // Percent 0 - 100
+int photogrammetry_resolution;  // Millimeter per pixel
 
 // Safety Aspects
 int photogrammetry_height_min;
@@ -61,15 +61,15 @@ int photogrammetry_radius_min;
 
 void init_photogrammetry_calculator(void)
 {
-  photogrammetry_sweep_angle 	= PHOTOGRAMMETRY_SWEEP_ANGLE;
+  photogrammetry_sweep_angle = PHOTOGRAMMETRY_SWEEP_ANGLE;
 
-  photogrammetry_sidelap 	= PHOTOGRAMMETRY_SIDELAP;
-  photogrammetry_overlap 	= PHOTOGRAMMETRY_OVERLAP;
-  photogrammetry_resolution 	= PHOTOGRAMMETRY_RESOLUTION;
+  photogrammetry_sidelap     = PHOTOGRAMMETRY_SIDELAP;
+  photogrammetry_overlap     = PHOTOGRAMMETRY_OVERLAP;
+  photogrammetry_resolution  = PHOTOGRAMMETRY_RESOLUTION;
 
-  photogrammetry_height_min	= PHOTOGRAMMETRY_HEIGHT_MIN;
-  photogrammetry_height_max 	= PHOTOGRAMMETRY_HEIGHT_MAX;
-  photogrammetry_radius_min 	= PHOTOGRAMMETRY_RADIUS_MIN;
+  photogrammetry_height_min  = PHOTOGRAMMETRY_HEIGHT_MIN;
+  photogrammetry_height_max  = PHOTOGRAMMETRY_HEIGHT_MAX;
+  photogrammetry_radius_min  = PHOTOGRAMMETRY_RADIUS_MIN;
 
   photogrammetry_calculator_update();
 }
diff --git a/sw/airborne/modules/core/extra_pprz_dl.h b/sw/airborne/modules/core/extra_pprz_dl.h
deleted file mode 100644
index 5c562122b2..0000000000
--- a/sw/airborne/modules/core/extra_pprz_dl.h
+++ /dev/null
@@ -1,226 +0,0 @@
-/*
- * Paparazzi $Id: pprz_transport.h 4870 2010-04-24 03:02:39Z poine $
- *
- * Copyright (C) 2010  ENAC
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file pprz_transport.h
- *  \brief Extra datalink using PPRZ protocol
- *
- *  Pprz frame:
- *
- *   |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B|
- *
- *   where checksum is computed over length and payload:
- *     ck_A = ck_B = length
- *     for each byte b in payload
- *       ck_A += b; ck_b += ck_A
- */
-
-#ifndef EXTRA_PPRZ_DL_H
-#define EXTRA_PPRZ_DL_H
-
-#include "pprz_transport.h"
-
-#define __ExtraLink(dev, _x) dev##_x
-#define _ExtraLink(dev, _x)  __ExtraLink(dev, _x)
-#define ExtraLink(_x) _ExtraLink(EXTRA_DOWNLINK_DEVICE, _x)
-
-/** 4 = STX + len + ck_a + ck_b */
-#define ExtraPprzTransportSizeOf(_payload) (_payload+4)
-
-#define ExtraPprzTransportCheckFreeSpace(_x) ExtraLink(CheckFreeSpace(_x))
-
-#define ExtraPprzTransportPut1Byte(_x) ExtraLink(Transmit(_x))
-#define ExtraPprzTransportSendMessage() ExtraLink(SendMessage())
-
-#define ExtraPprzTransportHeader(payload_len) { \
-  ExtraPprzTransportPut1Byte(STX);				\
-  uint8_t msg_len = ExtraPprzTransportSizeOf(payload_len);	\
-  ExtraPprzTransportPut1Byte(msg_len);			\
-  ck_a = msg_len; ck_b = msg_len;			\
-}
-
-#define ExtraPprzTransportTrailer() { \
-  ExtraPprzTransportPut1Byte(ck_a);	\
-  ExtraPprzTransportPut1Byte(ck_b);	\
-  ExtraPprzTransportSendMessage() \
-}
-
-#define ExtraPprzTransportPutUint8(_byte) { \
-    ck_a += _byte;			  \
-    ck_b += ck_a;			  \
-    ExtraPprzTransportPut1Byte(_byte);		  \
- }
-
-#define ExtraPprzTransportPutNamedUint8(_name, _byte) ExtraPprzTransportPutUint8(_byte)
-
-#define ExtraPprzTransportPut1ByteByAddr(_byte) {	 \
-    uint8_t _x = *(_byte);		 \
-    ExtraPprzTransportPutUint8(_x);	 \
-  }
-
-#define ExtraPprzTransportPut2ByteByAddr(_byte) { \
-    ExtraPprzTransportPut1ByteByAddr(_byte);	\
-    ExtraPprzTransportPut1ByteByAddr((const uint8_t*)_byte+1);	\
-  }
-
-#define ExtraPprzTransportPut4ByteByAddr(_byte) { \
-    ExtraPprzTransportPut2ByteByAddr(_byte);	\
-    ExtraPprzTransportPut2ByteByAddr((const uint8_t*)_byte+2);	\
-  }
-
-#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */
-#define ExtraPprzTransportPutDoubleByAddr(_byte) { \
-    ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_byte+4);	\
-    ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_byte);	\
-  }
-#else
-#define ExtraPprzTransportPutDoubleByAddr(_byte) { \
-    ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_byte);	\
-    ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_byte+4);	\
-  }
-#endif
-
-
-#define ExtraPprzTransportPutInt8ByAddr(_x) ExtraPprzTransportPut1ByteByAddr(_x)
-#define ExtraPprzTransportPutUint8ByAddr(_x) ExtraPprzTransportPut1ByteByAddr((const uint8_t*)_x)
-#define ExtraPprzTransportPutInt16ByAddr(_x) ExtraPprzTransportPut2ByteByAddr((const uint8_t*)_x)
-#define ExtraPprzTransportPutUint16ByAddr(_x) ExtraPprzTransportPut2ByteByAddr((const uint8_t*)_x)
-#define ExtraPprzTransportPutInt32ByAddr(_x) ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_x)
-#define ExtraPprzTransportPutUint32ByAddr(_x) ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_x)
-#define ExtraPprzTransportPutFloatByAddr(_x) ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_x)
-
-#define ExtraPprzTransportPutArray(_put, _n, _x) { \
-  uint8_t _i; \
-  ExtraPprzTransportPutUint8(_n); \
-  for(_i = 0; _i < _n; _i++) { \
-    _put(&_x[_i]); \
-  } \
-}
-
-#define ExtraPprzTransportPutFloatArray(_n, _x) ExtraPprzTransportPutArray(ExtraPprzTransportPutFloatByAddr, _n, _x)
-#define ExtraPprzTransportPutDoubleArray(_n, _x) ExtraPprzTransportPutArray(ExtraPprzTransportPutDoubleByAddr, _n, _x)
-
-#define ExtraPprzTransportPutInt16Array(_n, _x) ExtraPprzTransportPutArray(ExtraPprzTransportPutInt16ByAddr, _n, _x)
-#define ExtraPprzTransportPutUint16Array(_n, _x) ExtraPprzTransportPutArray(ExtraPprzTransportPutUint16ByAddr, _n, _x)
-
-#define ExtraPprzTransportPutInt32Array(_n, _x) ExtraPprzTransportPutArray(ExtraPprzTransportPutInt32ByAddr, _n, _x)
-#define ExtraPprzTransportPutUint32Array(_n, _x) ExtraPprzTransportPutArray(ExtraPprzTransportPutUint32ByAddr, _n, _x)
-
-#define ExtraPprzTransportPutUint8Array(_n, _x) ExtraPprzTransportPutArray(ExtraPprzTransportPutUint8ByAddr, _n, _x)
-
-
-/** Receiving pprz messages */
-
-extern uint8_t extra_pprz_payload[PPRZ_PAYLOAD_LEN];
-
-extern volatile bool_t extra_pprz_msg_received;
-extern uint8_t extra_pprz_ovrn, extra_pprz_error;
-extern volatile uint8_t extra_pprz_payload_len;
-
-#include "led.h"
-//#include "mcu_periph/uart.h"
-//#include "messages.h"
-//#include "downlink.h"
-static inline void parse_extra_pprz( uint8_t c ) {
-  static uint8_t pprz_status = UNINIT;
-  static uint8_t _ck_a, _ck_b, payload_idx;
-
-  //uint8_t tab[] = { c };
-  //DOWNLINK_SEND_DEBUG(DefaultChannel,1,tab);
-  switch (pprz_status) {
-  case UNINIT:
-    if (c == STX)
-      pprz_status++;
-    break;
-  case GOT_STX:
-    if (extra_pprz_msg_received) {
-      extra_pprz_ovrn++;
-      goto error;
-    }
-    extra_pprz_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
-    _ck_a = _ck_b = c;
-    pprz_status++;
-    payload_idx = 0;
-    break;
-  case GOT_LENGTH:
-    extra_pprz_payload[payload_idx] = c;
-    _ck_a += c; _ck_b += _ck_a;
-    payload_idx++;
-    if (payload_idx == extra_pprz_payload_len)
-      pprz_status++;
-    break;
-  case GOT_PAYLOAD:
-    if (c != _ck_a)
-      goto error;
-    pprz_status++;
-    break;
-  case GOT_CRC1:
-    if (c != _ck_b)
-      goto error;
-    extra_pprz_msg_received = TRUE;
-    goto restart;
-  default:
-    goto error;
-  }
-  return;
- error:
-  extra_pprz_error++;
- restart:
-  pprz_status = UNINIT;
-  return;
-}
-
-static inline void extra_pprz_parse_payload(void) {
-  uint8_t i;
-  for(i = 0; i < extra_pprz_payload_len; i++)
-    dl_buffer[i] = extra_pprz_payload[i];
-  dl_msg_available = TRUE;
-}
-
-#define __ExtraPprzLink(dev, _x) dev##_x
-#define _ExtraPprzLink(dev, _x)  __ExtraPprzLink(dev, _x)
-#define ExtraPprzLink(_x) _ExtraPprzLink(EXTRA_PPRZ_UART, _x)
-
-#define ExtraPprzBuffer() ExtraPprzLink(ChAvailable())
-#define ReadExtraPprzBuffer() { while (ExtraPprzLink(ChAvailable())&&!extra_pprz_msg_received) parse_extra_pprz(ExtraPprzLink(Getch())); }
-
-
-/* Datalink Event */
-
-#define ExtraDatalinkEvent() {			\
-  if (ExtraPprzBuffer()) {				\
-    ReadExtraPprzBuffer();				\
-    if (extra_pprz_msg_received) {			\
-      extra_pprz_parse_payload();			\
-      extra_pprz_msg_received = FALSE;		\
-    }						\
-  }						\
-  if (dl_msg_available) {			\
-    dl_parse_msg();				\
-    dl_msg_available = FALSE;			\
-  }						\
-}
-
-
-#endif /* EXTRA_PPRZ_DL_H */
-
diff --git a/sw/airborne/modules/core/sys_mon.c b/sw/airborne/modules/core/sys_mon.c
index badec58d30..f76ac7efe8 100644
--- a/sw/airborne/modules/core/sys_mon.c
+++ b/sw/airborne/modules/core/sys_mon.c
@@ -62,7 +62,7 @@ void init_sysmon(void) {
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 void periodic_report_sysmon(void) {
   /** Report system status at low frequency */
@@ -72,7 +72,7 @@ void periodic_report_sysmon(void) {
     cpu_load = 100 * periodic_cycle / periodic_time;
     event_number = sum_n_event / n_periodic;
 
-    DOWNLINK_SEND_SYS_MON(DefaultChannel,&periodic_time,&periodic_cycle,&periodic_cycle_min,&periodic_cycle_max,&event_number,&cpu_load);
+    DOWNLINK_SEND_SYS_MON(DefaultChannel, DefaultDevice,&periodic_time,&periodic_cycle,&periodic_cycle_min,&periodic_cycle_max,&event_number,&cpu_load);
   }
 
   n_periodic = 0;
diff --git a/sw/airborne/modules/core/extra_pprz_dl.c b/sw/airborne/modules/datalink/extra_pprz_dl.c
similarity index 80%
rename from sw/airborne/modules/core/extra_pprz_dl.c
rename to sw/airborne/modules/datalink/extra_pprz_dl.c
index 3e36b305e1..6fa859461c 100644
--- a/sw/airborne/modules/core/extra_pprz_dl.c
+++ b/sw/airborne/modules/datalink/extra_pprz_dl.c
@@ -23,11 +23,7 @@
  *
  */
 
-#include <inttypes.h>
 #include "extra_pprz_dl.h"
-#include "mcu_periph/uart.h"
 
-volatile bool_t extra_pprz_msg_received = FALSE;
-uint8_t extra_pprz_ovrn, extra_pprz_error;
-volatile uint8_t extra_pprz_payload_len;
-uint8_t extra_pprz_payload[PPRZ_PAYLOAD_LEN];
+struct pprz_transport extra_pprz_tp;
+
diff --git a/sw/airborne/csc/csc_protocol.h b/sw/airborne/modules/datalink/extra_pprz_dl.h
similarity index 53%
rename from sw/airborne/csc/csc_protocol.h
rename to sw/airborne/modules/datalink/extra_pprz_dl.h
index 40d9b0250e..5adc3d62e7 100644
--- a/sw/airborne/csc/csc_protocol.h
+++ b/sw/airborne/modules/datalink/extra_pprz_dl.h
@@ -1,7 +1,7 @@
 /*
- * $Id:$
+ * Paparazzi $Id: pprz_transport.h 4870 2010-04-24 03:02:39Z poine $
  *
- * Copyright (C) 2010 Piotr Esden-Tempski <piotr@esden.net>
+ * Copyright (C) 2010  ENAC
  *
  * This file is part of paparazzi.
  *
@@ -22,23 +22,27 @@
  *
  */
 
-#ifndef CSC_PROTOCOL_H
-#define CSC_PROTOCOL_H
-
-typedef void(* cscp_callback_t)(void *data);
-typedef enum { INIT=0, READ=1, WRITE=2 } queue_state_t;
-
-void cscp_init(void);
-int cscp_transmit(uint32_t client_id, uint8_t msg_id, const uint8_t *buf, uint8_t len);
-void cscp_event(void);
-
-/* Note that messages are only accepted and parsed if a callback function
- * has been registered for their respective message id.
- * Example:
- *
- * 		cscp_register_callback(CSC_VANE_MSG_ID, main_on_vane_msg, (void *)&csc_vane_msg);
+/** \file pprz_transport.h
+ *  \brief Extra datalink using PPRZ protocol
  *
  */
-void cscp_register_callback(uint32_t msg_id, cscp_callback_t callback, uint8_t *data);
 
-#endif
+#ifndef EXTRA_PPRZ_DL_H
+#define EXTRA_PPRZ_DL_H
+
+#include "subsystems/datalink/datalink.h"
+#include "subsystems/datalink/pprz_transport.h"
+
+/* PPRZ transport structure */
+extern struct pprz_transport extra_pprz_tp;
+
+/* Datalink Event */
+
+#define ExtraDatalinkEvent() {			                  \
+  PpprzCheckAndParse(EXTRA_PPRZ_UART, extra_pprz_tp); \
+  DlCheckAndParse();                                  \
+}
+
+
+#endif /* EXTRA_PPRZ_DL_H */
+
diff --git a/sw/airborne/modules/datalink/xtend_rssi.c b/sw/airborne/modules/datalink/xtend_rssi.c
index ddf650b9c5..69478e5928 100644
--- a/sw/airborne/modules/datalink/xtend_rssi.c
+++ b/sw/airborne/modules/datalink/xtend_rssi.c
@@ -39,7 +39,7 @@
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 //from Digi XTend manual
 #define XTEND_RSSI_PWM_PERIOD_USEC 8320 //rssi pwm period () in sys tics
@@ -63,7 +63,7 @@ void xtend_rssi_periodic( void ) {
       rssi_dB_fade_margin = (2 * duty_percent + 10) / 3; //not sure if this is right, datasheet isn't very informative
       pwm_input_duty_valid[XTEND_RSSI_PWM_ARRAY_INDEX] = FALSE;
   }
-  DOWNLINK_SEND_XTEND_RSSI(DefaultChannel,
+  DOWNLINK_SEND_XTEND_RSSI(DefaultChannel, DefaultDevice,
             &datalink_time,
             &rssi_dB_fade_margin,
             &duty_percent );
diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c
index 9c330ad88e..144f636a37 100644
--- a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c
+++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c
@@ -38,7 +38,7 @@
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "estimator.h"
 
 
@@ -95,7 +95,7 @@ void atmega_i2c_cam_ctrl_event( void )
   {
     unsigned char cam_ret[1];
     cam_ret[0] = atmega_i2c_cam_ctrl_trans.buf[0];
-    RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, 1, cam_ret ));
+    RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret ));
     atmega_i2c_cam_ctrl_trans.status = I2CTransDone;
   }
 }
diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c
index 9e7f4e5f2e..da3573b257 100644
--- a/sw/airborne/modules/digital_cam/dc.c
+++ b/sw/airborne/modules/digital_cam/dc.c
@@ -54,7 +54,7 @@ uint16_t dc_buffer = 0;
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "estimator.h"
 #include "subsystems/gps.h"
 
@@ -72,7 +72,7 @@ void dc_send_shot_position(void)
     photo_nr = dc_photo_nr;
   }
 
-  DOWNLINK_SEND_DC_SHOT(DefaultChannel,
+  DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
                         &photo_nr,
                         &gps.utm_pos.east,
                         &gps.utm_pos.north,
@@ -89,7 +89,7 @@ void dc_send_shot_position(void)
 uint8_t dc_info(void) {
 #ifdef DOWNLINK_SEND_DC_INFO
   float course = DegOfRad(estimator_psi);
-  DOWNLINK_SEND_DC_INFO(DefaultChannel,
+  DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
                         &dc_autoshoot,
                         &estimator_x,
                         &estimator_y,
diff --git a/sw/airborne/modules/digital_cam/dc.h b/sw/airborne/modules/digital_cam/dc.h
index ac0252aef2..55532659b6 100644
--- a/sw/airborne/modules/digital_cam/dc.h
+++ b/sw/airborne/modules/digital_cam/dc.h
@@ -76,26 +76,26 @@ extern uint8_t dc_cam_tracing;
 
 /* Generic Set of Digital Camera Commands */
 typedef enum {
-    DC_GET_STATUS = 0,
+  DC_GET_STATUS = 0,
 
-    DC_HOLD = 13,
-    DC_SHOOT = 32,
+  DC_HOLD = 13,
+  DC_SHOOT = 32,
 
-    DC_WIDER = 'w',
-    DC_TALLER = 't',
+  DC_WIDER = 'w',
+  DC_TALLER = 't',
 
-    DC_UP = 'u',
-    DC_DOWN = 'd',
-    DC_CENTER = 'c',
-    DC_LEFT = 'l',
-    DC_RIGHT = 'r',
+  DC_UP = 'u',
+  DC_DOWN = 'd',
+  DC_CENTER = 'c',
+  DC_LEFT = 'l',
+  DC_RIGHT = 'r',
 
-    DC_MENU = 'm',
-    DC_HOME = 'h',
-    DC_PLAY = 'p',
+  DC_MENU = 'm',
+  DC_HOME = 'h',
+  DC_PLAY = 'p',
 
-    DC_ON = 'O',
-    DC_OFF = 'o',
+  DC_ON = 'O',
+  DC_OFF = 'o',
 
 } dc_command_type;
 
@@ -104,12 +104,12 @@ static inline void dc_send_command(uint8_t cmd);
 
 /* Auotmatic Digital Camera Photo Triggering */
 typedef enum {
-    DC_AUTOSHOOT_STOP = 0,
-    DC_AUTOSHOOT_PERIODIC = 1,
-    DC_AUTOSHOOT_DISTANCE = 2,
-    DC_AUTOSHOOT_EXT_TRIG = 3,
-    DC_AUTOSHOOT_SURVEY = 4,
-    DC_AUTOSHOOT_CIRCLE = 5
+  DC_AUTOSHOOT_STOP = 0,
+  DC_AUTOSHOOT_PERIODIC = 1,
+  DC_AUTOSHOOT_DISTANCE = 2,
+  DC_AUTOSHOOT_EXT_TRIG = 3,
+  DC_AUTOSHOOT_SURVEY = 4,
+  DC_AUTOSHOOT_CIRCLE = 5
 } dc_autoshoot_type;
 extern dc_autoshoot_type dc_autoshoot;
 
@@ -144,34 +144,34 @@ void dc_send_shot_position(void);
  * FUNCTIONS
  *****************************************************************/
 /**
-  Sets the dc control in circle mode.
-  The 'start' value is the reference course and 'intervall'
-  the minimum angle between shots.
-  If 'start' is 0 the current course is used instead.
+   Sets the dc control in circle mode.
+   The 'start' value is the reference course and 'intervall'
+   the minimum angle between shots.
+   If 'start' is 0 the current course is used instead.
 
-  In this mode the dc control assumes a perfect circular
-  course.
-  The first picture is taken at angle start+interval.
+   In this mode the dc control assumes a perfect circular
+   course.
+   The first picture is taken at angle start+interval.
 */
 extern uint8_t dc_circle(float interval, float start);
 
 #define dc_Circle(interval) dc_circle(interval, DC_IGNORE)
 
 /**
-  Sets the dc control in distance mode.
-  The values of 'x' and 'y' are the coordinates
-  of the reference point used for the distance
-  calculations.
-  If 'y' is 0 the value of 'x' is interpreted
-  as index of a waypoint declared in the flight plan.
-  If both 'x' and 'y' are 0 the current position
-  will be used as reference point.
+   Sets the dc control in distance mode.
+   The values of 'x' and 'y' are the coordinates
+   of the reference point used for the distance
+   calculations.
+   If 'y' is 0 the value of 'x' is interpreted
+   as index of a waypoint declared in the flight plan.
+   If both 'x' and 'y' are 0 the current position
+   will be used as reference point.
 
-  In this mode, the dc control assumes a perfect
-  line formed course since the distance is calculated
-  relative to the first given point of reference.
-  So not usable for circles or other comparable
-  shapes.
+   In this mode, the dc control assumes a perfect
+   line formed course since the distance is calculated
+   relative to the first given point of reference.
+   So not usable for circles or other comparable
+   shapes.
 */
 extern uint8_t dc_survey(float interval, float x, float y);
 
@@ -179,8 +179,8 @@ extern uint8_t dc_survey(float interval, float x, float y);
 
 
 /**
-  Sets the dc control in inactive mode,
-  stopping all current actions.
+   Sets the dc control in inactive mode,
+   stopping all current actions.
 */
 extern uint8_t dc_stop(void);
 
@@ -205,16 +205,17 @@ static inline void dc_init(void)
 #endif
 }
 
-/* shoot on grid
+#if 0
+/* shoot on grid */
 static inline void dc_shot_on_utm_north_close_to_100m_grid( void )
 {
   uint32_t dist_to_100m_grid = (gps.utm_pos.north / 100) % 100;
-  if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid)
-  {
-      dc_send_command(DC_SHOOT);
+  if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid) {
+    dc_send_command(DC_SHOOT);
   }
 }
-*/
+#endif
+
 static float dim_mod(float a, float b, float m) {
   if (a < b) {
     float tmp = a;
@@ -227,9 +228,9 @@ static float dim_mod(float a, float b, float m) {
 /* periodic 4Hz function */
 static inline void dc_periodic_4Hz( void )
 {
-static uint8_t dc_shutter_timer = 0;
+  static uint8_t dc_shutter_timer = 0;
 
- switch (dc_autoshoot) {
+  switch (dc_autoshoot) {
 
   case DC_AUTOSHOOT_PERIODIC:
     if (dc_shutter_timer) {
@@ -237,23 +238,23 @@ static uint8_t dc_shutter_timer = 0;
     } else {
       dc_shutter_timer = dc_autoshoot_quartersec_period;
       dc_send_command(DC_SHOOT);
-      }
-      break;
+    }
+    break;
 
   case DC_AUTOSHOOT_DISTANCE:
-  {
-  uint32_t dist_to_100m_grid = (gps.utm_pos.north / 100) % 100;
-  if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid)
-  {
-      dc_send_command(DC_SHOOT);
-  }
-  }
-  break;
+    {
+      uint32_t dist_to_100m_grid = (gps.utm_pos.north / 100) % 100;
+      if (dist_to_100m_grid < dc_autoshoot_meter_grid || 100 - dist_to_100m_grid < dc_autoshoot_meter_grid)
+        {
+          dc_send_command(DC_SHOOT);
+        }
+    }
+    break;
 
   case DC_AUTOSHOOT_CIRCLE: {
     float course = DegOfRad(estimator_psi) - dc_circle_start_angle;
     if (course < 0.)
-     course += 360.;
+      course += 360.;
     float current_block = floorf(course/dc_circle_interval);
 
     if (dc_probing) {
@@ -267,8 +268,8 @@ static uint8_t dc_shutter_timer = 0;
       dc_circle_last_block = current_block;
       dc_send_command(DC_SHOOT);
     }
-      }
-      break;
+  }
+    break;
 
   case DC_AUTOSHOOT_SURVEY : {
     float dist_x = dc_gps_x - estimator_x;
@@ -285,12 +286,12 @@ static uint8_t dc_shutter_timer = 0;
       dc_gps_count++;
       dc_send_command(DC_SHOOT);
     }
-      }
-      break;
+  }
+    break;
 
   default :
-      dc_autoshoot = DC_AUTOSHOOT_STOP;
- }
+    dc_autoshoot = DC_AUTOSHOOT_STOP;
+  }
 }
 
 
diff --git a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c
index 97d5d74ac1..d577dfb4b1 100644
--- a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c
+++ b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c
@@ -35,7 +35,7 @@
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "estimator.h"
 
 
@@ -80,7 +80,7 @@ void atmega_i2c_cam_ctrl_send(uint8_t cmd)
   }
 
   cam_ret[0] = mode + zoom * 0x20;
-  RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, 1, cam_ret ));
+  RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret ));
 
 }
 
diff --git a/sw/airborne/anemotaxis.c b/sw/airborne/modules/enose/anemotaxis.c
similarity index 97%
rename from sw/airborne/anemotaxis.c
rename to sw/airborne/modules/enose/anemotaxis.c
index 562b02b7e1..055816bb84 100644
--- a/sw/airborne/anemotaxis.c
+++ b/sw/airborne/modules/enose/anemotaxis.c
@@ -1,11 +1,11 @@
-#include "anemotaxis.h"
+#include "modules/enose/anemotaxis.h"
 #include "generated/airframe.h"
 #include "estimator.h"
 #include "std.h"
 #include "subsystems/nav.h"
 #include "generated/flight_plan.h"
 #include "ap_downlink.h"
-#include "chemo_detect.h"
+#include "modules/enose/chemo_detect.h"
 
 enum status { UTURN, CROSSWIND };
 static enum status status;
diff --git a/sw/airborne/anemotaxis.h b/sw/airborne/modules/enose/anemotaxis.h
similarity index 100%
rename from sw/airborne/anemotaxis.h
rename to sw/airborne/modules/enose/anemotaxis.h
diff --git a/sw/airborne/chemotaxis.c b/sw/airborne/modules/enose/chemotaxis.c
similarity index 94%
rename from sw/airborne/chemotaxis.c
rename to sw/airborne/modules/enose/chemotaxis.c
index 6bc9514c92..bbb6504da2 100644
--- a/sw/airborne/chemotaxis.c
+++ b/sw/airborne/modules/enose/chemotaxis.c
@@ -1,11 +1,11 @@
-#include "chemotaxis.h"
+#include "modules/enose/chemotaxis.h"
 #include "generated/airframe.h"
 #include "estimator.h"
 #include "std.h"
 #include "subsystems/nav.h"
 #include "generated/flight_plan.h"
 #include "ap_downlink.h"
-#include "chemo_detect.h"
+#include "modules/enose/chemo_detect.h"
 
 #define MAX_RADIUS 250
 #define ALPHA 0.5
diff --git a/sw/airborne/chemotaxis.h b/sw/airborne/modules/enose/chemotaxis.h
similarity index 100%
rename from sw/airborne/chemotaxis.h
rename to sw/airborne/modules/enose/chemotaxis.h
diff --git a/sw/airborne/modules/enose/enose.c b/sw/airborne/modules/enose/enose.c
index 7d8b333b28..20e878b692 100644
--- a/sw/airborne/modules/enose/enose.c
+++ b/sw/airborne/modules/enose/enose.c
@@ -49,7 +49,7 @@ void enose_set_heat(uint8_t no_sensor, uint8_t value) {
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 void enose_periodic( void ) {
   enose_PID_val = buf_PID.sum / buf_PID.av_nb_sample;
@@ -87,5 +87,5 @@ void enose_periodic( void ) {
       enose_status = ENOSE_IDLE;
     }
   }
-  DOWNLINK_SEND_ENOSE_STATUS(DefaultChannel,&enose_val[0], &enose_val[1], &enose_val[2], &enose_PID_val, 3, enose_heat);
+  DOWNLINK_SEND_ENOSE_STATUS(DefaultChannel, DefaultDevice,&enose_val[0], &enose_val[1], &enose_val[2], &enose_PID_val, 3, enose_heat);
 }
diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c
index aa56a5395c..1688915b47 100644
--- a/sw/airborne/modules/gps/gps_ubx_ucenter.c
+++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c
@@ -323,7 +323,7 @@ static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 #undef GOT_PAYLOAD
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 static bool_t gps_ubx_ucenter_configure(uint8_t nr)
 {
@@ -352,7 +352,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
     gps_ubx_ucenter.replies[3] = gps_ubx_ucenter.sw_ver_l;
     gps_ubx_ucenter.replies[4] = gps_ubx_ucenter.hw_ver_h;
     gps_ubx_ucenter.replies[5] = gps_ubx_ucenter.hw_ver_l;
-    DOWNLINK_SEND_DEBUG(DefaultChannel,6,gps_ubx_ucenter.replies);
+    DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,6,gps_ubx_ucenter.replies);
 
     //////////////////////////////////
     // Actual configuration start
@@ -397,7 +397,7 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
     break;
   case 16:
     // Debug Downlink the result of all configuration steps: see messages
-    DOWNLINK_SEND_DEBUG(DefaultChannel,GPS_UBX_UCENTER_CONFIG_STEPS,gps_ubx_ucenter.replies);
+    DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,GPS_UBX_UCENTER_CONFIG_STEPS,gps_ubx_ucenter.replies);
     return FALSE;
   default:
     break;
diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c
index def37babce..5f2c10212f 100644
--- a/sw/airborne/modules/gsm/gsm.c
+++ b/sw/airborne/modules/gsm/gsm.c
@@ -67,8 +67,8 @@ Receiving:
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 #include "mcu_periph/uart.h"
-#include "downlink.h"
-#include "ap_downlink.h"
+#include "subsystems/datalink/downlink.h"
+#include "ap_subsystems/datalink/downlink.h"
 #include "subsystems/gps.h"
 #include "autopilot.h"
 #include "estimator.h"
@@ -190,11 +190,11 @@ void gsm_event(void) {
   }
 
   if (gsm_line_received) {
-    if (gsm_buf_len > 0) DOWNLINK_SEND_DEBUG_GSM_RECEIVE(DefaultChannel, gsm_buf_len, gsm_buf);
+    if (gsm_buf_len > 0) DOWNLINK_SEND_DEBUG_GSM_RECEIVE(DefaultChannel, DefaultDevice, gsm_buf_len, gsm_buf);
     gsm_got_line();
     gsm_line_received = false;
   } else if (prompt_received) {
-    DOWNLINK_SEND_DEBUG_GSM_RECEIVE(DefaultChannel, 1, ">");
+    DOWNLINK_SEND_DEBUG_GSM_RECEIVE(DefaultChannel, DefaultDevice, 1, ">");
     gsm_got_prompt();
     prompt_received = false;
   }
@@ -509,7 +509,7 @@ static void Send(const char string[])
     GSMTransmit(string[i++]);
   GSMTransmit(GSM_CMD_LINE_TERMINATION);
 
-  DOWNLINK_SEND_DEBUG_GSM_SEND(DefaultChannel, i, string);
+  DOWNLINK_SEND_DEBUG_GSM_SEND(DefaultChannel, DefaultDevice, i, string);
 }
 
 /* Returns a pointer to the first occurrence of the character c in the firtn
diff --git a/sw/airborne/modules/ins/alt_filter.c b/sw/airborne/modules/ins/alt_filter.c
index ffe188659a..def8795654 100644
--- a/sw/airborne/modules/ins/alt_filter.c
+++ b/sw/airborne/modules/ins/alt_filter.c
@@ -29,7 +29,7 @@
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 TypeKalman alt_filter;
 
@@ -75,7 +75,7 @@ void alt_filter_periodic(void) {
     last_gps_alt = ga;
   }
 
-  RunOnceEvery(6,DOWNLINK_SEND_VFF(DefaultChannel, &baro_ets_altitude,
+  RunOnceEvery(6,DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, &baro_ets_altitude,
         &(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]),
         &(alt_filter.P[0][0]), &(alt_filter.P[1][1]), &(alt_filter.P[2][2])));
 
diff --git a/sw/airborne/modules/ins/fw_ins_vn100.c b/sw/airborne/modules/ins/fw_ins_vn100.c
index a8524a7f03..c5f01c08d3 100644
--- a/sw/airborne/modules/ins/fw_ins_vn100.c
+++ b/sw/airborne/modules/ins/fw_ins_vn100.c
@@ -104,7 +104,7 @@ void ins_periodic_task( void ) {
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 void ins_event_task( void ) {
   if (spi_message_received) {
@@ -116,7 +116,7 @@ void ins_event_task( void ) {
     EstimatorSetAtt((ins_eulers.phi - ins_roll_neutral), ins_eulers.psi, (ins_eulers.theta - ins_pitch_neutral));
 #endif
     //uint8_t s = 4+VN100_REG_QMR_SIZE;
-    //DOWNLINK_SEND_DEBUG(DefaultChannel,s,spi_buffer_input);
+    //DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
   }
 }
 
diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c
index dbea8c92f2..5f574befd2 100644
--- a/sw/airborne/modules/ins/ins_arduimu.c
+++ b/sw/airborne/modules/ins/ins_arduimu.c
@@ -35,7 +35,7 @@ int32_t GPS_Data[14];
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 struct i2c_transaction ardu_gps_trans;
 struct i2c_transaction ardu_ins_trans;
@@ -162,7 +162,7 @@ void ArduIMU_periodicGPS( void ) {
       messageNr = 0;
     }
 
-        //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d);
+        //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d);
 }
 
 void ArduIMU_periodic( void ) {
@@ -186,9 +186,9 @@ void ArduIMU_periodic( void ) {
 
 
     //Nachricht zum GCS senden
-    // DOWNLINK_SEND_ArduIMU(DefaultChannel, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]);
+    // DOWNLINK_SEND_ArduIMU(DefaultChannel, DefaultDevice, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]);
 
-    //    DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert);
+    //    DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert);
 }
 
 void IMU_Daten_verarbeiten( void ) {
@@ -215,6 +215,6 @@ void IMU_Daten_verarbeiten( void ) {
 
     {
     float psi=0;
-    RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &estimator_phi, &estimator_theta, &psi));
+    RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &estimator_phi, &estimator_theta, &psi));
     }
 }
diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c
index a64cad7a05..5285492d6a 100644
--- a/sw/airborne/modules/ins/ins_arduimu_basic.c
+++ b/sw/airborne/modules/ins/ins_arduimu_basic.c
@@ -54,7 +54,7 @@
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #endif
 
 struct i2c_transaction ardu_gps_trans;
@@ -108,7 +108,7 @@ void ArduIMU_periodicGPS( void ) {
 
   if (ardu_gps_trans.status != I2CTransDone) { return; }
 
-#ifdef USE_HIGH_ACCEL_FLAG
+#if USE_HIGH_ACCEL_FLAG
   // Test for high acceleration:
   //  - low speed
   //  - high thrust
@@ -193,9 +193,9 @@ void ArduIMU_event( void ) {
     ardu_ins_trans.status = I2CTransDone;
 
 #ifdef ARDUIMU_SYNC_SEND
-    //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
-    RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
-    RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
+    //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
+    RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
+    RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
 #endif
   }
   else if (ardu_ins_trans.status == I2CTransFailed) {
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
index ccc83f3222..01d441fcab 100644
--- a/sw/airborne/modules/ins/ins_chimu_spi.c
+++ b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -22,7 +22,7 @@ C code to connect a CHIMU using uart
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "ins_module.h"
 #include "imu_chimu.h"
@@ -92,7 +92,7 @@ void parse_ins_msg( void )
       else if(CHIMU_DATA.m_MsgID==0x02)
       {
 
-    RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
+    RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
 
       }
     }
diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
index 17a2539e18..44cc695f7b 100644
--- a/sw/airborne/modules/ins/ins_chimu_uart.c
+++ b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -22,7 +22,7 @@ C code to connect a CHIMU using uart
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "ins_module.h"
 #include "imu_chimu.h"
@@ -90,7 +90,7 @@ void parse_ins_msg( void )
 	EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
 	//EstimatorSetRate(ins_p,ins_q,ins_r);
 
-	DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
+	DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
 
       }
     }
diff --git a/sw/airborne/modules/ins/ins_vn100.c b/sw/airborne/modules/ins/ins_vn100.c
index 576272072d..c595d49676 100644
--- a/sw/airborne/modules/ins/ins_vn100.c
+++ b/sw/airborne/modules/ins/ins_vn100.c
@@ -31,7 +31,7 @@
 #include "generated/airframe.h"
 #include "led.h"
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "messages.h"
 
 /* neutrals */
@@ -186,10 +186,10 @@ void parse_ins_msg( void ) {
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 extern void ins_report_task( void ) {
-  DOWNLINK_SEND_AHRS_LKF(DefaultChannel,
+  DOWNLINK_SEND_AHRS_LKF(DefaultChannel, DefaultDevice,
       &ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi,
       &ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz,
       &ins_rates.p, &ins_rates.q, &ins_rates.r,
diff --git a/sw/airborne/modules/ins/ins_vn100.h b/sw/airborne/modules/ins/ins_vn100.h
index 20d58f153f..d43630e84b 100644
--- a/sw/airborne/modules/ins/ins_vn100.h
+++ b/sw/airborne/modules/ins/ins_vn100.h
@@ -84,7 +84,7 @@ extern uint8_t ins_init_status;
 #define INS_VN100_READY         3
 
 /* Telemetry */
-#define PERIODIC_SEND_AHRS(_chan) DOWNLINK_SEND_AHRS_LKF(_chan, \
+#define PERIODIC_SEND_AHRS(_chan, _dev) DOWNLINK_SEND_AHRS_LKF(_chan, _dev, \
     &ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi, \
     &ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz, \
     &ins_rates.p, &ins_rates.q, &ins_rates.r, \
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 1df53e98c7..dbde59c3b8 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -34,10 +34,10 @@
 #include "generated/airframe.h"
 
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "messages.h"
 
-#ifdef USE_GPS_XSENS
+#if USE_GPS_XSENS
 #include "subsystems/gps.h"
 #include "math/pprz_geodetic_wgs84.h"
 #include "math/pprz_geodetic_float.h"
@@ -187,7 +187,7 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD];
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 
 uint8_t xsens_errorcode;
@@ -353,19 +353,19 @@ void parse_ins_msg( void ) {
   else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
     xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );
 
-    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
+    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
   }
   else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
     xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
     xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
     xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
 
-    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
+    DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z);
   }
   else if (xsens_id == XSENS_Error_ID) {
     xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
   }
-#ifdef USE_GPS_XSENS
+#if USE_GPS_XSENS
   else if (xsens_id == XSENS_GPSStatus_ID) {
     gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
     gps.num_sv = 0;
@@ -398,7 +398,7 @@ void parse_ins_msg( void ) {
         offset += XSENS_DATA_RAWInertial_LENGTH;
       }
       if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
-#if defined(USE_GPS_XSENS_RAW_DATA) && defined(USE_GPS_XSENS)
+#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
 #ifdef GPS_LED
     LED_TOGGLE(GPS_LED);
 #endif
@@ -509,7 +509,7 @@ void parse_ins_msg( void ) {
         offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
       }
       if (XSENS_MASK_Position(xsens_output_mode)) {
-#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
+#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
         gps.last_fix_time = cpu_time_sec;
 
         lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
@@ -530,7 +530,7 @@ void parse_ins_msg( void ) {
         offset += XSENS_DATA_Position_LENGTH;
       }
       if (XSENS_MASK_Velocity(xsens_output_mode)) {
-#if (!defined(USE_GPS_XSENS_RAW_DATA)) && defined(USE_GPS_XSENS)
+#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
         ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
         ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
         ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
@@ -539,7 +539,7 @@ void parse_ins_msg( void ) {
       }
       if (XSENS_MASK_Status(xsens_output_mode)) {
         xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
-#ifdef USE_GPS_XSENS
+#if USE_GPS_XSENS
         if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
         else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
         else gps.fix = GPS_FIX_NONE;
@@ -548,7 +548,7 @@ void parse_ins_msg( void ) {
       }
       if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
         xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
-#ifdef USE_GPS_XSENS
+#if USE_GPS_XSENS
         gps.tow = xsens_time_stamp;
 #endif
         offset += XSENS_DATA_TimeStamp_LENGTH;
diff --git a/sw/airborne/modules/max3100/max3100_hw.c b/sw/airborne/modules/max3100/max3100_hw.c
index af85b690cb..27b25ea0da 100644
--- a/sw/airborne/modules/max3100/max3100_hw.c
+++ b/sw/airborne/modules/max3100/max3100_hw.c
@@ -161,5 +161,5 @@ void SPI1_ISR(void) {
 }
 
 void max3100_debug(void) {
-  /***     DOWNLINK_SEND_DEBUG(DefaultChannel, 16, max3100_rx_buf); ***/
+  /***     DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 16, max3100_rx_buf); ***/
 }
diff --git a/sw/airborne/modules/meteo/charge_sens.c b/sw/airborne/modules/meteo/charge_sens.c
index 691905cd0f..39f47afb7b 100644
--- a/sw/airborne/modules/meteo/charge_sens.c
+++ b/sw/airborne/modules/meteo/charge_sens.c
@@ -31,7 +31,7 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -64,7 +64,7 @@ void charge_sens_event( void ) {
     charge_trans.status = I2CTransDone;
 
     if (++charge_cnt >= CHARGE_NB) {
-      DOWNLINK_SEND_ATMOSPHERE_CHARGE(DefaultChannel,
+      DOWNLINK_SEND_ATMOSPHERE_CHARGE(DefaultChannel, DefaultDevice,
           &charge[0], &charge[1], &charge[2], &charge[3], &charge[4],
           &charge[5], &charge[6], &charge[7], &charge[8], &charge[9]);
       charge_cnt = 0;
diff --git a/sw/airborne/modules/meteo/dust_gp2y.c b/sw/airborne/modules/meteo/dust_gp2y.c
index 0b2ae16f69..114e0e67dd 100644
--- a/sw/airborne/modules/meteo/dust_gp2y.c
+++ b/sw/airborne/modules/meteo/dust_gp2y.c
@@ -35,7 +35,7 @@
 #include "sys_time.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 uint8_t  dust_gp2y_status;
 uint16_t dust_gp2y_density;
@@ -77,7 +77,7 @@ void dust_gp2y_event( void ) {
     if (dust_gp2y_density_f < 0)
       dust_gp2y_density_f = 0;
 
-    DOWNLINK_SEND_GP2Y_STATUS(DefaultChannel, &dust_gp2y_density, &dust_gp2y_density_f);
+    DOWNLINK_SEND_GP2Y_STATUS(DefaultChannel, DefaultDevice, &dust_gp2y_density, &dust_gp2y_density_f);
 
     gp2y_trans.status = I2CTransDone;
   }
diff --git a/sw/airborne/modules/meteo/geiger_counter.c b/sw/airborne/modules/meteo/geiger_counter.c
index 7fadd1560d..ca3aba01cf 100644
--- a/sw/airborne/modules/meteo/geiger_counter.c
+++ b/sw/airborne/modules/meteo/geiger_counter.c
@@ -31,7 +31,7 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -70,7 +70,7 @@ void geiger_counter_event( void ) {
 
     if (volt_geiger & 0x8000) {
       volt_geiger &= 0x7FFF;
-      DOWNLINK_SEND_GEIGER_COUNTER(DefaultChannel,
+      DOWNLINK_SEND_GEIGER_COUNTER(DefaultChannel, DefaultDevice,
           &count_geiger_1, &count_geiger_2, &volt_geiger);
     }
   }
diff --git a/sw/airborne/modules/meteo/humid_dpicco.c b/sw/airborne/modules/meteo/humid_dpicco.c
index 118711c058..85187d385c 100644
--- a/sw/airborne/modules/meteo/humid_dpicco.c
+++ b/sw/airborne/modules/meteo/humid_dpicco.c
@@ -11,7 +11,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -51,7 +51,7 @@ void dpicco_event( void ) {
     dpicco_humid = (dpicco_val[0] * DPICCO_HUMID_RANGE) / DPICCO_HUMID_MAX;
     dpicco_temp = ((dpicco_val[1] * DPICCO_TEMP_RANGE) / DPICCO_TEMP_MAX) + DPICCO_TEMP_OFFS;
 
-    DOWNLINK_SEND_DPICCO_STATUS(DefaultChannel, &dpicco_val[0], &dpicco_val[1], &dpicco_humid, &dpicco_temp);
+    DOWNLINK_SEND_DPICCO_STATUS(DefaultChannel, DefaultDevice, &dpicco_val[0], &dpicco_val[1], &dpicco_humid, &dpicco_temp);
     dpicco_trans.status = I2CTransDone;
   }
 }
diff --git a/sw/airborne/modules/meteo/humid_hih.c b/sw/airborne/modules/meteo/humid_hih.c
index 28888bcf0f..b901ae2c32 100644
--- a/sw/airborne/modules/meteo/humid_hih.c
+++ b/sw/airborne/modules/meteo/humid_hih.c
@@ -36,7 +36,7 @@
 #include "mcu_periph/adc.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -77,6 +77,6 @@ void humid_hih_periodic( void ) {
   /* temperature compensation */
   fhih_humid = fhih_humid/(1.0546 - (0.00216 * fhih_temp));
 
-  DOWNLINK_SEND_HIH_STATUS(DefaultChannel, &adc_humid_hih, &fhih_humid, &fhih_temp);
+  DOWNLINK_SEND_HIH_STATUS(DefaultChannel, DefaultDevice, &adc_humid_hih, &fhih_humid, &fhih_temp);
 }
 
diff --git a/sw/airborne/modules/meteo/humid_htm_b71.c b/sw/airborne/modules/meteo/humid_htm_b71.c
index 30af8b3dc4..aa011ca74f 100644
--- a/sw/airborne/modules/meteo/humid_htm_b71.c
+++ b/sw/airborne/modules/meteo/humid_htm_b71.c
@@ -36,7 +36,7 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -93,7 +93,7 @@ void humid_htm_event( void ) {
         /* temperature */
         temphtm = (htm_trans.buf[2] << 6) | (htm_trans.buf[3] >> 2);
         ftemphtm = -40.00 + 0.01 * temphtm;
-        DOWNLINK_SEND_HTM_STATUS(DefaultChannel, &humidhtm, &temphtm, &fhumidhtm, &ftemphtm);
+        DOWNLINK_SEND_HTM_STATUS(DefaultChannel, DefaultDevice, &humidhtm, &temphtm, &fhumidhtm, &ftemphtm);
       }
       htm_trans.status = I2CTransDone;
       break;
diff --git a/sw/airborne/modules/meteo/humid_pcap01.c b/sw/airborne/modules/meteo/humid_pcap01.c
index 9fd713676c..29fb221dc8 100644
--- a/sw/airborne/modules/meteo/humid_pcap01.c
+++ b/sw/airborne/modules/meteo/humid_pcap01.c
@@ -34,7 +34,7 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "modules/meteo/humid_pcap01.h"
 #ifdef PCAP01_LOAD_FIRMWARE
 #include "modules/meteo/humid_pcap01_firmware.h"
@@ -246,7 +246,7 @@ void pcap01_event(void)
 	 	pcap01Value.R_ratio |= pcap01_trans.buf[2];
 		humidity = pcap01Value.C_ratio * (-0.0023959245437) + 516.4124438673063;
 		temperature = pcap01Value.R_ratio * 61.927 - 259.74;
-		DOWNLINK_SEND_PCAP01_STATUS(DefaultChannel,
+		DOWNLINK_SEND_PCAP01_STATUS(DefaultChannel, DefaultDevice,
 				&pcap01Value.C_ratio,
 				&pcap01Value.R_ratio,
 				&humidity,
diff --git a/sw/airborne/modules/meteo/humid_sht.c b/sw/airborne/modules/meteo/humid_sht.c
index 768244a861..2de7164e4d 100644
--- a/sw/airborne/modules/meteo/humid_sht.c
+++ b/sw/airborne/modules/meteo/humid_sht.c
@@ -10,7 +10,7 @@
 #include "LPC21xx.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "humid_sht.h"
 
 #include "led.h"
@@ -320,7 +320,7 @@ void humid_sht_periodic(void) {
       s_connectionreset();
       s_start_measure(HUMI);
       humid_sht_status = SHT_MEASURING_HUMID;
-      DOWNLINK_SEND_SHT_STATUS(DefaultChannel, &humidsht, &tempsht, &fhumidsht, &ftempsht);
+      DOWNLINK_SEND_SHT_STATUS(DefaultChannel, DefaultDevice, &humidsht, &tempsht, &fhumidsht, &ftempsht);
       humid_sht_available = FALSE;
     }
   }
diff --git a/sw/airborne/modules/meteo/humid_sht_i2c.c b/sw/airborne/modules/meteo/humid_sht_i2c.c
index 94b4e7d06e..5f498c2c13 100644
--- a/sw/airborne/modules/meteo/humid_sht_i2c.c
+++ b/sw/airborne/modules/meteo/humid_sht_i2c.c
@@ -33,7 +33,7 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -102,7 +102,7 @@ void humid_sht_periodic_i2c( void ) {
     sht_status = SHT2_TRIG_TEMP;
     I2CTransmit(SHT_I2C_DEV, sht_trans, SHT_SLAVE_ADDR, 1);
     /* send serial number every 30 seconds */
-    RunOnceEvery((4*30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, &sht_serial1, &sht_serial2));
+    RunOnceEvery((4*30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2));
     break;
   }
 }
@@ -167,7 +167,7 @@ void humid_sht_event_i2c( void ) {
       sht_trans.status = I2CTransDone;
 
       if (humid_sht_crc(sht_trans.buf) == 0) {
-        DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, &humidsht_i2c, &tempsht_i2c, &fhumidsht_i2c, &ftempsht_i2c);
+        DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, DefaultDevice, &humidsht_i2c, &tempsht_i2c, &fhumidsht_i2c, &ftempsht_i2c);
       }
       break;
 
@@ -197,7 +197,7 @@ void humid_sht_event_i2c( void ) {
       sht_serial[6] = sht_trans.buf[4];
       sht_serial1=sht_serial[7]<<24|sht_serial[6]<<16|sht_serial[5]<<8|sht_serial[4];
       sht_serial2=sht_serial[3]<<24|sht_serial[2]<<16|sht_serial[1]<<8|sht_serial[0];
-      DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, &sht_serial1, &sht_serial2);
+      DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2);
       sht_status = SHT2_IDLE;
       sht_trans.status = I2CTransDone;
       break;
diff --git a/sw/airborne/modules/meteo/ir_mlx.c b/sw/airborne/modules/meteo/ir_mlx.c
index ac3c9fde2d..d11e3eb3f8 100644
--- a/sw/airborne/modules/meteo/ir_mlx.c
+++ b/sw/airborne/modules/meteo/ir_mlx.c
@@ -36,7 +36,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -76,7 +76,7 @@ void ir_mlx_periodic( void ) {
       I2CTransceive(MLX_I2C_DEV, mlx_trans, MLX90614_ADDR, 1, 2);
       ir_mlx_status = IR_MLX_RD_CASE_TEMP;
       /* send serial number every 30 seconds */
-      RunOnceEvery((8*30), DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, &ir_mlx_id_01, &ir_mlx_id_23));
+      RunOnceEvery((8*30), DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, DefaultDevice, &ir_mlx_id_01, &ir_mlx_id_23));
     } else if (ir_mlx_status == IR_MLX_UNINIT) {
       /* start two byte ID 0 */
       mlx_trans.buf[0] = MLX90614_ID_0;
@@ -126,7 +126,7 @@ void ir_mlx_event( void ) {
       ir_mlx_id_23 |= mlx_trans.buf[1] << 24;
       ir_mlx_status = IR_MLX_IDLE;
       mlx_trans.status = I2CTransDone;
-      DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, &ir_mlx_id_01, &ir_mlx_id_23);
+      DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, DefaultDevice, &ir_mlx_id_01, &ir_mlx_id_23);
       break;
 
     case IR_MLX_RD_CASE_TEMP:
@@ -148,7 +148,7 @@ void ir_mlx_event( void ) {
       ir_mlx_temp_obj = ir_mlx_itemp_obj*0.02 - 273.15;
       mlx_trans.status = I2CTransDone;
 
-      DOWNLINK_SEND_MLX_STATUS(DefaultChannel,
+      DOWNLINK_SEND_MLX_STATUS(DefaultChannel, DefaultDevice,
                               &ir_mlx_itemp_case,
                               &ir_mlx_temp_case,
                               &ir_mlx_itemp_obj,
diff --git a/sw/airborne/modules/meteo/light_solar.c b/sw/airborne/modules/meteo/light_solar.c
index 494116d3dc..72f28005fa 100644
--- a/sw/airborne/modules/meteo/light_solar.c
+++ b/sw/airborne/modules/meteo/light_solar.c
@@ -32,7 +32,7 @@
 #include "mcu_periph/adc.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "modules/meteo/light_solar.h"
 
 #ifndef ADC_CHANNEL_LIGHT_SOLAR_UP
@@ -70,7 +70,7 @@ void light_solar_periodic( void ) {
   /* 10k/10k voltage divider, 10 bits adc, 3.3V max */
 
   if (++light_cnt >= LIGHT_NB) {
-    DOWNLINK_SEND_SOLAR_RADIATION(DefaultChannel,
+    DOWNLINK_SEND_SOLAR_RADIATION(DefaultChannel, DefaultDevice,
           &up[0], &dn[0], &up[1], &dn[1], &up[2], &dn[2], &up[3], &dn[3],
           &up[4], &dn[4], &up[5], &dn[5], &up[6], &dn[6], &up[7], &dn[7],
           &up[8], &dn[8], &up[9], &dn[9]);
diff --git a/sw/airborne/modules/meteo/light_temt.c b/sw/airborne/modules/meteo/light_temt.c
index cfccd86d23..0f94b70a1b 100644
--- a/sw/airborne/modules/meteo/light_temt.c
+++ b/sw/airborne/modules/meteo/light_temt.c
@@ -33,7 +33,7 @@
 #include "mcu_periph/adc.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef ADC_CHANNEL_LIGHT_TEMT
 #define ADC_CHANNEL_LIGHT_TEMT ADC_4
@@ -63,6 +63,6 @@ void light_temt_periodic( void ) {
   /* 3.6k/6.8k voltage divider, 10 bits adc */
   f_light_temt = (adc_light_temt / 1024.) * 100.;
 
-  DOWNLINK_SEND_TEMT_STATUS(DefaultChannel, &adc_light_temt, &f_light_temt);
+  DOWNLINK_SEND_TEMT_STATUS(DefaultChannel, DefaultDevice, &adc_light_temt, &f_light_temt);
 }
 
diff --git a/sw/airborne/modules/meteo/temp_lm75.c b/sw/airborne/modules/meteo/temp_lm75.c
index 510fd190a0..c66b775b52 100644
--- a/sw/airborne/modules/meteo/temp_lm75.c
+++ b/sw/airborne/modules/meteo/temp_lm75.c
@@ -35,7 +35,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -77,7 +77,7 @@ void lm75_event( void ) {
 
     flm75_temperature = ((int16_t) lm75_temperature) / 2.;
 
-    DOWNLINK_SEND_TMP_STATUS(DefaultChannel, &lm75_temperature, &flm75_temperature);
+    DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &lm75_temperature, &flm75_temperature);
     lm75_trans.status = I2CTransDone;
   }
 }
diff --git a/sw/airborne/modules/meteo/temp_tcouple_adc.c b/sw/airborne/modules/meteo/temp_tcouple_adc.c
index d18f746a13..1005874ec2 100644
--- a/sw/airborne/modules/meteo/temp_tcouple_adc.c
+++ b/sw/airborne/modules/meteo/temp_tcouple_adc.c
@@ -33,7 +33,7 @@
 #include "mcu_periph/adc.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "modules/meteo/temp_tcouple_adc.h"
 
 #ifndef ADC_CHANNEL_TEMP_REF
@@ -80,7 +80,7 @@ void temp_tcouple_adc_periodic( void ) {
                      * 100. - 13.;
 
   if (++temp_cnt >= TCOUPLE_NB) {
-    DOWNLINK_SEND_TEMP_TCOUPLE(DefaultChannel,
+    DOWNLINK_SEND_TEMP_TCOUPLE(DefaultChannel, DefaultDevice,
           &fval[0], &fval[1], &fval[2], &fval[3],
           &fref[0], &fref[1], &fref[2], &fref[3],
           &val[0], &val[1], &val[2], &val[3],
diff --git a/sw/airborne/modules/meteo/temp_temod.c b/sw/airborne/modules/meteo/temp_temod.c
index 350d2e73d3..67fb01362d 100644
--- a/sw/airborne/modules/meteo/temp_temod.c
+++ b/sw/airborne/modules/meteo/temp_temod.c
@@ -33,7 +33,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 float ftmd_temperature;
 struct i2c_transaction tmd_trans;
@@ -72,7 +72,7 @@ void temod_event( void ) {
 
       ftmd_temperature = (tmd_temperature / TEMOD_TYPE) - 32.;
 
-      DOWNLINK_SEND_TMP_STATUS(DefaultChannel, &tmd_temperature, &ftmd_temperature);
+      DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmd_temperature, &ftmd_temperature);
       tmd_trans.status = I2CTransDone;
   }
 }
diff --git a/sw/airborne/modules/meteo/temp_tmp102.c b/sw/airborne/modules/meteo/temp_tmp102.c
index 33c23edd7c..d1856fa6ec 100644
--- a/sw/airborne/modules/meteo/temp_tmp102.c
+++ b/sw/airborne/modules/meteo/temp_tmp102.c
@@ -35,7 +35,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -95,7 +95,7 @@ void tmp102_event( void ) {
 
       ftmp_temperature = ((int16_t) tmp_temperature) / 16.;
 
-      DOWNLINK_SEND_TMP_STATUS(DefaultChannel, &tmp_temperature, &ftmp_temperature);
+      DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmp_temperature, &ftmp_temperature);
       tmp_trans.status = I2CTransDone;
   }
 }
diff --git a/sw/airborne/modules/meteo/wind_gfi.c b/sw/airborne/modules/meteo/wind_gfi.c
index 27d81ddb1a..75b0f19c55 100644
--- a/sw/airborne/modules/meteo/wind_gfi.c
+++ b/sw/airborne/modules/meteo/wind_gfi.c
@@ -35,7 +35,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 struct i2c_transaction pcf_trans;
 
@@ -105,7 +105,7 @@ void wind_gfi_event( void ) {
       /* 2048 digits per 360 degrees */
       fpcf_direction = fmod((pcf_direction * (360./2048.)) + ZERO_OFFSET_DEGREES, 360.);
 
-      DOWNLINK_SEND_TMP_STATUS(DefaultChannel, &pcf_direction, &fpcf_direction);
+      DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &pcf_direction, &fpcf_direction);
     }
     else if (pcf_status == PCF_IDLE) {
       pcf_trans.status = I2CTransDone;
diff --git a/sw/airborne/modules/meteo/windturbine.c b/sw/airborne/modules/meteo/windturbine.c
index 24e3a2538c..5b81a6b847 100644
--- a/sw/airborne/modules/meteo/windturbine.c
+++ b/sw/airborne/modules/meteo/windturbine.c
@@ -40,7 +40,7 @@
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 
 void windturbine_periodic( void ) {
@@ -52,7 +52,7 @@ void windturbine_periodic( void ) {
     sync_itow = gps_tow_from_ticks(trigger_t0);
     cycle_time = MSEC_OF_SYS_TICS(trigger_delta_t0);
 
-    DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel,
+    DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, DefaultDevice,
                 &ac_id,
                 &turb_id,
                 &sync_itow,
diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c
index 48c2f53d11..0377137ffd 100644
--- a/sw/airborne/modules/multi/formation.c
+++ b/sw/airborne/modules/multi/formation.c
@@ -9,7 +9,7 @@
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "multi/formation.h"
 #include "estimator.h"
@@ -85,7 +85,7 @@ int formation_init(void) {
 
 int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) {
   if (_id != AC_ID && the_acs_id[_id] == 0) return FALSE; // no info for this AC
-  DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel,&_id, &form_mode, &slot_e, &slot_n, &slot_a);
+  DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice,&_id, &form_mode, &slot_e, &slot_n, &slot_a);
   formation[the_acs_id[_id]].status = IDLE;
   formation[the_acs_id[_id]].east = slot_e;
   formation[the_acs_id[_id]].north = slot_n;
@@ -100,7 +100,7 @@ int start_formation(void) {
     if (formation[i].status == IDLE) formation[i].status = ACTIVE;
   }
   enum slot_status active = ACTIVE;
-  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel,&ac_id,&leader_id,&active);
+  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&active);
   // store current cruise and alt
   old_cruise = v_ctl_auto_throttle_cruise_throttle;
   old_alt = nav_altitude;
@@ -114,7 +114,7 @@ int stop_formation(void) {
     if (formation[i].status == ACTIVE) formation[i].status = IDLE;
   }
   enum slot_status idle = IDLE;
-  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel,&ac_id,&leader_id,&idle);
+  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&idle);
   // restore cruise and alt
   v_ctl_auto_throttle_cruise_throttle = old_cruise;
   old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
@@ -147,10 +147,10 @@ int formation_flight(void) {
   // broadcast info
   uint8_t ac_id = AC_ID;
   enum slot_status status = formation[the_acs_id[AC_ID]].status;
-  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel,&ac_id,&leader_id,&status);
+  DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&status);
   if (++_1Hz>=4) {
     _1Hz=0;
-    DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel,&ac_id, &form_mode,
+    DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice,&ac_id, &form_mode,
         &formation[the_acs_id[AC_ID]].east,
         &formation[the_acs_id[AC_ID]].north,
         &formation[the_acs_id[AC_ID]].alt);
diff --git a/sw/airborne/modules/multi/potential.c b/sw/airborne/modules/multi/potential.c
index 8572bb909e..fe82759935 100644
--- a/sw/airborne/modules/multi/potential.c
+++ b/sw/airborne/modules/multi/potential.c
@@ -8,7 +8,7 @@
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "dl_protocol.h"
 
 #include "potential.h"
@@ -124,7 +124,7 @@ int potential_task(void) {
   BoundAbs(potential_force.climb, V_CTL_ALTITUDE_MAX_CLIMB);
   NavVerticalClimbMode(potential_force.climb);
 
-  DOWNLINK_SEND_POTENTIAL(DefaultChannel,&potential_force.east,&potential_force.north,&potential_force.alt,&potential_force.speed,&potential_force.climb);
+  DOWNLINK_SEND_POTENTIAL(DefaultChannel, DefaultDevice,&potential_force.east,&potential_force.north,&potential_force.alt,&potential_force.speed,&potential_force.climb);
 
   return TRUE;
 }
diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c
index a86db7aae7..518d1c4c02 100644
--- a/sw/airborne/modules/multi/tcas.c
+++ b/sw/airborne/modules/multi/tcas.c
@@ -35,7 +35,7 @@
 #include "generated/flight_plan.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 float tcas_alt_setpoint;
 float tcas_tau_ta, tcas_tau_ra, tcas_dmod, tcas_alim;
@@ -140,7 +140,7 @@ void tcas_periodic_task_1Hz( void ) {
         if (tau >= TCAS_HUGE_TAU && !inside) {
           tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
           tcas_acs_status[i].resolve = RA_NONE;
-          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel,&(the_acs[i].ac_id));
+          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
         }
         break;
       case TCAS_TA:
@@ -148,25 +148,25 @@ void tcas_periodic_task_1Hz( void ) {
           tcas_acs_status[i].status = TCAS_RA; // TA -> RA
           // Downlink alert
           //test_dir = tcas_test_direction(the_acs[i].ac_id);
-          //DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
+          //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
           break;
         }
         if (tau > tcas_tau_ta && !inside)
           tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
           tcas_acs_status[i].resolve = RA_NONE;
-          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel,&(the_acs[i].ac_id));
+          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
         break;
       case TCAS_NO_ALARM:
         if (tau < tcas_tau_ta || inside) {
           tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
           // Downlink warning
-          DOWNLINK_SEND_TCAS_TA(DefaultChannel,&(the_acs[i].ac_id));
+          DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
         }
         if (tau < tcas_tau_ra || inside) {
           tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
           // Downlink alert
           //test_dir = tcas_test_direction(the_acs[i].ac_id);
-          //DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id),&test_dir);
+          //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);
         }
         break;
     }
@@ -207,11 +207,11 @@ void tcas_periodic_task_1Hz( void ) {
       }
     }
     // Downlink alert
-    DOWNLINK_SEND_TCAS_RA(DefaultChannel,&tcas_ac_RA,&tcas_resolve);
+    DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&tcas_ac_RA,&tcas_resolve);
   }
   else tcas_ac_RA = AC_ID; // no conflict
 #ifdef TCAS_DEBUG
-  if (tcas_status == TCAS_RA) DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel,&ac_id_close,&tau_min);
+  if (tcas_status == TCAS_RA) DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice,&ac_id_close,&tau_min);
 #endif
 }
 
diff --git a/sw/airborne/modules/sensors/AOA_adc.c b/sw/airborne/modules/sensors/AOA_adc.c
index 7c4774293e..38b38e583f 100644
--- a/sw/airborne/modules/sensors/AOA_adc.c
+++ b/sw/airborne/modules/sensors/AOA_adc.c
@@ -33,7 +33,7 @@
 //Messages
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 uint16_t adc_AOA_val;
 
@@ -75,7 +75,7 @@ void AOA_adc_update( void ) {
 	AOA = AOA_filter * AOA_old + (1 - AOA_filter) * (adc_AOA_val*(2*M_PI)/1024-M_PI+AOA_offset);
 	AOA_old = AOA;
 #endif
-	RunOnceEvery(30, DOWNLINK_SEND_AOA_adc(DefaultChannel, &adc_AOA_val, &AOA));
+	RunOnceEvery(30, DOWNLINK_SEND_AOA_adc(DefaultChannel, DefaultDevice, &adc_AOA_val, &AOA));
 
 #ifdef USE_AOA
 	EstimatorSetAOA(AOA);
diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c
index 3139f391df..adb8f80a91 100755
--- a/sw/airborne/modules/sensors/airspeed_amsys.c
+++ b/sw/airborne/modules/sensors/airspeed_amsys.c
@@ -27,11 +27,11 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include <math.h>
 //#include <stdlib.h>
 
-#ifndef USE_AIRSPEED
+#if !USE_AIRSPEED
 // Just a Warning --> We do't use it.
 //#ifndef SENSOR_SYNC_SEND
 //#warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use amsys_airspeed
@@ -145,13 +145,13 @@ void airspeed_amsys_read_event( void ) {
 		airspeed_amsys = airspeed_filter * airspeed_old + (1 - airspeed_filter) * airspeed_tmp;
 		airspeed_old = airspeed_amsys;
 
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
 		EstimatorSetAirspeed(airspeed_amsys);
 #endif
 #ifdef SENSOR_SYNC_SEND
-		DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature);
+		DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature);
 #else
-		RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature));
+		RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature));
 #endif
 	}
 
diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c
index 1859cf5ca2..227526ad9a 100644
--- a/sw/airborne/modules/sensors/airspeed_ets.c
+++ b/sw/airborne/modules/sensors/airspeed_ets.c
@@ -39,10 +39,10 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include <math.h>
 
-#ifndef USE_AIRSPEED
+#if !USE_AIRSPEED
 #ifndef SENSOR_SYNC_SEND
 #warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use ets_airspeed
 #endif
@@ -166,11 +166,11 @@ void airspeed_ets_read_event( void ) {
     for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n)
       airspeed_ets += airspeed_ets_buffer[n];
     airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
     EstimatorSetAirspeed(airspeed_ets);
 #endif
 #ifdef SENSOR_SYNC_SEND
-    DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
+    DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
 #endif
   } else {
     airspeed_ets = 0.0;
diff --git a/sw/airborne/modules/sensors/alt_srf08.c b/sw/airborne/modules/sensors/alt_srf08.c
index 733b25f5af..3b3b091efa 100644
--- a/sw/airborne/modules/sensors/alt_srf08.c
+++ b/sw/airborne/modules/sensors/alt_srf08.c
@@ -31,7 +31,7 @@
 #include "alt_srf08.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include "led.h"
 
 #ifndef DOWNLINK_DEVICE
@@ -148,7 +148,7 @@ void srf08_event(void)
     else if (srf08_got) {
       srf08_got = FALSE;
       srf08_copy();
-      DOWNLINK_SEND_RANGEFINDER(DefaultChannel, &srf08_range, &f, &f, &f, &f, &f, &i);
+      DOWNLINK_SEND_RANGEFINDER(DefaultChannel, DefaultDevice, &srf08_range, &f, &f, &f, &f, &f, &i);
     }
   }
 }
diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c
index ecd698bbed..e89f8519ea 100644
--- a/sw/airborne/modules/sensors/baro_MS5534A.c
+++ b/sw/airborne/modules/sensors/baro_MS5534A.c
@@ -209,7 +209,7 @@ static void calibration( void ) {
   ut1 = (c5 << 3) + 20224;
 
 #ifndef BARO_NO_DOWNLINK
-  DOWNLINK_SEND_BARO_WORDS(DefaultChannel, &words[0], &words[1], &words[2], &words[3]);
+  DOWNLINK_SEND_BARO_WORDS(DefaultChannel, DefaultDevice, &words[0], &words[1], &words[2], &words[3]);
 #endif
 }
 
diff --git a/sw/airborne/modules/sensors/baro_MS5534A.h b/sw/airborne/modules/sensors/baro_MS5534A.h
index 9e4aa3420a..d8e39137ba 100644
--- a/sw/airborne/modules/sensors/baro_MS5534A.h
+++ b/sw/airborne/modules/sensors/baro_MS5534A.h
@@ -32,7 +32,7 @@
 
 #include "std.h"
 
-#ifdef USE_BARO_MS5534A
+#if USE_BARO_MS5534A
 
 extern bool_t baro_MS5534A_do_reset;
 extern bool_t baro_MS5534A_available;
diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c
index 08bd9a1807..8e244f37e6 100755
--- a/sw/airborne/modules/sensors/baro_amsys.c
+++ b/sw/airborne/modules/sensors/baro_amsys.c
@@ -36,7 +36,7 @@
 //Messages
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 //#include "gps.h"
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -194,9 +194,9 @@ void baro_amsys_read_event( void ) {
 	// Transaction has been read
 	baro_amsys_i2c_trans.status = I2CTransDone;
 #ifdef SENSOR_SYNC_SEND
-	DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)
+	DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)
 #else
-	RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp));
+	RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp));
 #endif
 
 }
diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c
index 0aa51e660f..e6e424e5bc 100644
--- a/sw/airborne/modules/sensors/baro_bmp.c
+++ b/sw/airborne/modules/sensors/baro_bmp.c
@@ -36,7 +36,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -166,7 +166,7 @@ void baro_bmp_event( void ) {
       baro_bmp_temperature = bmp_t;
       baro_bmp_pressure = bmp_p;
 #ifdef SENSOR_SYNC_SEND
-      DOWNLINK_SEND_BMP_STATUS(DefaultChannel, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
+      DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
 #endif
     }
   }
diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c
index 9ad14245ef..4f9a355c0c 100644
--- a/sw/airborne/modules/sensors/baro_ets.c
+++ b/sw/airborne/modules/sensors/baro_ets.c
@@ -112,7 +112,7 @@ void baro_ets_read_periodic( void ) {
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #endif
 
@@ -151,7 +151,7 @@ void baro_ets_read_event( void ) {
       // New value available
       EstimatorSetAlt(baro_ets_altitude);
 #ifdef BARO_ETS_TELEMETRY
-      DOWNLINK_SEND_BARO_ETS(DefaultChannel, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
+      DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
 #endif
     } else {
       baro_ets_altitude = 0.0;
diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c
index af31cf3af2..cab44e00fb 100644
--- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c
+++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c
@@ -34,7 +34,7 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -89,7 +89,7 @@ void baro_ms5611_periodic( void ) {
       ms5611_status = MS5611_CONV_D1;
       ms5611_trans.buf[0] = MS5611_START_CONV_D1;
       I2CTransmit(MS5611_I2C_DEV, ms5611_trans, MS5611_SLAVE_ADDR, 1);
-      RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel,
+      RunOnceEvery((4*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
           &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
           &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]));
     }
@@ -152,7 +152,7 @@ void baro_ms5611_event( void ) {
         ms5611_trans.status = I2CTransDone;
         /* check prom crc */
         if (baro_ms5611_crc(ms5611_c) == 0) {
-          DOWNLINK_SEND_MS5611_COEFF(DefaultChannel,
+          DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice,
               &ms5611_c[0], &ms5611_c[1], &ms5611_c[2], &ms5611_c[3],
               &ms5611_c[4], &ms5611_c[5], &ms5611_c[6], &ms5611_c[7]);
           ms5611_status = MS5611_IDLE;
@@ -220,7 +220,7 @@ void baro_ms5611_event( void ) {
 #ifdef SENSOR_SYNC_SEND
       ftempms = tempms / 100.;
       fbaroms = baroms / 100.;
-      DOWNLINK_SEND_BARO_MS5611(DefaultChannel,
+      DOWNLINK_SEND_BARO_MS5611(DefaultChannel, DefaultDevice,
                                 &ms5611_d1, &ms5611_d2, &fbaroms, &ftempms);
 #endif
       break;
diff --git a/sw/airborne/modules/sensors/baro_scp.c b/sw/airborne/modules/sensors/baro_scp.c
index a4321049ae..bc5d8286cd 100644
--- a/sw/airborne/modules/sensors/baro_scp.c
+++ b/sw/airborne/modules/sensors/baro_scp.c
@@ -5,7 +5,7 @@
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "mcu_periph/spi.h"
 
@@ -179,7 +179,7 @@ static void baro_scp_read(void) {
 void baro_scp_event( void ) {
   if (baro_scp_available == TRUE) {
 #ifdef SENSOR_SYNC_SEND
-    DOWNLINK_SEND_SCP_STATUS(DefaultChannel, &baro_scp_pressure, &baro_scp_temperature);
+    DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature);
 #endif
     baro_scp_available = FALSE;
   }
diff --git a/sw/airborne/modules/sensors/baro_scp_i2c.c b/sw/airborne/modules/sensors/baro_scp_i2c.c
index 1c85bb5d16..bed1c9899f 100644
--- a/sw/airborne/modules/sensors/baro_scp_i2c.c
+++ b/sw/airborne/modules/sensors/baro_scp_i2c.c
@@ -13,7 +13,7 @@
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef SENSOR_SYNC_SEND
 #warning set SENSOR_SYNC_SEND to use baro_scp_i2c
@@ -100,7 +100,7 @@ void baro_scp_event( void ) {
       baro_scp_pressure *= 25;
 
 #ifdef SENSOR_SYNC_SEND
-      DOWNLINK_SEND_SCP_STATUS(DefaultChannel, &baro_scp_pressure, &baro_scp_temperature);
+      DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature);
 #endif
 
       baro_scp_status = BARO_SCP_IDLE;
diff --git a/sw/airborne/modules/sensors/imu_ppzuav.c b/sw/airborne/modules/sensors/imu_ppzuav.c
index ba8b137cc8..b2371ae4e1 100644
--- a/sw/airborne/modules/sensors/imu_ppzuav.c
+++ b/sw/airborne/modules/sensors/imu_ppzuav.c
@@ -29,7 +29,7 @@
 // Downlink
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
@@ -200,9 +200,9 @@ void imu_periodic( void )
 
 void ppzuavimu_module_downlink_raw( void )
 {
-  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
-  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
-  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
+  DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z);
 }
 
 void ppzuavimu_module_event( void )
diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c
index cc3965c14d..07a7b8e5d7 100644
--- a/sw/airborne/modules/sensors/mag_hmc5843.c
+++ b/sw/airborne/modules/sensors/mag_hmc5843.c
@@ -21,7 +21,7 @@
 #include "mcu_periph/i2c.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 #include <math.h>
 
 #include "../../peripherals/hmc5843.h"
@@ -46,7 +46,7 @@ void hmc5843_module_periodic ( void )
   mag_x = hmc5843.data.value[0];
   mag_y = hmc5843.data.value[1];
   mag_z = hmc5843.data.value[2];
-  RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&mag_x,&mag_y,&mag_z));
+  RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&mag_x,&mag_y,&mag_z));
 }
 
 void hmc5843_module_event( void )
diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c
index 6e4efeadf9..e719d49191 100644
--- a/sw/airborne/modules/sensors/mag_hmc58xx.c
+++ b/sw/airborne/modules/sensors/mag_hmc58xx.c
@@ -21,7 +21,7 @@
 #include "modules/sensors/mag_hmc58xx.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 
 #ifndef DOWNLINK_DEVICE
@@ -30,6 +30,6 @@
 
 void mag_hmc58xx_report( void )
 {
-  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,&hmc58xx_data.x,&hmc58xx_data.y,&hmc58xx_data.z);
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&hmc58xx_data.x,&hmc58xx_data.y,&hmc58xx_data.z);
 }
 
diff --git a/sw/airborne/modules/sensors/mag_micromag_fw.c b/sw/airborne/modules/sensors/mag_micromag_fw.c
index 91fa517cef..33d7356c60 100644
--- a/sw/airborne/modules/sensors/mag_micromag_fw.c
+++ b/sw/airborne/modules/sensors/mag_micromag_fw.c
@@ -7,7 +7,7 @@
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 volatile uint8_t micromag_status;
 volatile int16_t micromag_values[MM_NB_AXIS];
@@ -39,7 +39,7 @@ void micromag_event( void ) {
   int32_t mz=micromag_values[2];
 
   if (micromag_status == MM_DATA_AVAILABLE) {
-    DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+    DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
                 &mx,
                 &my,
                 &mz );
diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c
index 63e57f2d25..09fd922c2a 100644
--- a/sw/airborne/modules/sensors/pressure_board_navarro.c
+++ b/sw/airborne/modules/sensors/pressure_board_navarro.c
@@ -139,7 +139,7 @@ void pbn_read_event( void ) {
       pbn_altitude = PBN_ALTITUDE_SCALE*(float)(altitude_adc-altitude_offset);
 
       pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.);
-#ifdef USE_AIRSPEED
+#if USE_AIRSPEED
       EstimatorSetAirspeed(pbn_airspeed);
 #endif
       //alt_kalman(pbn_altitude);
diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h
index 5b1fe68bff..f5e78f2a2f 100644
--- a/sw/airborne/modules/sensors/pressure_board_navarro.h
+++ b/sw/airborne/modules/sensors/pressure_board_navarro.h
@@ -54,6 +54,6 @@ extern void pbn_read_event( void );
 
 #define PbnEvent() { if (pbn_trans.status == I2CTransSuccess) pbn_read_event(); }
 
-#define PERIODIC_SEND_PBN(_chan) DOWNLINK_SEND_PBN(DefaultChannel,&airspeed_adc,&altitude_adc,&pbn_airspeed,&pbn_altitude,&airspeed_offset,&altitude_offset);
+#define PERIODIC_SEND_PBN(_chan) DOWNLINK_SEND_PBN(DefaultChannel, DefaultDevice,&airspeed_adc,&altitude_adc,&pbn_airspeed,&pbn_altitude,&airspeed_offset,&altitude_offset);
 
 #endif // PRESSURE_BOARD_NAVARRO_H
diff --git a/sw/airborne/modules/sensors/trigger_ext.c b/sw/airborne/modules/sensors/trigger_ext.c
index 3f0b0643be..35ee19e9e3 100644
--- a/sw/airborne/modules/sensors/trigger_ext.c
+++ b/sw/airborne/modules/sensors/trigger_ext.c
@@ -36,7 +36,7 @@
 #include "sys_time.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 
 void trigger_ext_init ( void ) {
@@ -52,7 +52,7 @@ void trigger_ext_periodic( void ) {
     sync_itow = gps_tow_from_ticks(trigger_t0);
     cycle_time =  MSEC_OF_SYS_TICS(delta_t0);
 
-    DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel,
+    DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, DefaultDevice,
                 &ac_id,
                 &turb_id,
                 &sync_itow,
diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.c b/sw/airborne/modules/vehicle_interface/vi_datalink.c
index 356776c65b..6df5e4f016 100644
--- a/sw/airborne/modules/vehicle_interface/vi_datalink.c
+++ b/sw/airborne/modules/vehicle_interface/vi_datalink.c
@@ -34,7 +34,7 @@ void vi_impl_set_enabled(bool_t enabled __attribute__ ((unused))) {
 
 #define ViMaxHSpeed ((int16_t)SPEED_BFP_OF_REAL(VI_MAX_H_SPEED))
 #define ViMaxVSpeed ((int16_t)SPEED_BFP_OF_REAL(VI_MAX_V_SPEED))
-#define ViMaxHeadingRate ((int16_t)RATE_BFP_OF_REAL(RadOfDeg(VI_MAX_HEADING_RATE)))
+#define ViMaxHeadingRate ((int16_t)RATE_BFP_OF_REAL(VI_MAX_HEADING_RATE))
 
 struct Int16Vect3 wp_speed_max = { ViMaxHSpeed, ViMaxHSpeed, ViMaxVSpeed };
 
diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.h b/sw/airborne/modules/vehicle_interface/vi_datalink.h
index 20df4ab6c6..2a8af91347 100644
--- a/sw/airborne/modules/vehicle_interface/vi_datalink.h
+++ b/sw/airborne/modules/vehicle_interface/vi_datalink.h
@@ -37,7 +37,7 @@
 #endif
 
 #ifndef VI_MAX_HEADING_RATE
-#define VI_MAX_HEADING_RATE 60.
+#define VI_MAX_HEADING_RATE RadOfDeg(60.)
 #endif
 
 extern void vi_update_wp(uint8_t wp_id);
diff --git a/sw/airborne/obsolete/3dmg.c b/sw/airborne/obsolete/3dmg.c
deleted file mode 100644
index 32fa008c47..0000000000
--- a/sw/airborne/obsolete/3dmg.c
+++ /dev/null
@@ -1,165 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2005 Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include "generated/airframe.h"
-
-#include "std.h"
-#include "3dmg.h"
-#include "mcu_periph/uart.h"
-
-volatile bool_t _3dmg_data_ready;
-int16_t   _3dmg_roll, _3dmg_pitch, _3dmg_yaw;
-int16_t   _3dmg_roll_dot, _3dmg_pitch_dot, _3dmg_yaw_dot;
-uint16_t  _3dmg_timer_tick;
-uint8_t   type, state;
-uint16_t  checksum_read, checksum_comp;
-
-void _3dmg_set_continuous_mode ( void ) {
-#define REQ_CONT_LEN 5
-  uint8_t msg[REQ_CONT_LEN] = { 0x10, 0x00, 0x31, 0x00, 0x41};
-  uint8_t i;
-  for (i=0; i<REQ_CONT_LEN; i++)
-    Uart0Transmit(msg[i]);
-}
-
-void _3dmg_capture_neutral ( void ) {
-#define CAP_GYR_LEN 3
-  uint8_t msg[REQ_CONT_LEN] = { 0x06, 0x00, 0x06};
-  uint8_t i;
-  for (i=0; i<CAP_GYR_LEN; i++)
-    Uart0Transmit(msg[i]);
-}
-
-
-#define READ_MSB(my_short, my_char) {		 \
-    {						 \
-     my_short = (((int16_t)my_char)<<8)&0xFF00;  \
-     state++;					 \
-    }						 \
-  }
-
-#define READ_LSB(my_short, my_char) {		\
-    {						\
-    my_short += (((int16_t)my_char)&0x00FF);	\
-    checksum_comp += (uint16_t)my_short;	\
-    state++;					\
-    }						\
-  }
-
-static inline void on_3dmg_receive(uint8_t c) {
-  int16_t foo = 0;
-  switch(state) {
-  case 0:
-    _3dmg_data_ready = FALSE ;
-    if(c==0x31) {
-      type = c;
-      checksum_comp = (int16_t)type;
-      state++ ;
-    }
-    break;
-  case 1:
-    READ_MSB(_3dmg_roll, c);
-    break ;
-  case 2:
-    READ_LSB(_3dmg_roll, c);
-    break ;
-  case 3:
-    READ_MSB(_3dmg_pitch, c);
-    break ;
-  case 4:
-    READ_LSB(_3dmg_pitch, c);
-    break ;
-  case 5:
-    READ_MSB(_3dmg_yaw, c);
-    break ;
-  case 6:
-    READ_LSB(_3dmg_yaw, c);
-    break ;
-  case 7:
-    READ_MSB(foo, c);
-     /* accel_x_msb */
-    break ;
-  case 8:
-    READ_LSB(foo, c);
-    /* accel_x_lsb */
-    break ;
-  case 9:
-    READ_MSB(foo, c);
-    /* accel_y_msb */
-    break ;
-  case 10:
-    READ_LSB(foo, c);
-    /* accel_y_lsb */
-    break ;
-  case 11:
-    READ_MSB(foo, c);
-    /* accel_z_msb */
-    break ;
-  case 12:
-    READ_LSB(foo, c);
-    /* accel_z_lsb */
-    break ;
-  case 13:
-    READ_MSB(_3dmg_roll_dot, c);
-    break ;
-  case 14:
-    READ_LSB(_3dmg_roll_dot, c);
-    break ;
-  case 15:
-    READ_MSB(_3dmg_pitch_dot, c);
-    break ;
-  case 16:
-    READ_LSB(_3dmg_pitch_dot, c);
-    break ;
-  case 17:
-    READ_MSB(_3dmg_yaw_dot, c);
-    break ;
-  case 18:
-    READ_LSB(_3dmg_yaw_dot, c);
-    break ;
-  case 19:
-    READ_MSB(_3dmg_timer_tick, c);
-    break ;
-  case 20:
-    READ_LSB(_3dmg_timer_tick, c);
-    break ;
-  case 21:
-    checksum_read = (((uint16_t)c)&0x00FF)<<8;
-    state++;
-    break ;
-  case 22:
-    checksum_read += ((uint16_t)c)&0x00FF;
-    state = 0 ;
-    //   if (checksum_read == checksum_comp)
-      _3dmg_data_ready = TRUE ;
-    break ;
-  }
-}
-
-
-#ifdef AVR_ARCH
-
-ReceiveUart(on_3dmg_receive);
-
-#endif /* AVR_ARCH */
diff --git a/sw/airborne/obsolete/3dmg.h b/sw/airborne/obsolete/3dmg.h
deleted file mode 100644
index d5a8ca8310..0000000000
--- a/sw/airborne/obsolete/3dmg.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef _3DMG_H
-#define _3DMG_H
-
-#include <inttypes.h>
-#include "std.h"
-
-void _3dmg_capture_gyro_bias ( void );
-void _3dmg_set_continuous_mode ( void );
-void _3dmg_capture_neutral ( void );
-
-extern volatile bool_t _3dmg_data_ready;
-extern int16_t         _3dmg_roll, _3dmg_pitch, _3dmg_yaw;
-extern int16_t         _3dmg_roll_dot, _3dmg_pitch_dot, _3dmg_yaw_dot;
-
-#endif /* _3DMG_H */
diff --git a/sw/airborne/obsolete/booz_gps.h b/sw/airborne/obsolete/booz_gps.h
deleted file mode 100644
index d7d7a7cd8c..0000000000
--- a/sw/airborne/obsolete/booz_gps.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2010 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#ifndef BOOZ2_GPS_H
-#define BOOZ2_GPS_H
-
-#include "std.h"
-#include "math/pprz_geodetic_int.h"
-#include "mcu_periph/uart.h"
-
-struct Booz_gps_state {
-  struct EcefCoor_i ecef_pos;    /* pos ECEF in cm        */
-  struct EcefCoor_i ecef_vel;    /* speed ECEF in cm/s    */
-  struct LlaCoor_i lla_pos;      /* pos LLA               */
-  int32_t hmsl;                  /* above mean sea level  */
-  uint32_t pacc;                 /* position accuracy     */
-  uint32_t sacc;                 /* speed accuracy        */
-  uint16_t pdop;                 /* dilution of precision */
-  uint8_t  num_sv;               /* number of sat in fix  */
-  uint8_t  fix;                  /* status of fix         */
-  uint32_t tow;                  /* time of week in 1e-2s */
-
-  uint8_t  lost_counter;         /* updated at 4Hz        */
-};
-
-extern struct Booz_gps_state booz_gps_state;
-
-
-/* GPS model specific implementation */
-
-#ifdef BOOZ_GPS_TYPE_H
-#include BOOZ_GPS_TYPE_H
-#endif
-extern void booz_gps_impl_init(void);
-
-
-#define  BOOZ2_GPS_FIX_NONE 0x00
-#define  BOOZ2_GPS_FIX_3D   0x03
-
-#define GpsFixValid() (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)
-
-/*
- * This part is used by the simulator to feed simulated data
- *
- */
-#ifdef SITL
-
-extern bool_t booz_gps_available;
-#define GPS_LINKChAvailable() (FALSE)
-#define GPS_LINKGetch() (TRUE)
-#include "nps_sensors.h"
-#include "generated/flight_plan.h"
-
-static inline void  booz_gps_feed_value() {
-  booz_gps_state.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
-  booz_gps_state.ecef_pos.y = sensors.gps.ecef_pos.y * 100.;
-  booz_gps_state.ecef_pos.z = sensors.gps.ecef_pos.z * 100.;
-  booz_gps_state.ecef_vel.x = sensors.gps.ecef_vel.x * 100.;
-  booz_gps_state.ecef_vel.y = sensors.gps.ecef_vel.y * 100.;
-  booz_gps_state.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
-  booz_gps_state.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
-  booz_gps_state.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
-  booz_gps_state.lla_pos.alt = sensors.gps.lla_pos.alt * 100.;
-  booz_gps_state.hmsl        = sensors.gps.hmsl * 100.;
-  booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
-  booz_gps_available = TRUE;
-}
-
-#define BoozGpsEvent(_sol_available_callback) {			\
-    if (booz_gps_available) {					\
-      if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)               \
-        booz_gps_state.lost_counter = 0;			\
-      _sol_available_callback();				\
-      booz_gps_available = FALSE;				\
-    }								\
-  }
-#else /* ! SITL */
-/*
- * This part is used by the autopilot to read data from a uart
- *
- */
-
-
-#define __GpsLink(dev, _x) dev##_x
-#define _GpsLink(dev, _x)  __GpsLink(dev, _x)
-#define GpsLink(_x) _GpsLink(GPS_LINK, _x)
-
-#define GpsBuffer() GpsLink(ChAvailable())
-
-
-
-
-#endif /* !SITL */
-
-
-extern void booz_gps_init(void);
-
-static inline void booz_gps_periodic( void ) {
-  RunOnceEvery(128, booz_gps_state.lost_counter++; );
-}
-
-#define GpsIsLost() (booz_gps_state.lost_counter > 20) /* 4Hz -> 5s */
-
-
-
-
-#endif /* BOOZ2_GPS_H */
diff --git a/sw/airborne/obsolete/booz_gps_skytraq.c b/sw/airborne/obsolete/booz_gps_skytraq.c
deleted file mode 100644
index 853fa157c3..0000000000
--- a/sw/airborne/obsolete/booz_gps_skytraq.c
+++ /dev/null
@@ -1,155 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#include "booz_gps.h"
-
-struct BoozGpsSkytraq booz_gps_skytraq;
-
-/* parser status */
-#define UNINIT        0
-#define GOT_SYNC1     1
-#define GOT_SYNC2     2
-#define GOT_LEN1      3
-#define GOT_LEN2      4
-#define GOT_ID        5
-#define GOT_PAYLOAD   6
-#define GOT_CHECKSUM  7
-#define GOT_SYNC3     8
-
-//#include "my_debug_servo.h"
-#include "led.h"
-
-void booz_gps_impl_init(void) {
-
-  booz_gps_skytraq.status = UNINIT;
-
-
-  //DEBUG_SERVO1_INIT();
-
-}
-
-
-void booz_gps_skytraq_read_message(void) {
-
-  //DEBUG_S1_ON();
-
-  if (booz_gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {
-    booz_gps_state.ecef_pos.x  = SKYTRAQ_NAVIGATION_DATA_ECEFX(booz_gps_skytraq.msg_buf);
-    booz_gps_state.ecef_pos.y  = SKYTRAQ_NAVIGATION_DATA_ECEFY(booz_gps_skytraq.msg_buf);
-    booz_gps_state.ecef_pos.z  = SKYTRAQ_NAVIGATION_DATA_ECEFZ(booz_gps_skytraq.msg_buf);
-    booz_gps_state.ecef_vel.x  = SKYTRAQ_NAVIGATION_DATA_ECEFVX(booz_gps_skytraq.msg_buf);
-    booz_gps_state.ecef_vel.y  = SKYTRAQ_NAVIGATION_DATA_ECEFVY(booz_gps_skytraq.msg_buf);
-    booz_gps_state.ecef_vel.z  = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(booz_gps_skytraq.msg_buf);
-    booz_gps_state.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(booz_gps_skytraq.msg_buf);
-    booz_gps_state.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(booz_gps_skytraq.msg_buf);
-    booz_gps_state.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(booz_gps_skytraq.msg_buf);
-    booz_gps_state.hmsl        = SKYTRAQ_NAVIGATION_DATA_ASL(booz_gps_skytraq.msg_buf);
-    //   pacc;
-    //   sacc;
-    //     booz_gps_state.pdop       = SKYTRAQ_NAVIGATION_DATA_PDOP(booz_gps_skytraq.msg_buf);
-    booz_gps_state.num_sv      = SKYTRAQ_NAVIGATION_DATA_NumSV(booz_gps_skytraq.msg_buf);
-    booz_gps_state.fix         = SKYTRAQ_NAVIGATION_DATA_FixMode(booz_gps_skytraq.msg_buf);
-    booz_gps_state.tow         = SKYTRAQ_NAVIGATION_DATA_TOW(booz_gps_skytraq.msg_buf);
-    //DEBUG_S2_TOGGLE();
-
-#ifdef GPS_LED
-    if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
-      LED_ON(GPS_LED);
-    }
-    else {
-      LED_TOGGLE(GPS_LED);
-    }
-#endif
-  }
-
-  //DEBUG_S1_OFF();
-}
-
-void booz_gps_skytraq_parse(uint8_t c) {
-  if (booz_gps_skytraq.status < GOT_PAYLOAD)
-    booz_gps_skytraq.checksum ^= c;
-  switch (booz_gps_skytraq.status) {
-  case UNINIT:
-    if (c == SKYTRAQ_SYNC1)
-      booz_gps_skytraq.status = GOT_SYNC1;
-    break;
-  case GOT_SYNC1:
-    if (c != SKYTRAQ_SYNC2) {
-      booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC;
-      goto error;
-    }
-    booz_gps_skytraq.status = GOT_SYNC2;
-    break;
-  case GOT_SYNC2:
-    booz_gps_skytraq.len = c<<8;
-    booz_gps_skytraq.status = GOT_LEN1;
-    break;
-  case GOT_LEN1:
-    booz_gps_skytraq.len += c;
-    booz_gps_skytraq.status = GOT_LEN2;
-    if (booz_gps_skytraq.len > GPS_SKYTRAQ_MAX_PAYLOAD) {
-      booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_MSG_TOO_LONG;
-      goto error;
-    }
-    break;
-  case GOT_LEN2:
-    booz_gps_skytraq.msg_id = c;
-    booz_gps_skytraq.msg_idx = 0;
-    booz_gps_skytraq.checksum = c;
-    booz_gps_skytraq.status = GOT_ID;
-    break;
-  case GOT_ID:
-    booz_gps_skytraq.msg_buf[booz_gps_skytraq.msg_idx] = c;
-    booz_gps_skytraq.msg_idx++;
-    if (booz_gps_skytraq.msg_idx >= booz_gps_skytraq.len-1) {
-      booz_gps_skytraq.status = GOT_PAYLOAD;
-    }
-    break;
-  case GOT_PAYLOAD:
-    if (c != booz_gps_skytraq.checksum) {
-      booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_CHECKSUM;
-      goto error;
-    }
-    booz_gps_skytraq.status = GOT_CHECKSUM;
-    break;
-  case GOT_CHECKSUM:
-    if (c != SKYTRAQ_SYNC3) {
-      booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_OUT_OF_SYNC;
-      goto error;
-    }
-    booz_gps_skytraq.status = GOT_SYNC3;
-    break;
-  case GOT_SYNC3:
-    booz_gps_skytraq.msg_available = TRUE;
-    goto restart;
-  default:
-    booz_gps_skytraq.error_last = GPS_SKYTRAQ_ERR_UNEXPECTED;
-    goto error;
-  }
-  return;
- error:
-  booz_gps_skytraq.error_cnt++;
- restart:
-  booz_gps_skytraq.status = UNINIT;
-  return;
-}
diff --git a/sw/airborne/obsolete/booz_gps_skytraq.h b/sw/airborne/obsolete/booz_gps_skytraq.h
deleted file mode 100644
index 27f2345764..0000000000
--- a/sw/airborne/obsolete/booz_gps_skytraq.h
+++ /dev/null
@@ -1,87 +0,0 @@
-#ifndef BOOZ_GPS_SKYTRAQ_H
-#define BOOZ_GPS_SKYTRAQ_H
-
-/*
- *
-*
-*/
-
-#define SKYTRAQ_SYNC1 0xA0
-#define SKYTRAQ_SYNC2 0xA1
-
-#define SKYTRAQ_SYNC3 0x0D
-#define SKYTRAQ_SYNC4 0x0A
-
-
-#define SKYTRAQ_ID_NAVIGATION_DATA 0XA8
-
-#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2))
-#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload)   (uint8_t) (*((uint8_t*)_payload+3-2))
-
-//#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)     (uint32_t)(_payload[7] + (((uint32_t)_payload[6])<<8) + (((uint32_t)_payload[5])<<16) + (((uint32_t)_payload[4])<<24))
-#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)     __builtin_bswap32(*(uint32_t*)&_payload[ 6-2])
-#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload)     __builtin_bswap32(*( int32_t*)&_payload[10-2])
-#define SKYTRAQ_NAVIGATION_DATA_LON(_payload)     __builtin_bswap32(*( int32_t*)&_payload[14-2])
-#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload)     __builtin_bswap32(*(uint32_t*)&_payload[18-2])
-#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload)     __builtin_bswap32(*(uint32_t*)&_payload[22-2])
-//#define SKYTRAQ_NAVIGATION_DATA_GDOP(_payload)    __builtin_bswap16(*(uint16_t*)&_payload[26-2])
-//#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload)    __builtin_bswap(*(uint16_t*)&_payload[28-2])
-
-#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload)   __builtin_bswap32(*( int32_t*)&_payload[36-2])
-#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload)   __builtin_bswap32(*( int32_t*)&_payload[40-2])
-#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload)   __builtin_bswap32(*( int32_t*)&_payload[44-2])
-#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload)  __builtin_bswap32(*( int32_t*)&_payload[48-2])
-#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload)  __builtin_bswap32(*( int32_t*)&_payload[52-2])
-#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload)  __builtin_bswap32(*( int32_t*)&_payload[56-2])
-
-
-
-/* last error type */
-#define GPS_SKYTRAQ_ERR_NONE         0
-#define GPS_SKYTRAQ_ERR_OVERRUN      1
-#define GPS_SKYTRAQ_ERR_MSG_TOO_LONG 2
-#define GPS_SKYTRAQ_ERR_CHECKSUM     3
-#define GPS_SKYTRAQ_ERR_OUT_OF_SYNC  4
-#define GPS_SKYTRAQ_ERR_UNEXPECTED   5
-
-#define GPS_SKYTRAQ_MAX_PAYLOAD 255
-struct BoozGpsSkytraq {
-  uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD] __attribute__ ((aligned));
-  bool_t  msg_available;
-  uint8_t msg_id;
-
-  uint8_t  status;
-  uint16_t len;
-  uint8_t  msg_idx;
-  uint8_t  checksum;
-  uint8_t  error_cnt;
-  uint8_t  error_last;
-};
-
-extern struct BoozGpsSkytraq booz_gps_skytraq;
-
-extern void booz_gps_skytraq_read_message(void);
-extern void booz_gps_skytraq_parse(uint8_t c);
-
-//#include "my_debug_servo.h"
-
-#define BoozGpsEvent(_sol_available_callback) {				\
-    if (GpsBuffer()) {							\
-      ReadGpsBuffer();							\
-    }									\
-    if (booz_gps_skytraq.msg_available) {				\
-      booz_gps_skytraq_read_message();					\
-      if (booz_gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) {	\
-        if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)                     \
-          booz_gps_state.lost_counter = 0;				\
-    _sol_available_callback();					\
-      }									\
-      booz_gps_skytraq.msg_available = FALSE;				\
-    }									\
-  }
-
-#define ReadGpsBuffer() {						\
-    while (GpsLink(ChAvailable())&&!booz_gps_skytraq.msg_available)	\
-      booz_gps_skytraq_parse(GpsLink(Getch()));				\
-  }
-#endif /* BOOZ_GPS_SKYTRAQ_H */
diff --git a/sw/airborne/obsolete/booz_gps_ubx.c b/sw/airborne/obsolete/booz_gps_ubx.c
deleted file mode 100644
index b8f26e1142..0000000000
--- a/sw/airborne/obsolete/booz_gps_ubx.c
+++ /dev/null
@@ -1,171 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-
-#include "booz_gps.h"
-
-#include "led.h"
-
-/* parser status */
-#define UNINIT        0
-#define GOT_SYNC1     1
-#define GOT_SYNC2     2
-#define GOT_CLASS     3
-#define GOT_ID        4
-#define GOT_LEN1      5
-#define GOT_LEN2      6
-#define GOT_PAYLOAD   7
-#define GOT_CHECKSUM1 8
-
-/* last error type */
-#define GPS_UBX_ERR_NONE         0
-#define GPS_UBX_ERR_OVERRUN      1
-#define GPS_UBX_ERR_MSG_TOO_LONG 2
-#define GPS_UBX_ERR_CHECKSUM     3
-#define GPS_UBX_ERR_UNEXPECTED   4
-#define GPS_UBX_ERR_OUT_OF_SYNC  5
-
-struct BoosGpsUbx booz_gps_ubx;
-
-void booz_gps_impl_init(void) {
-   booz_gps_ubx.status = UNINIT;
-   booz_gps_ubx.msg_available = FALSE;
-   booz_gps_ubx.error_cnt = 0;
-   booz_gps_ubx.error_last = GPS_UBX_ERR_NONE;
-}
-
-
-void booz_gps_ubx_read_message(void) {
-
-  if (booz_gps_ubx.msg_class == UBX_NAV_ID) {
-    if (booz_gps_ubx.msg_id == UBX_NAV_SOL_ID) {
-      booz_gps_state.fix        = UBX_NAV_SOL_GPSfix(booz_gps_ubx.msg_buf);
-      booz_gps_state.ecef_pos.x = UBX_NAV_SOL_ECEF_X(booz_gps_ubx.msg_buf);
-      booz_gps_state.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(booz_gps_ubx.msg_buf);
-      booz_gps_state.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(booz_gps_ubx.msg_buf);
-      booz_gps_state.pacc       = UBX_NAV_SOL_Pacc(booz_gps_ubx.msg_buf);
-      booz_gps_state.ecef_vel.x = UBX_NAV_SOL_ECEFVX(booz_gps_ubx.msg_buf);
-      booz_gps_state.ecef_vel.y = UBX_NAV_SOL_ECEFVY(booz_gps_ubx.msg_buf);
-      booz_gps_state.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(booz_gps_ubx.msg_buf);
-      booz_gps_state.sacc       = UBX_NAV_SOL_Sacc(booz_gps_ubx.msg_buf);
-      booz_gps_state.pdop       = UBX_NAV_SOL_PDOP(booz_gps_ubx.msg_buf);
-      booz_gps_state.num_sv     = UBX_NAV_SOL_numSV(booz_gps_ubx.msg_buf);
-#ifdef GPS_LED
-      if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
-        LED_ON(GPS_LED);
-      }
-      else {
-        LED_TOGGLE(GPS_LED);
-      }
-#endif
-    } else if (booz_gps_ubx.msg_id == UBX_NAV_POSLLH_ID) {
-      booz_gps_state.lla_pos.lat = UBX_NAV_POSLLH_LAT(booz_gps_ubx.msg_buf);
-      booz_gps_state.lla_pos.lon = UBX_NAV_POSLLH_LON(booz_gps_ubx.msg_buf);
-      booz_gps_state.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(booz_gps_ubx.msg_buf) / 10;
-      booz_gps_state.hmsl        = UBX_NAV_POSLLH_HMSL(booz_gps_ubx.msg_buf) / 10;
-    }
-  }
-}
-
-
-
-/* UBX parsing */
-
-void booz_gps_ubx_parse( uint8_t c ) {
-  if (booz_gps_ubx.status < GOT_PAYLOAD) {
-    booz_gps_ubx.ck_a += c;
-    booz_gps_ubx.ck_b += booz_gps_ubx.ck_a;
-  }
-  switch (booz_gps_ubx.status) {
-  case UNINIT:
-    if (c == UBX_SYNC1)
-      booz_gps_ubx.status++;
-    break;
-  case GOT_SYNC1:
-    if (c != UBX_SYNC2) {
-      booz_gps_ubx.error_last = GPS_UBX_ERR_OUT_OF_SYNC;
-      goto error;
-    }
-    booz_gps_ubx.ck_a = 0;
-    booz_gps_ubx.ck_b = 0;
-    booz_gps_ubx.status++;
-    break;
-  case GOT_SYNC2:
-    if (booz_gps_ubx.msg_available) {
-      /* Previous message has not yet been parsed: discard this one */
-      booz_gps_ubx.error_last = GPS_UBX_ERR_OVERRUN;
-      goto error;
-    }
-    booz_gps_ubx.msg_class = c;
-    booz_gps_ubx.status++;
-    break;
-  case GOT_CLASS:
-    booz_gps_ubx.msg_id = c;
-    booz_gps_ubx.status++;
-    break;
-  case GOT_ID:
-    booz_gps_ubx.len = c;
-    booz_gps_ubx.status++;
-    break;
-  case GOT_LEN1:
-    booz_gps_ubx.len |= (c<<8);
-    if (booz_gps_ubx.len > GPS_UBX_MAX_PAYLOAD) {
-      booz_gps_ubx.error_last = GPS_UBX_ERR_MSG_TOO_LONG;
-      goto error;
-    }
-    booz_gps_ubx.msg_idx = 0;
-    booz_gps_ubx.status++;
-    break;
-  case GOT_LEN2:
-    booz_gps_ubx.msg_buf[booz_gps_ubx.msg_idx] = c;
-    booz_gps_ubx.msg_idx++;
-    if (booz_gps_ubx.msg_idx >= booz_gps_ubx.len) {
-      booz_gps_ubx.status++;
-    }
-    break;
-  case GOT_PAYLOAD:
-    if (c != booz_gps_ubx.ck_a) {
-      booz_gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM;
-      goto error;
-    }
-    booz_gps_ubx.status++;
-    break;
-  case GOT_CHECKSUM1:
-    if (c != booz_gps_ubx.ck_b) {
-      booz_gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM;
-      goto error;
-    }
-    booz_gps_ubx.msg_available = TRUE;
-    goto restart;
-    break;
-  default:
-    booz_gps_ubx.error_last = GPS_UBX_ERR_UNEXPECTED;
-    goto error;
-  }
-  return;
- error:
-  booz_gps_ubx.error_cnt++;
- restart:
-  booz_gps_ubx.status = UNINIT;
-  return;
-}
diff --git a/sw/airborne/obsolete/booz_gps_ubx.h b/sw/airborne/obsolete/booz_gps_ubx.h
deleted file mode 100644
index ab2d25f709..0000000000
--- a/sw/airborne/obsolete/booz_gps_ubx.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#ifndef BOOZ_GPS_UBX_H
-#define BOOZ_GPS_UBX_H
-
-#include "ubx_protocol.h"
-
-#define GPS_UBX_MAX_PAYLOAD 255
-struct BoosGpsUbx {
-  bool_t  msg_available;
-  uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__ ((aligned));
-  uint8_t msg_id;
-  uint8_t msg_class;
-
-  uint8_t  status;
-  uint16_t len;
-  uint8_t  msg_idx;
-  uint8_t  ck_a, ck_b;
-  uint8_t  error_cnt;
-  uint8_t  error_last;
-};
-
-extern struct BoosGpsUbx booz_gps_ubx;
-
-extern void booz_gps_ubx_read_message(void);
-extern void booz_gps_ubx_parse(uint8_t c);
-
-#define BoozGpsEvent(_sol_available_callback) {				\
-    if (GpsBuffer()) {							\
-      ReadGpsBuffer();							\
-    }									\
-    if (booz_gps_ubx.msg_available) {					\
-      booz_gps_ubx_read_message();					\
-      if (booz_gps_ubx.msg_class == UBX_NAV_ID &&			\
-      booz_gps_ubx.msg_id == UBX_NAV_SOL_ID) {			\
-        if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D)                     \
-          booz_gps_state.lost_counter = 0;				\
-    _sol_available_callback();					\
-      }									\
-      booz_gps_ubx.msg_available = FALSE;				\
-    }									\
-  }
-
-#define ReadGpsBuffer() {					\
-    while (GpsLink(ChAvailable())&&!booz_gps_ubx.msg_available)	\
-      booz_gps_ubx_parse(GpsLink(Getch()));			\
-  }
-
-#endif /* BOOZ_GPS_UBX_H */
diff --git a/sw/airborne/obsolete/gps.c b/sw/airborne/obsolete/gps.c
deleted file mode 100644
index 6b058fb07f..0000000000
--- a/sw/airborne/obsolete/gps.c
+++ /dev/null
@@ -1,104 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2005  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file gps.c
- *  \brief GPS hardware independent handling
- *
- */
-
-#include <stdlib.h>
-
-#include "gps.h"
-#include "latlong.h"
-#ifndef FMS_PERIODIC_FREQ
-#include "sys_time.h"
-#endif
-#include "generated/airframe.h"
-#include "generated/periodic.h"
-
-#ifdef USE_USB_SERIAL
-#include "mcu_periph/usb_serial.h"
-#endif
-
-
-/** For backward compatibility */
-#ifndef DOWNLINK_GPS_DEVICE
-#define DOWNLINK_GPS_DEVICE DOWNLINK_AP_DEVICE
-#endif
-
-
-
-#ifdef FMS_PERIODIC_FREQ
-#include "messages2.h"
-#include "firmwares/beth/overo_gcs_com.h"
-#define DefaultChannel gcs_com.udp_transport
-#else
-#define DOWNLINK_DEVICE DOWNLINK_GPS_DEVICE
-#include "messages.h"
-#include "downlink.h"
-#endif
-
-uint16_t last_gps_msg_t;	/** cputime of the last gps message */
-bool_t gps_verbose_downlink;
-
-#ifdef UGEAR
-volatile uint16_t gps_msg_received_counter;
-#endif
-
-void gps_downlink( void ) {
-
-#  ifdef DOWNLINK_GPS_1Hz
-#  warning "Deperecated DOWNLINK_GPS_1Hz: Please Use Telemetry xml to reduce data rate"
-#  endif
-
-
-#ifndef PERIOD_GPS_DefaultChannel_0
-  gps_send();
-#endif
-}
-
-void gps_send( void ) {
-
-  DOWNLINK_SEND_GPS(DefaultChannel, &gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_week, &gps_itow, &gps_utm_zone, &gps_nb_ovrn);
-
-  static uint8_t i;
-  static uint8_t last_cnos[GPS_NB_CHANNELS];
-  if (i == gps_nb_channels) i = 0;
-  if (i < gps_nb_channels && gps_svinfos[i].cno > 0 && gps_svinfos[i].cno != last_cnos[i]) {
-    DOWNLINK_SEND_SVINFO(DefaultChannel, &i, &gps_svinfos[i].svid, &gps_svinfos[i].flags, &gps_svinfos[i].qi, &gps_svinfos[i].cno, &gps_svinfos[i].elev, &gps_svinfos[i].azim);
-    last_cnos[i] = gps_svinfos[i].cno;
-  }
-
-  if (gps_verbose_downlink) {
-    uint8_t j;
-    for(j = 0; j < gps_nb_channels; j++) {
-      uint8_t cno = gps_svinfos[j].cno;
-      if (cno > 0 && j != i && abs(cno-last_cnos[j]) >= 2) {
-        DOWNLINK_SEND_SVINFO(DefaultChannel, &j, &gps_svinfos[j].svid, &gps_svinfos[j].flags, &gps_svinfos[j].qi, &cno, &gps_svinfos[j].elev, &gps_svinfos[j].azim);
-        last_cnos[j] = gps_svinfos[j].cno;
-      }
-    }
-  }
-  i++;
-}
diff --git a/sw/airborne/obsolete/gps.h b/sw/airborne/obsolete/gps.h
deleted file mode 100644
index 03b22300be..0000000000
--- a/sw/airborne/obsolete/gps.h
+++ /dev/null
@@ -1,173 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file gps.h
- *  \brief Device independent GPS code
- *
-*/
-
-#ifndef GPS_H
-#define GPS_H
-
-#include "std.h"
-#include "gps_hw.h"
-
-#ifdef GPS
-#error "The flag GPS has been deprecated. Please replace it with USE_GPS."
-#endif
-
-#ifdef UBX
-#include "gps_ubx.h"
-#elif defined USE_GPS_XSENS
-#include "gps_xsens.h"
-#elif defined SITL
-#define GPS_NB_CHANNELS 16
-#define GpsFixValid() (gps_mode == 3)
-#elif defined UGEAR
-#define GPS_NB_CHANNELS 16
-#else
-#define GPS_NB_CHANNELS 1
-#endif
-
-
-extern uint8_t gps_mode; /* Receiver status */
-extern uint8_t gps_status_flags;
-extern uint8_t gps_sol_flags;
-extern uint16_t gps_week;    /* weeks */
-extern uint32_t gps_itow;    /* ms */
-extern int32_t  gps_alt;    /* above MSL in cm       */
-extern uint16_t gps_speed_3d;  /* cm/s     */
-extern uint16_t gps_gspeed;  /* cm/s     */
-extern int16_t  gps_climb;  /* m/s     */
-extern int16_t  gps_course; /* decideg     */
-extern int32_t gps_utm_east, gps_utm_north; /** cm */
-extern uint8_t gps_utm_zone;
-extern int32_t gps_lat, gps_lon; /* 1e7 deg */
-extern int32_t gps_hmsl;
-extern uint16_t gps_PDOP;
-extern uint32_t gps_Pacc, gps_Sacc;
-extern int32_t gps_ecefVZ;
-extern uint8_t gps_numSV;
-extern uint8_t gps_configuring;
-
-extern uint16_t last_gps_msg_t; /** cputime of the last gps message */
-extern bool_t gps_verbose_downlink;
-
-void gps_init( void );
-void gps_configure( void );
-void parse_gps_msg( void );
-void gps_downlink( void );
-void gps_send( void );
-void gps_configure_uart( void );
-
-
-extern volatile uint8_t gps_msg_received;
-extern bool_t gps_pos_available;
-extern uint8_t gps_nb_ovrn;
-
-#ifdef UGEAR
-extern volatile uint16_t gps_msg_received_counter;
-#endif
-
-/** Number of scanned satellites */
-extern uint8_t gps_nb_channels;
-
-/** Space Vehicle Information */
-struct svinfo {
-  uint8_t svid;
-  uint8_t flags;
-  uint8_t qi;
-  uint8_t cno;
-  int8_t elev; /** deg */
-  int16_t azim; /** deg */
-};
-
-extern struct svinfo gps_svinfos[GPS_NB_CHANNELS];
-
-#ifndef SITL
-#include "mcu_periph/uart.h"
-
-#define __GpsLink(dev, _x) dev##_x
-#define _GpsLink(dev, _x)  __GpsLink(dev, _x)
-#define GpsLink(_x) _GpsLink(GPS_LINK, _x)
-
-#define GpsBuffer() GpsLink(ChAvailable())
-#define ReadGpsBuffer() { while (GpsLink(ChAvailable())&&!gps_msg_received) parse_ubx(GpsLink(Getch())); }
-#define GpsUartSend1(c) GpsLink(Transmit(c))
-#define GpsUartSetBaudrate(_b) GpsLink(SetBaudrate(_b))
-#define GpsUartRunning GpsLink(TxRunning)
-#define GpsUartSendMessage GpsLink(SendMessage)
-
-
-#endif /** !SITL */
-
-#ifdef GPS_LED
-#define GpsToggleLed() LED_TOGGLE(GPS_LED)
-#else
-#define GpsToggleLed() {}
-#endif
-
-#if defined(USE_GPS) || defined(USE_GPS_XSENS) || defined(SITL)
-#  define GpsTimeoutError (cpu_time_sec - last_gps_msg_t > FAILSAFE_DELAY_WITHOUT_GPS)
-#else
-#  define GpsTimeoutError 1
-#endif
-
-#define UseGpsPosNoSend(_callback) {			\
-    if (GpsFixValid()) {			\
-      last_gps_msg_t = cpu_time_sec;		\
-      _callback();				\
-      GpsToggleLed();				\
-    }						\
-  }
-
-#ifdef GPS_CONFIGURE
-#define GpsParseOrConfigure() {			\
-    if (gps_configuring)			\
-      gps_configure();				\
-    else					\
-      parse_gps_msg();				\
-  }
-#else
-#define GpsParseOrConfigure() parse_gps_msg()
-#endif
-
-
-#define GpsEventCheckAndHandle(_callback, _verbose) {   \
-    if (GpsBuffer()) {                                  \
-      ReadGpsBuffer();                                  \
-    }                                                   \
-    if (gps_msg_received) {                             \
-      GpsParseOrConfigure();                            \
-      gps_msg_received = FALSE;                         \
-      if (gps_pos_available) {                          \
-        gps_verbose_downlink = _verbose;                \
-        UseGpsPos(_callback);                           \
-        gps_pos_available = FALSE;                      \
-      }                                                 \
-    }                                                   \
-  }
-
-
-#endif /* GPS_H */
diff --git a/sw/airborne/obsolete/gps_ubx.c b/sw/airborne/obsolete/gps_ubx.c
deleted file mode 100644
index d351c1c26b..0000000000
--- a/sw/airborne/obsolete/gps_ubx.c
+++ /dev/null
@@ -1,418 +0,0 @@
-/*
- * Paparazzi mcu0 $Id$
- *
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file gps_ubx.c
- * \brief Parser for the UBX protocol (u-blox.com devices)
- */
-
-#include <inttypes.h>
-#include <string.h>
-#include <math.h>
-
-#ifdef FMS_PERIODIC_FREQ
-//for printf
-#include <stdio.h>
-//for baudrate
-#include "fms_serial_port.h"
-#endif /* FMS_PERIODIC_FREQ */
-
-#include "generated/flight_plan.h"
-#include "mcu_periph/uart.h"
-#include "gps.h"
-#include "gps_ubx.h"
-#include "subsystems/nav.h"
-#include "latlong.h"
-
-#ifdef GPS_TIMESTAMP
-#include "sys_time.h"
-#define MSEC_PER_WEEK (1000*60*60*24*7)
-#endif
-
-#define UbxInitCheksum() { send_ck_a = send_ck_b = 0; }
-#define UpdateChecksum(c) { send_ck_a += c; send_ck_b += send_ck_a; }
-#define UbxTrailer() { GpsUartSend1(send_ck_a);  GpsUartSend1(send_ck_b); GpsUartSendMessage(); }
-
-#define UbxSend1(c) { uint8_t i8=c; GpsUartSend1(i8); UpdateChecksum(i8); }
-#define UbxSend2(c) { uint16_t i16=c; UbxSend1(i16&0xff); UbxSend1(i16 >> 8); }
-#define UbxSend1ByAddr(x) { UbxSend1(*x); }
-#define UbxSend2ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); }
-#define UbxSend4ByAddr(x) { UbxSend1(*x); UbxSend1(*(x+1)); UbxSend1(*(x+2)); UbxSend1(*(x+3)); }
-
-#define UbxHeader(nav_id, msg_id, len) { \
-  GpsUartSend1(UBX_SYNC1); \
-  GpsUartSend1(UBX_SYNC2); \
-  UbxInitCheksum(); \
-  UbxSend1(nav_id); \
-  UbxSend1(msg_id); \
-  UbxSend2(len); \
-}
-
-
-/** Includes macros generated from ubx.xml */
-#include "ubx_protocol.h"
-
-
-uint16_t gps_week;
-uint32_t gps_itow;
-#ifdef GPS_TIMESTAMP
-uint32_t gps_t0;
-uint32_t gps_t0_itow;
-uint32_t gps_t0_frac;
-#endif
-int32_t gps_alt;
-uint16_t gps_speed_3d;
-uint16_t gps_gspeed;
-int16_t gps_climb;
-int16_t gps_course;
-int32_t gps_utm_east, gps_utm_north;
-uint8_t gps_utm_zone;
-uint8_t gps_mode;
-uint8_t gps_status_flags;
-uint8_t gps_sol_flags;
-volatile bool_t gps_msg_received;
-bool_t gps_pos_available;
-uint8_t ubx_id, ubx_class;
-uint16_t ubx_len;
-int32_t gps_lat, gps_lon;
-int32_t gps_hmsl;
-uint16_t gps_reset;
-
-uint16_t gps_PDOP;
-uint32_t gps_Pacc, gps_Sacc;
-int32_t gps_ecefVZ;
-uint8_t gps_numSV;
-
-#define UTM_HEM_NORTH 0
-#define UTM_HEM_SOUTH 1
-
-#define UBX_MAX_PAYLOAD 255
-
-uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD] __attribute__ ((aligned));
-
-#define UNINIT        0
-#define GOT_SYNC1     1
-#define GOT_SYNC2     2
-#define GOT_CLASS     3
-#define GOT_ID        4
-#define GOT_LEN1      5
-#define GOT_LEN2      6
-#define GPS_UBX_GOT_PAYLOAD   7
-#define GOT_CHECKSUM1 8
-
-static uint8_t  ubx_status;
-static uint8_t  ubx_msg_idx;
-static uint8_t ck_a, ck_b;
-uint8_t send_ck_a, send_ck_b;
-
-bool_t gps_configuring;
-#ifdef GPS_CONFIGURE
-static uint8_t gps_status_config;
-#endif
-
-void gps_init( void ) {
-  ubx_status = UNINIT;
-#ifdef GPS_CONFIGURE
-  gps_status_config = 0;
-  gps_configuring = TRUE;
-#endif
-}
-
-#define UBX_PROTO_MASK  0x0001
-#define NMEA_PROTO_MASK 0x0002
-#define RTCM_PROTO_MASK 0x0004
-
-#define GPS_PORT_DDC   0x00
-#define GPS_PORT_UART1 0x01
-#define GPS_PORT_UART2 0x02
-#define GPS_PORT_USB   0x03
-#define GPS_PORT_SPI   0x04
-
-
-#ifdef GPS_CONFIGURE
-/* GPS dynamic configuration */
-
-
-#ifndef GPS_PORT_ID
-#define GPS_PORT_ID GPS_PORT_UART1
-#endif
-
-
-/* Configure the GPS baud rate using the current uart baud rate. Busy
-   wait for the end of the transmit. Then, BEFORE waiting for the ACK,
-   change the uart rate. */
-#if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2
-void gps_configure_uart ( void ) {
-#ifdef FMS_PERIODIC_FREQ
-  UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
-  uint8_t loop=0;
-  while (GpsUartRunning) {
-    //doesn't work unless some printfs are used, so :
-    if (loop<9) {
-      printf("."); loop++;
-    } else {
-      printf("\b"); loop--;
-    }
-  }
-#else
-  UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
-  while (GpsUartRunning); /* FIXME */
-#endif
-
-  GpsUartSetBaudrate(GPS_BAUD);
-}
-#endif
-
-#if GPS_PORT_ID == GPS_PORT_DDC
-void gps_configure_uart ( void ) {
-  UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
-}
-#endif
-
-#define IGNORED 0
-#define RESERVED 0
-
-#ifdef USER_GPS_CONFIGURE
-#include USER_GPS_CONFIGURE
-#else
-static bool_t user_gps_configure(bool_t cpt) {
-  switch (cpt) {
-  case 0:
-    //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
-    //UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
-    UbxSend_CFG_NAV5(0x05, NAV5_DYN_AIRBORNE_2G, NAV5_3D_ONLY, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, IGNORED, RESERVED, RESERVED, RESERVED, RESERVED);
-    break;
-  case 1:
-    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);
-    break;
-  case 2:
-    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
-    break;
-  case 3:
-    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_STATUS_ID, 0, 1, 0, 0);
-    break;
-  case 4:
-    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 0, 4, 0, 0);
-    break;
-  case 5:
-    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SOL_ID, 0, 8, 0, 0);
-    break;
-  case 6:
-    UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
-    break;
-  case 7:
-    UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);
-    return FALSE;
-  }
-  return TRUE; /* Continue, except for the last case */
-}
-#endif // ! USER_GPS_CONFIGURE
-
-/* GPS configuration. Must be called on ack message reception while
-   gps_status_config < GPS_CONFIG_DONE */
-void gps_configure ( void ) {
-  if (ubx_class == UBX_ACK_ID) {
-    if (ubx_id == UBX_ACK_ACK_ID) {
-      gps_status_config++;
-    }
-  }
-  gps_configuring = user_gps_configure(gps_status_config);
-}
-#endif /* GPS_CONFIGURE */
-
-void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
-#ifdef GPS_LINK
-  UbxSend_CFG_RST(bbr, reset_mode, 0x00);
-#endif /* else less harmful for HITL */
-}
-
-
-struct svinfo gps_svinfos[GPS_NB_CHANNELS];
-uint8_t gps_nb_channels;
-
-void parse_gps_msg( void ) {
-  if (ubx_class == UBX_NAV_ID) {
-    if (ubx_id == UBX_NAV_STATUS_ID) {
-      gps_mode = UBX_NAV_STATUS_GPSfix(ubx_msg_buf);
-      gps_status_flags = UBX_NAV_STATUS_Flags(ubx_msg_buf);
-      gps_sol_flags = UBX_NAV_SOL_Flags(ubx_msg_buf);
-#ifdef GPS_USE_LATLONG
-      /* Computes from (lat, long) in the referenced UTM zone */
-    } else if (ubx_id == UBX_NAV_POSLLH_ID) {
-      gps_lat = UBX_NAV_POSLLH_LAT(ubx_msg_buf);
-      gps_lon = UBX_NAV_POSLLH_LON(ubx_msg_buf);
-      gps_hmsl = UBX_NAV_POSLLH_HMSL(ubx_msg_buf);
-
-      latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
-
-      gps_utm_east = latlong_utm_x * 100;
-      gps_utm_north = latlong_utm_y * 100;
-      gps_alt = UBX_NAV_POSLLH_HMSL(ubx_msg_buf) / 10;
-      gps_utm_zone = nav_utm_zone0;
-#else
-    } else if (ubx_id == UBX_NAV_POSUTM_ID) {
-      gps_utm_east = UBX_NAV_POSUTM_EAST(ubx_msg_buf);
-      gps_utm_north = UBX_NAV_POSUTM_NORTH(ubx_msg_buf);
-      uint8_t hem = UBX_NAV_POSUTM_HEM(ubx_msg_buf);
-      if (hem == UTM_HEM_SOUTH)
-        gps_utm_north -= 1000000000; /* Subtract false northing: -10000km */
-      gps_alt = UBX_NAV_POSUTM_ALT(ubx_msg_buf);
-      gps_utm_zone = UBX_NAV_POSUTM_ZONE(ubx_msg_buf);
-#endif
-    } else if (ubx_id == UBX_NAV_VELNED_ID) {
-      gps_speed_3d = UBX_NAV_VELNED_Speed(ubx_msg_buf);
-      gps_gspeed = UBX_NAV_VELNED_GSpeed(ubx_msg_buf);
-      gps_climb = - UBX_NAV_VELNED_VEL_D(ubx_msg_buf);
-      gps_course = UBX_NAV_VELNED_Heading(ubx_msg_buf) / 10000;
-      gps_itow = UBX_NAV_VELNED_ITOW(ubx_msg_buf);
-
-      gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */
-    } else if (ubx_id == UBX_NAV_SOL_ID) {
-#ifdef GPS_TIMESTAMP
-      /* get hardware clock ticks */
-      gps_t0 = T0TC;
-      /* set receive time */
-      gps_t0_itow = UBX_NAV_SOL_ITOW(ubx_msg_buf);
-      gps_t0_frac = UBX_NAV_SOL_Frac(ubx_msg_buf);
-#endif
-      gps_mode = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
-      gps_PDOP = UBX_NAV_SOL_PDOP(ubx_msg_buf);
-      gps_Pacc = UBX_NAV_SOL_Pacc(ubx_msg_buf);
-      gps_ecefVZ = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf);
-      gps_Sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf);
-      gps_numSV = UBX_NAV_SOL_numSV(ubx_msg_buf);
-      gps_week = UBX_NAV_SOL_week(ubx_msg_buf);
-    } else if (ubx_id == UBX_NAV_SVINFO_ID) {
-      gps_nb_channels = Min(UBX_NAV_SVINFO_NCH(ubx_msg_buf), GPS_NB_CHANNELS);
-      uint8_t i;
-      for(i = 0; i < gps_nb_channels; i++) {
-        gps_svinfos[i].svid = UBX_NAV_SVINFO_SVID(ubx_msg_buf, i);
-        gps_svinfos[i].flags = UBX_NAV_SVINFO_Flags(ubx_msg_buf, i);
-        gps_svinfos[i].qi = UBX_NAV_SVINFO_QI(ubx_msg_buf, i);
-        gps_svinfos[i].cno = UBX_NAV_SVINFO_CNO(ubx_msg_buf, i);
-        gps_svinfos[i].elev = UBX_NAV_SVINFO_Elev(ubx_msg_buf, i);
-        gps_svinfos[i].azim = UBX_NAV_SVINFO_Azim(ubx_msg_buf, i);
-      }
-    }
-  }
-}
-
-
-uint8_t gps_nb_ovrn;
-
-
-void parse_ubx( uint8_t c ) {
-  if (ubx_status < GPS_UBX_GOT_PAYLOAD) {
-    ck_a += c;
-    ck_b += ck_a;
-  }
-  switch (ubx_status) {
-  case UNINIT:
-    if (c == UBX_SYNC1)
-      ubx_status++;
-    break;
-  case GOT_SYNC1:
-    if (c != UBX_SYNC2)
-      goto error;
-    ck_a = 0;
-    ck_b = 0;
-    ubx_status++;
-    break;
-  case GOT_SYNC2:
-    if (gps_msg_received) {
-      /* Previous message has not yet been parsed: discard this one */
-      gps_nb_ovrn++;
-      goto error;
-    }
-    ubx_class = c;
-    ubx_status++;
-    break;
-  case GOT_CLASS:
-    ubx_id = c;
-    ubx_status++;
-    break;
-  case GOT_ID:
-    ubx_len = c;
-    ubx_status++;
-    break;
-  case GOT_LEN1:
-    ubx_len |= (c<<8);
-    if (ubx_len > UBX_MAX_PAYLOAD)
-      goto error;
-    ubx_msg_idx = 0;
-    ubx_status++;
-    break;
-  case GOT_LEN2:
-    ubx_msg_buf[ubx_msg_idx] = c;
-    ubx_msg_idx++;
-    if (ubx_msg_idx >= ubx_len) {
-      ubx_status++;
-    }
-    break;
-  case GPS_UBX_GOT_PAYLOAD:
-    if (c != ck_a)
-      goto error;
-    ubx_status++;
-    break;
-  case GOT_CHECKSUM1:
-    if (c != ck_b)
-      goto error;
-    gps_msg_received = TRUE;
-    goto restart;
-    break;
-  default:
-    goto error;
-  }
-  return;
- error:
- restart:
-  ubx_status = UNINIT;
-  return;
-}
-
-#ifdef GPS_TIMESTAMP
-
-#ifndef PCLK
-#error unknown PCLK frequency
-#endif
-
-uint32_t itow_from_ticks(uint32_t clock_ticks)
-{
-  uint32_t clock_delta;
-  uint32_t time_delta;
-  uint32_t itow_now;
-
-  if (clock_ticks < gps_t0) {
-    clock_delta = (0xFFFFFFFF - clock_ticks) + gps_t0 + 1;
-  } else {
-    clock_delta = clock_ticks - gps_t0;
-  }
-
-  time_delta = MSEC_OF_SYS_TICS(clock_delta);
-
-  itow_now = gps_t0_itow + time_delta;
-  if (itow_now > MSEC_PER_WEEK) itow_now %= MSEC_PER_WEEK;
-
-  return itow_now;
-}
-#endif
diff --git a/sw/airborne/obsolete/gps_ubx.h b/sw/airborne/obsolete/gps_ubx.h
deleted file mode 100644
index e75ebfda9f..0000000000
--- a/sw/airborne/obsolete/gps_ubx.h
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
- * Paparazzi autopilot $Id$
- *
- * Copyright (C) 2004-2006  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file gps_ubx.h
- * \brief UBX protocol specific code
- *
-*/
-
-
-#ifndef UBX_H
-#define UBX_H
-
-#define GPS_NB_CHANNELS 16
-
-extern uint16_t gps_reset;
-
-extern uint8_t ubx_id, ubx_class;
-extern uint16_t ubx_len;
-#define UBX_MAX_PAYLOAD 255
-extern uint8_t ubx_msg_buf[UBX_MAX_PAYLOAD];
-
-/** The function to be called when a characted friom the device is available */
-extern void parse_ubx( uint8_t c );
-
-#define GpsParse(_gps_buffer, _gps_buffer_size) { \
-  uint8_t i; \
-  for(i = 0; i < _gps_buffer_size; i++) { \
-    parse_ubx(_gps_buffer[i]); \
-  } \
-}
-
-#define GpsFixValid() (gps_mode == 3)
-
-#define CFG_RST_BBR_Hotstart  0x0000
-#define CFG_RST_BBR_Warmstart 0x0001
-#define CFG_RST_BBR_Coldstart 0xffff
-
-#define CFG_RST_Reset_Hardware 0x00
-#define CFG_RST_Reset_Controlled 0x01
-#define CFG_RST_Reset_Controlled_GPS_only 0x02
-#define CFG_RST_Reset_Controlled_GPS_stop 0x08
-#define CFG_RST_Reset_Controlled_GPS_start 0x09
-
-#define NAV_DYN_STATIONARY  1
-#define NAV_DYN_PEDESTRIAN  2
-#define NAV_DYN_AUTOMOTIVE  3
-#define NAV_DYN_SEA         4
-#define NAV_DYN_AIRBORNE_1G 5
-#define NAV_DYN_AIRBORNE_2G 6
-#define NAV_DYN_AIRBORNE_4G 7
-
-#define NAV5_DYN_PORTABLE    0
-#define NAV5_DYN_FIXED       1
-#define NAV5_DYN_STATIONARY  2
-#define NAV5_DYN_PEDESTRIAN  3
-#define NAV5_DYN_AUTOMOTIVE  4
-#define NAV5_DYN_SEA         5
-#define NAV5_DYN_AIRBORNE_1G 6
-#define NAV5_DYN_AIRBORNE_2G 7
-#define NAV5_DYN_AIRBORNE_4G 8
-
-#define NAV5_2D_ONLY 1
-#define NAV5_3D_ONLY 2
-#define NAV5_AUTO    3
-
-
-extern void ubxsend_cfg_rst(uint16_t, uint8_t);
-
-#define gps_ubx_Reset(_val) { \
-  gps_reset = _val; \
-  if (gps_reset > CFG_RST_BBR_Warmstart) \
-    gps_reset = CFG_RST_BBR_Coldstart; \
-  ubxsend_cfg_rst(gps_reset, CFG_RST_Reset_Controlled); \
-}
-
-#ifdef GPS_TIMESTAMP
-uint32_t itow_from_ticks(uint32_t clock_ticks);
-#endif
-
-#endif /* UBX_H */
diff --git a/sw/airborne/obsolete/ins_osam_ugear.c b/sw/airborne/obsolete/ins_osam_ugear.c
deleted file mode 100644
index adc3bdd00c..0000000000
--- a/sw/airborne/obsolete/ins_osam_ugear.c
+++ /dev/null
@@ -1,327 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003-2006  Haiyang Chao
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file osam_imu_ugear.c
- *  \brief Communication with any IMU through serial communication (originally gps uart)
- * This file is first generated by Haiyang Chao on 20080507 for communication.
- */
-
-#include <stdlib.h>
-#include <string.h>
-
-#include "ins_osam_ugear.h"
-#include "gps.h"
-#include "gps_ubx.h"
-#include "latlong.h"
-#include "sys_time.h"
-#include "airframe.h"
-#include "generated/subsystems/nav.h"
-#include "estimator.h"
-#include "xsens_protocol.h"
-
-#define UNINIT      0
-#define GOT_SYNC1   1
-#define GOT_SYNC2   2
-#define GOT_ID          3
-#define GOT_LEN         4
-#define GOT_PAYLOAD     5
-#define GOT_CHECKSUM1   6
-#define GOT_CHECKSUM2   7
-
-#define UGEAR_SYNC1 0x93
-#define UGEAR_SYNC2 0xE0
-/*#define UGEAR_MAX_PAYLOAD 24*/
-#define UGEAR_MAX_PAYLOAD 40
-#define IMU_PACKET_SIZE 12
-#define RAD2DEG 57.3
-#define WrapUp(x) (x < 0 ? x+3600 : x)
-
-/* from main_ap.c */
-extern bool_t launch;
-
-/* variable defined for dlsetting generated by Haiyang 20080717*/
-bool_t imu1_ir0 = 0;
-float fd_alpha = 0;
-float fi_alpha = 0;
-float ins_roll_neutral;
-float ins_pitch_neutral;
-
-/* variable definition copied from gps_ubx.c 20080508*/
-uint16_t gps_week;
-uint32_t gps_itow;
-int32_t gps_alt;
-uint16_t gps_gspeed;
-int16_t gps_climb;
-int16_t gps_course;
-int32_t gps_utm_east, gps_utm_north;
-uint8_t gps_utm_zone;
-uint8_t gps_mode;
-volatile bool_t gps_msg_received;
-bool_t gps_pos_available;
-uint8_t ugear_id, ugear_class;
-int32_t gps_lat, gps_lon;
-uint16_t gps_reset;
-
-uint16_t gps_PDOP;
-uint32_t gps_Pacc, gps_Sacc;
-uint8_t gps_numSV;
-/* variable definition copied from gps_ubx.c20080508*/
-
-/* variable definition copied from gps.c20080515*/
-uint8_t gps_nb_ovrn;
-struct svinfo gps_svinfos[GPS_NB_CHANNELS];
-uint8_t gps_nb_channels;
-
-/* variable definition copied from gps.c20080515*/
-
-//uint16_t last_gps_msg_t;	/** cputime of the last gps message */
-
-volatile bool_t ugear_msg_received;
-
-static uint8_t  ugear_status;
-static uint8_t  ugear_type;
-static uint8_t  ugear_len;
-static uint8_t  ugear_msg_idx;
-static uint8_t ck_a, ck_b;
-
-int16_t ugear_phi;
-int16_t ugear_psi;
-int16_t ugear_theta;
-
-int16_t gps_ve;
-int16_t gps_vn;
-int16_t gps_vd;
-/* added 20080522 for debugging*/
-int16_t ugear_debug1;
-int16_t ugear_debug2;
-int16_t ugear_debug3;
-int32_t ugear_debug4;
-int32_t ugear_debug5;
-int32_t ugear_debug6;
-
-//bool_t gps_verbose_downlink;
-
-uint8_t ugear_msg_buf[UGEAR_MAX_PAYLOAD] __attribute__ ((aligned));
-
-/*The following definition is for ugear data 20080509, modified on 0512 Haiyang*/
-struct imu {
-   int32_t phi,the,psi;          /* attitudes             */
-};
-
-struct gps {
-   int32_t lon,lat,alt;
-   int16_t ve,vn,vd;
-};
-
-struct imu imupacket;
-struct gps gpspacket;
-/*The above definition is for ugear data 20080509 Haiyang*/
-
-void ugear_init( void ) {
-  ugear_status = UNINIT;
-  ugear_phi = 0;
-  ugear_psi = 0;
-  ugear_theta = 0;
-  ugear_debug2 = 0;
-  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
-  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
-}
-
-void parse_ugear( uint8_t c ) {
-  /*checksum go first*/
-  if (ugear_status < GOT_PAYLOAD) {
-    ck_a += c;
-    ck_b += ck_a;
-  }
-  switch (ugear_status) {
-  case UNINIT:
-    if (c == UGEAR_SYNC1)
-      ugear_status++;
-    break;
-  case GOT_SYNC1:
-    if (c != UGEAR_SYNC2)
-      goto error;
-    ck_a = 0;
-    ck_b = 0;
-    ugear_status++;
-    break;
-  case GOT_SYNC2:
-    if (ugear_msg_received) {
-      /* Previous message has not yet been parsed: discard this one */
-      goto error;
-    }
-    ugear_type = c;
-    ugear_status++;
-    //ugear_theta = 30; // for debug
-    if (ugear_type > 2)
-    goto restart;
-    break;
-  case GOT_ID:
-    ugear_len = c;
-    ugear_msg_idx = 0;
-    ugear_status++;
-    break;
-  case GOT_LEN:
-    ugear_msg_buf[ugear_msg_idx] = c;
-    ugear_msg_idx++;
-    if (ugear_msg_idx >= ugear_len) {
-      ugear_status++;
-    }
-    break;
-  case GOT_PAYLOAD:
-    if (c != ck_a)
-      goto error;
-    ugear_status++;
-    break;
-  case GOT_CHECKSUM1:
-    if (c != ck_b)
-      goto error;
-    ugear_msg_received = TRUE;
-    UgearToggleLed();
-    goto restart;
-    break;
-  }
-  return;
-error:
-restart:
-  ugear_status = UNINIT;
-  return;
-}
-/*
-void decode_imupacket( struct imu *data, uint8_t* buffer){
-
-    data->phi = (double)((((signed char)buffer[25])<<8)|buffer[26]);
-    data->theta = (double)((((signed char)buffer[27])<<8)|buffer[28]);
-    data->psi = (double)((((signed char)buffer[29])<<8)|buffer[30]);
-
-}
-*/
-void parse_ugear_msg( void ){
-
-    float ins_phi, ins_psi, ins_theta;
-
-    switch (ugear_type){
-        case 0:  /*gps*/
-            ugear_debug1 = ugear_debug1+1;
-            gps_lat = UGEAR_NAV_POSLLH_LAT(ugear_msg_buf);
-            gps_lon = UGEAR_NAV_POSLLH_LON(ugear_msg_buf);
-
-            nav_utm_zone0 = (gps_lon/10000000+180) / 6 + 1;
-            latlong_utm_of(RadOfDeg(gps_lat/1e7), RadOfDeg(gps_lon/1e7), nav_utm_zone0);
-            gps_utm_east = latlong_utm_x * 100;
-            gps_utm_north = latlong_utm_y * 100;
-
-            gps_alt = UGEAR_NAV_POSLLH_HEIGHT(ugear_msg_buf);
-            gps_utm_zone = nav_utm_zone0;
-
-            gps_gspeed = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
-                gps_climb = - UGEAR_NAV_POSLLH_VD(ugear_msg_buf);
-                gps_course = UGEAR_NAV_VELNED_Heading(ugear_msg_buf)/10000; /*in decdegree */
-                gps_PDOP = UGEAR_NAV_SOL_PDOP(ugear_msg_buf);
-                gps_Pacc = UGEAR_NAV_SOL_Pacc(ugear_msg_buf);
-                gps_Sacc = UGEAR_NAV_SOL_Sacc(ugear_msg_buf);
-                gps_numSV = UGEAR_NAV_SOL_numSV(ugear_msg_buf);
-            gps_week = 0; // FIXME
-            gps_itow = UGEAR_NAV_VELNED_ITOW(ugear_msg_buf);
-
-            //ugear_debug2 = gps_climb;
-            //ugear_debug4 = (int32_t)(UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf));
-            //ugear_debug5 = UGEAR_NAV_VELNED_GSpeed(ugear_msg_buf);
-            //ugear_debug6 = (int16_t)estimator_phi*100;
-
-            gps_mode = 3;  /*force GPSfix to be valided*/
-            gps_pos_available = TRUE; /* The 3 UBX messages are sent in one rafale */
-            break;
-        case 1:  /*IMU*/
-            ugear_debug2 = ugear_debug2+1;
-            ugear_phi = UGEAR_IMU_PHI(ugear_msg_buf);
-            ugear_psi = UGEAR_IMU_PSI(ugear_msg_buf);
-            ugear_theta = UGEAR_IMU_THE(ugear_msg_buf);
-            ugear_debug4 = (int32_t)ugear_phi;
-            ugear_debug5 = (int32_t)ugear_theta;
-            ugear_debug6 = (int32_t)ugear_psi;
-            ugear_debug3 = 333;
-            ins_phi  = (float)ugear_phi/10000 - ins_roll_neutral;
-            ins_psi = 0;
-            ins_theta  = (float)ugear_theta/10000 - ins_pitch_neutral;
-#ifndef USE_INFRARED
-            EstimatorSetAtt(ins_phi, ins_psi, ins_theta);
-#endif
-            break;
-        case 2:  /*GPS status*/
-//			ugear_debug1 = 2;
-                gps_nb_channels = XSENS_GPSStatus_nch(ugear_msg_buf);
-            uint8_t is;
-            for(is = 0; is < Min(gps_nb_channels, 16); is++) {
-                uint8_t ch = XSENS_GPSStatus_chn(ugear_msg_buf,is);
-                    if (ch > 16) continue;
-                    gps_svinfos[ch].svid = XSENS_GPSStatus_svid(ugear_msg_buf, is);
-                    gps_svinfos[ch].flags = XSENS_GPSStatus_bitmask(ugear_msg_buf, is);
-                    gps_svinfos[ch].qi = XSENS_GPSStatus_qi(ugear_msg_buf, is);
-                    gps_svinfos[ch].cno = XSENS_GPSStatus_cnr(ugear_msg_buf, is);
-                    gps_svinfos[ch].elev = 0;
-                    gps_svinfos[ch].azim = 0;
-            }
-            break;
-        case 3:  /*servo*/
-            break;
-        case 4:  /*health*/
-            break;
-
-    }
-
-}
-
-/* add the following function only to get rid of compilation error in datalink.c 20080608 Haiyang*/
-void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) {
-}
-
-void ugear_event( void ) {
-    if (UgearBuffer()){
-        ReadUgearBuffer();
-    }
-    if (ugear_msg_received){
-        parse_ugear_msg();
-        ugear_msg_received = FALSE;
-        if (gps_pos_available){
-            //gps_downlink();
-            gps_verbose_downlink = !launch;
-            UseGpsPosNoSend(estimator_update_state_gps);
-            gps_msg_received_counter = gps_msg_received_counter+1;
-            #ifdef GX2
-            if (gps_msg_received_counter == 1){
-                gps_send();
-                gps_msg_received_counter = 0;
-            }
-            #endif
-            #ifdef XSENSDL
-            if (gps_msg_received_counter == 25){
-                gps_send();
-                gps_msg_received_counter = 0;
-            }
-            #endif
-            gps_pos_available = FALSE;
-        }
-    }
-}
diff --git a/sw/airborne/obsolete/ins_osam_ugear.h b/sw/airborne/obsolete/ins_osam_ugear.h
deleted file mode 100644
index e120bd7145..0000000000
--- a/sw/airborne/obsolete/ins_osam_ugear.h
+++ /dev/null
@@ -1,94 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2003-2006  Haiyang Chao
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file osam_imu_ugear.h
- *  \brief Communication with any IMU through serial communication (originally gps uart)
- * This file is first generated by Haiyang Chao on 20080507 for communication.
- */
-
-#include "std.h"
-
-void ugear_init( void );
-void parse_ugear(uint8_t c);
-void parse_ugear_msg( void );
-void ugear_event( void );
-
-#define __UgearLink(dev, _x) dev##_x
-#define _UgearLink(dev, _x)  __UgearLink(dev, _x)
-#define UgearLink(_x) _UgearLink(UGEAR_LINK, _x)
-
-#define UgearBuffer() UgearLink(ChAvailable())
-#define ReadUgearBuffer() { while (UgearLink(ChAvailable())&&!ugear_msg_received) parse_ugear(UgearLink(Getch())); }
-
-#ifdef UGEAR_LED
-#define UgearToggleLed() LED_TOGGLE(UGEAR_LED)
-#else
-#define UgearToggleLed() {}
-#endif
-
-#define UGEAR_MAX_PAYLOAD 40
-/*#define UGEAR_MAX_PAYLOAD 24*/ /*Bugs!!! why work for the former version ??20080708*/
-/*#define GPS_NB_CHANNELS 16*/
-
-#define UGEAR_NAV_POSLLH_LAT(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+4))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+4))<<24)
-#define UGEAR_NAV_POSLLH_LON(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload)|*((uint8_t*)_ubx_payload+1)<<8|((int32_t)*((uint8_t*)_ubx_payload+2))<<16|((int32_t)*((uint8_t*)_ubx_payload+3))<<24)
-#define UGEAR_NAV_POSLLH_HEIGHT(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+8)|*((uint8_t*)_ubx_payload+1+8)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+8))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+8))<<24)
-#define UGEAR_NAV_POSLLH_VD(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+12)|*((uint8_t*)_ubx_payload+1+12)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+12))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+12))<<24)
-#define UGEAR_NAV_VELNED_Heading(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+16)|*((uint8_t*)_ubx_payload+1+16)<<8|((int32_t)*((uint8_t*)_ubx_payload+2+16))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+16))<<24)
-#define UGEAR_NAV_SOL_Pacc(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+20)|*((uint8_t*)_ubx_payload+1+20)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+20))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+20))<<24)
-#define UGEAR_NAV_SOL_Sacc(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+24)|*((uint8_t*)_ubx_payload+1+24)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+24))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+24))<<24)
-#define UGEAR_NAV_VELNED_GSpeed(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+28)|*((uint8_t*)_ubx_payload+1+28)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+28))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+28))<<24)
-#define UGEAR_NAV_VELNED_ITOW(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+32)|*((uint8_t*)_ubx_payload+1+32)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+32))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+32))<<24)
-#define UGEAR_NAV_SOL_PDOP(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+36)|*((uint8_t*)_ubx_payload+1+36)<<8)
-#define UGEAR_NAV_SOL_numSV(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+38))
-#define UGEAR_IMU_PHI(_ubx_payload) (int16_t)(*((uint8_t*)_ubx_payload)|*((uint8_t*)_ubx_payload+1)<<8)
-#define UGEAR_IMU_THE(_ubx_payload) (int16_t)(*((uint8_t*)_ubx_payload+2)|*((uint8_t*)_ubx_payload+1+2)<<8)
-#define UGEAR_IMU_PSI(_ubx_payload) (int16_t)(*((uint8_t*)_ubx_payload+4)|*((uint8_t*)_ubx_payload+1+4)<<8)
-
-extern volatile uint8_t ugear_msg_received;
-
-/* variable defined for dlsetting generated by Haiyang 20080717*/
-extern bool_t imu1_ir0;
-extern float fd_alpha;
-extern float fi_alpha;
-
-/*define the following varables for communication with ugear by haiyang 20080508*/
-extern int16_t ugear_phi;
-extern int16_t ugear_psi;
-extern int16_t ugear_theta;
-
-/* added 20080522 for debugging*/
-extern int16_t ugear_debug1;
-extern int16_t ugear_debug2;
-extern int16_t ugear_debug3;
-extern int32_t ugear_debug4;
-extern int32_t ugear_debug5;
-extern int32_t ugear_debug6;
-
-extern struct imu imupacket;
-extern struct gps gpspacket;
-extern uint8_t ugear_msg_buf[UGEAR_MAX_PAYLOAD];
-
-extern float ins_roll_neutral;
-extern float ins_pitch_neutral;
diff --git a/sw/airborne/obsolete/latlong.c b/sw/airborne/obsolete/latlong.c
deleted file mode 100644
index 9aff7963cc..0000000000
--- a/sw/airborne/obsolete/latlong.c
+++ /dev/null
@@ -1,150 +0,0 @@
-/** c.f. sw/lib/ocaml/latlong.ml */
-
-#include <inttypes.h>
-#include <math.h>
-#include "std.h"
-#include "latlong.h"
-#include "led.h"
-
-float latlong_utm_x, latlong_utm_y; /* m */
-float latlong_lat, latlong_lon; /* rad */
-
-/* Computation for the WGS84 geoid only */
-#define E 0.08181919106
-#define K0 0.9996
-#define XS 500000.
-#define YS 0.
-#define A 6378137.0
-#define N (K0*A)
-
-static const float serie_coeff_proj_mercator[5] = {
-  0.99832429842242842444,
-  0.00083632803657738403,
-  0.00000075957783563707,
-  0.00000000119563131778,
-  0.00000000000241079916
-};
-
-static const float serie_coeff_proj_mercator_inverse[5] = {
-  0.998324298422428424,
-  0.000837732168742475825,
-  5.90586914811817062e-08,
-  1.6734091890305064e-10,
-  2.13883575853313883e-13
-};
-
-struct complex { float re; float im; };
-#define CScal(k, z) { z.re *= k;  z.im *= k; }
-#define CAdd(z1, z2) { z2.re += z1.re;  z2.im += z1.im; }
-#define CSub(z1, z2) { z2.re -= z1.re;  z2.im -= z1.im; }
-#define CI(z) { float tmp = z.re; z.re = - z.im; z.im = tmp; }
-#define CExp(z) { float e = exp(z.re); z.re = e*cos(z.im); z.im = e*sin(z.im); }
-/* Expanded #define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; CExp(z); CExp(_z); CSub(_z, z); CScal(-0.5, z); CI(z); } */
-
-#define CSin(z) { CI(z); struct complex _z = {-z.re, -z.im}; float e = exp(z.re); float cos_z_im = cos(z.im); z.re = e*cos_z_im; float sin_z_im = sin(z.im); z.im = e*sin_z_im; _z.re = cos_z_im/e; _z.im = -sin_z_im/e; CSub(_z, z); CScal(-0.5, z); CI(z); }
-
-#define CPrint(z) printf("%.6f+%.6fi\n", z.re, z.im)
-
-
-static inline float isometric_latitude(float phi, float e) {
-  return log (tan (M_PI_4 + phi / 2.0)) - e / 2.0 * log((1.0 + e * sin(phi)) / (1.0 - e * sin(phi)));
-}
-
-static inline float isometric_latitude0(float phi) {
-  return log (tan (M_PI_4 + phi / 2.0));
-}
-
-static inline float inverse_isometric_latitude(float lat, float e, float epsilon) {
-  float exp_l = exp(lat);
-  float phi0 = 2 * atan(exp_l) - M_PI_2;
-  float phi_;
-  uint8_t bound = 3; /* To be sure to return */
-
-  do {
-    phi_ = phi0;
-    float sin_phi = e * sin(phi_);
-    phi0 = 2 * atan (pow((1 + sin_phi) / (1. - sin_phi), e/2.) * exp_l) - M_PI_2;
-    bound--;
-  } while (bound && fabs(phi_ - phi0) > epsilon);
-  return phi0;
-}
-
-/** use on ARM7 at 60MHz, i.e. percentage of cputime at 4Hz
-    specialize CSin: 951us i.e.
-    One CSin: 120us
-    init:
-    iso_lat: 155, 172
-    for loop: 258
-*/
-
-#define LambdaOfUtmZone(utm_zone) RadOfDeg((utm_zone-1)*6-180+3)
-
-void latlong_utm_of(float phi, float lambda, uint8_t utm_zone) {
-  float lambda_c = LambdaOfUtmZone(utm_zone);
-  float ll = isometric_latitude(phi , E);
-  float dl = lambda - lambda_c;
-  float phi_ = asin(sin(dl) / cosh(ll));
-  float ll_ = isometric_latitude0(phi_);
-  float lambda_ = atan(sinh(ll) / cos(dl));
-  struct complex z_ = { lambda_,  ll_ };
-  CScal(serie_coeff_proj_mercator[0], z_);
-  uint8_t k;
-  for(k = 1; k < 3; k++) {
-    struct complex z = { lambda_,  ll_ };
-    CScal(2*k, z);
-    CSin(z);
-    CScal(serie_coeff_proj_mercator[k], z);
-    CAdd(z, z_);
-  }
-  CScal(N, z_);
-  latlong_utm_x = XS + z_.im;
-  latlong_utm_y = z_.re;
-}
-
-void latlong_of_utm(float x, float y, uint8_t utm_zone) {
-  float lambda_c = LambdaOfUtmZone(utm_zone);
-  float scale = 1 / N / serie_coeff_proj_mercator[0];
-  float real = (y - YS) * scale;
-  float img = (x - XS) * scale;
-  struct complex z = { real, img };
-
-  uint8_t k;
-  for(k = 1; k < 2; k++) {
-    struct complex z_ = { real, img };
-    CScal(2*k, z_);
-    CSin(z_);
-    CScal(serie_coeff_proj_mercator_inverse[k], z_);
-    CSub(z_, z);
-  }
-  float ll = z.re;
-  float lls = z.im;
-  latlong_lon = lambda_c + atan (sinh(lls) / cos(ll));
-  float phi_ = asin (sin(ll) / cosh(lls));
-  ll = isometric_latitude0(phi_);
-  latlong_lat = inverse_isometric_latitude(ll, E, 1e-8);
-}
-
-
-#ifdef TEST_LATLONG
-#include <stdio.h>
-
-int main () {
-  struct complex z = { 1., 1.};
-  CExp(z);
-  CPrint(z);
-
-  CSin(z);
-  CPrint(z);
-
-  latlong_utm_of(0.75, 0.02, 31);
-  printf("utm_x=%.2f utm_y=%.2f {utm_x = 348805.714503056777; utm_y = 4759354.89376700297; utm_zone = 31}\n", latlong_utm_x, latlong_utm_y);
-
-  latlong_utm_of(0.5, -1.5707, 16);
-  printf("utm_x=%.2f utm_y=%.2f {utm_x = 207282.313014950836; utm_y = 3172645.2213267833; utm_zone = 16}\n", latlong_utm_x, latlong_utm_y);
-
-  latlong_of_utm(348805.71, 4759354.89, 31);
-  printf("lat=%.8f lon=%.8f posn_lat = 0.749999999392454875; posn_long = 0.019999999054505127}\n", latlong_lat, latlong_lon);
-
-  return 0;
-}
-#endif
diff --git a/sw/airborne/obsolete/latlong.h b/sw/airborne/obsolete/latlong.h
deleted file mode 100644
index 83044d33e6..0000000000
--- a/sw/airborne/obsolete/latlong.h
+++ /dev/null
@@ -1,11 +0,0 @@
-#ifndef LATLONG_H
-#define LATLONG_H
-
-extern float latlong_utm_x, latlong_utm_y; /* m */
-extern float latlong_lat, latlong_lon; /* rad */
-
-/** Convert geographic coordinates in a given UTM zone */
-void latlong_utm_of(float lat_rad, float lon_rad, uint8_t utm_zone);
-void latlong_of_utm(float x, float y, uint8_t utm_zone);
-
-#endif
diff --git a/sw/airborne/obsolete/max1167.c b/sw/airborne/obsolete/max1167.c
deleted file mode 100644
index e8b4123212..0000000000
--- a/sw/airborne/obsolete/max1167.c
+++ /dev/null
@@ -1,15 +0,0 @@
-#include "max1167.h"
-
-
-volatile uint8_t max1167_status;
-uint16_t max1167_values[MAX1167_NB_CHAN];
-
-extern void max1167_init( void ) {
-  max1167_hw_init();
-
-  uint8_t i;
-  for (i=0; i<MAX1167_NB_CHAN; i++)
-    max1167_values[i] = 0;
-
-  max1167_status = STA_MAX1167_IDLE;
-}
diff --git a/sw/airborne/obsolete/max1167.h b/sw/airborne/obsolete/max1167.h
deleted file mode 100644
index ba0a4bd13e..0000000000
--- a/sw/airborne/obsolete/max1167.h
+++ /dev/null
@@ -1,24 +0,0 @@
-#ifndef MAX1167_H
-#define MAX1167_H
-
-#include "std.h"
-
-#define MAX1167_NB_CHAN 4
-
-extern void max1167_init( void );
-extern void max1167_read( void );
-
-#define STA_MAX1167_IDLE           0
-#define STA_MAX1167_SENDING_REQ    1
-#define STA_MAX1167_READING_RES    2
-#define STA_MAX1167_DATA_AVAILABLE 3
-extern volatile uint8_t max1167_status;
-
-extern uint16_t max1167_values[MAX1167_NB_CHAN];
-
-
-extern void max1167_hw_init( void );
-
-#include "max1167_hw.h"
-
-#endif /* MAX1167_H */
diff --git a/sw/airborne/obsolete/max1167_hw.c b/sw/airborne/obsolete/max1167_hw.c
deleted file mode 100644
index 1344786c07..0000000000
--- a/sw/airborne/obsolete/max1167_hw.c
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008  Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include "max1167.h"
-
-#include "led.h"
-
-static void EXTINT0_ISR(void) __attribute__((naked));
-
-extern void max1167_hw_init( void ) {
-
-  /* SS pin is output */
-  SetBit(MAX1167_SS_IODIR, MAX1167_SS_PIN);
-  /* unselected max1167 */
-  Max1167Unselect();
-
-  /* connect P0.16 to extint0 (EOC) */
-  MAX1167_EOC_PINSEL |= MAX1167_EOC_PINSEL_VAL << MAX1167_EOC_PINSEL_BIT;
-  /* extint0 is edge trigered */
-  SetBit(EXTMODE, MAX1167_EOC_EINT);
-  /* extint0 is trigered on falling edge */
-  ClearBit(EXTPOLAR, MAX1167_EOC_EINT);
-  /* clear pending extint0 before enabling interrupts */
-  SetBit(EXTINT, MAX1167_EOC_EINT);
-
-   /* initialize interrupt vector */
-  VICIntSelect &= ~VIC_BIT( VIC_EINT0 );                     // EXTINT0 selected as IRQ
-  VICIntEnable = VIC_BIT( VIC_EINT0 );                       // EXTINT0 interrupt enabled
-  _VIC_CNTL(MAX1167_EOC_VIC_SLOT) = VIC_ENABLE | VIC_EINT0;
-  _VIC_ADDR(MAX1167_EOC_VIC_SLOT) = (uint32_t)EXTINT0_ISR;   // address of the ISR
-}
-
-
-void max1167_read( void ) {
-  //ASSERT((max1167_status == STA_MAX1167_IDLE),		\
-	// DEBUG_MAX_1117, MAX1167_ERR_READ_OVERUN);
-  /* select max1167 */
-  Max1167Select();
-  /* enable SPI */
-  SpiClearRti();
-  SpiDisableRti();
-  SpiEnable();
-  /* write control byte - wait EOC on extint */
-  const uint8_t control_byte = 1 << 0 | 1 << 3 | 2 << 5;
-  SpiSend(control_byte);
-  max1167_status = STA_MAX1167_SENDING_REQ;
-}
-
-void EXTINT0_ISR(void) {
-  ISR_ENTRY();
-
-  //ASSERT((max1167_status == STA_MAX1167_SENDING_REQ),	\
-//	 DEBUG_MAX_1117, MAX1167_ERR_SPURIOUS_EOC);
-  /* trigger 6 bytes read */
-  SpiSend(0);
-  SpiSend(0);
-  SpiSend(0);
-  SpiSend(0);
-  SpiSend(0);
-  SpiSend(0);
-  SpiClearRti();
-  SpiEnableRti();
-  max1167_status = STA_MAX1167_READING_RES;
-
-  SetBit(EXTINT, MAX1167_EOC_EINT);   /* clear extint0 */
-  VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
-
-  ISR_EXIT();
-}
diff --git a/sw/airborne/obsolete/max1167_hw.h b/sw/airborne/obsolete/max1167_hw.h
deleted file mode 100644
index 0aaba0901f..0000000000
--- a/sw/airborne/obsolete/max1167_hw.h
+++ /dev/null
@@ -1,55 +0,0 @@
-#ifndef MAX1167_HW_H
-#define MAX1167_HW_H
-
-/*
-  MAX1167 SPI ADC connected on SPI1
-  SS on P1.29
-  EOC on P0.16 ( EINT0 )
-*/
-
-#include "std.h"
-#include "LPC21xx.h"
-#include "interrupt_hw.h"
-#include "spi_hw.h"
-
-//#include "pprz_debug.h"
-
-#define MAX1167_ERR_ISR_STATUS   0
-#define MAX1167_ERR_READ_OVERUN  1
-#define MAX1167_ERR_SPURIOUS_EOC 2
-
-#define MAX1167_SS_PIN 29
-#define MAX1167_SS_IODIR IO1DIR
-#define MAX1167_SS_IOSET IO1SET
-#define MAX1167_SS_IOCLR IO1CLR
-
-#define MAX1167_EOC_PIN 16
-#define MAX1167_EOC_PINSEL PINSEL1
-#define MAX1167_EOC_PINSEL_BIT 0
-#define MAX1167_EOC_PINSEL_VAL 1
-#define MAX1167_EOC_EINT 0
-
-#define Max1167Unselect() SetBit(MAX1167_SS_IOSET, MAX1167_SS_PIN)
-#define Max1167Select() SetBit(MAX1167_SS_IOCLR, MAX1167_SS_PIN)
-
-#define Max1167OnSpiInt() {						\
-   /* ASSERT((max1167_status == STA_MAX1167_READING_RES),			\
-	   DEBUG_MAX_1117, MAX1167_ERR_ISR_STATUS);*/			\
-    /* read dummy control byte reply */					\
-    uint8_t foo __attribute__ ((unused));				\
-    foo = SSPDR;							\
-    /* store convertion result */					\
-    max1167_values[0] = SSPDR << 8;					\
-    max1167_values[0] += SSPDR;						\
-    max1167_values[1] = SSPDR << 8;					\
-    max1167_values[1] += SSPDR;						\
-    max1167_values[2] = SSPDR << 8;					\
-    max1167_values[2] += SSPDR;						\
-    SpiClearRti();							\
-    SpiDisableRti();							\
-    SpiDisable();							\
-    Max1167Unselect();							\
-    max1167_status = STA_MAX1167_DATA_AVAILABLE;			\
-  }
-
-#endif /* MAX1167_WH */
diff --git a/sw/airborne/peripherals/mcp355x.c b/sw/airborne/peripherals/mcp355x.c
index 386f1aae59..7cf0892807 100644
--- a/sw/airborne/peripherals/mcp355x.c
+++ b/sw/airborne/peripherals/mcp355x.c
@@ -51,7 +51,7 @@ void mcp355x_read(void) {
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 void mcp355x_event(void) {
   static uint32_t filtered = 0;
@@ -65,8 +65,8 @@ void mcp355x_event(void) {
           ((uint32_t)mcp355x_spi_buf[2]<<1) |
           (mcp355x_spi_buf[3]>>7));
       filtered = (5*filtered + mcp355x_data) / (6);
-      DOWNLINK_SEND_DEBUG(DefaultChannel,4,mcp355x_spi_buf);
-      DOWNLINK_SEND_BARO_RAW(DefaultChannel,&mcp355x_data,&filtered);
+      DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,4,mcp355x_spi_buf);
+      DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice,&mcp355x_data,&filtered);
     }
   }
 }
diff --git a/sw/airborne/pprz_debug.h b/sw/airborne/pprz_debug.h
index 79d764b26f..f42de1a262 100644
--- a/sw/airborne/pprz_debug.h
+++ b/sw/airborne/pprz_debug.h
@@ -33,7 +33,7 @@
 #include "std.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 extern uint8_t pprz_debug_mod;
 extern uint8_t pprz_debug_err;
@@ -48,7 +48,7 @@ extern uint8_t pprz_debug_err;
     if (!(cond)) {                                                  \
       pprz_debug_mod = mod;                                         \
       pprz_debug_err = err;                                         \
-      DOWNLINK_SEND_PPRZ_DEBUG(&pprz_debug_mod, &pprz_debug_err);	\
+      DOWNLINK_SEND_PPRZ_DEBUG(DefaultChannel, DefaultDevice, &pprz_debug_mod, &pprz_debug_err);	\
     }                                                               \
   }
 #else
diff --git a/sw/airborne/pprz_transport.h b/sw/airborne/pprz_transport.h
deleted file mode 100644
index 8df145dd90..0000000000
--- a/sw/airborne/pprz_transport.h
+++ /dev/null
@@ -1,217 +0,0 @@
-/*
- * Paparazzi $Id$
- *
- * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file pprz_transport.h
- *  \brief Building and parsing Paparazzi frames
- *
- *  Pprz frame:
- *
- *   |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B|
- *
- *   where checksum is computed over length and payload:
- *     ck_A = ck_B = length
- *     for each byte b in payload
- *       ck_A += b; ck_b += ck_A
- */
-
-#ifndef PPRZ_TRANSPORT_H
-#define PPRZ_TRANSPORT_H
-
-#include <inttypes.h>
-#include "std.h"
-#include "datalink.h"
-
-extern uint8_t ck_a, ck_b;
-
-#define STX  0x99
-
-/** 4 = STX + len + ck_a + ck_b */
-#define PprzTransportSizeOf(_payload) (_payload+4)
-
-#define __Link(dev, _x) dev##_x
-#define _Link(dev, _x)  __Link(dev, _x)
-#define Link(_x) _Link(DOWNLINK_DEVICE, _x)
-
-#define PprzTransportCheckFreeSpace(_x) Link(CheckFreeSpace(_x))
-
-#define PprzTransportPut1Byte(_x) Link(Transmit(_x))
-#define PprzTransportSendMessage() Link(SendMessage())
-
-#define PprzTransportHeader(payload_len) { \
-  PprzTransportPut1Byte(STX);				\
-  uint8_t msg_len = PprzTransportSizeOf(payload_len);	\
-  PprzTransportPut1Byte(msg_len);			\
-  ck_a = msg_len; ck_b = msg_len;			\
-}
-
-#define PprzTransportTrailer() { \
-  PprzTransportPut1Byte(ck_a);	\
-  PprzTransportPut1Byte(ck_b);	\
-  PprzTransportSendMessage() \
-}
-
-#define PprzTransportPutUint8(_byte) { \
-    ck_a += _byte;			  \
-    ck_b += ck_a;			  \
-    PprzTransportPut1Byte(_byte);		  \
- }
-
-#define PprzTransportPutNamedUint8(_name, _byte) PprzTransportPutUint8(_byte)
-
-#define PprzTransportPut1ByteByAddr(_byte) {	 \
-    uint8_t _x = *(_byte);		 \
-    PprzTransportPutUint8(_x);	 \
-  }
-
-#define PprzTransportPut2ByteByAddr(_byte) { \
-    PprzTransportPut1ByteByAddr(_byte);	\
-    PprzTransportPut1ByteByAddr((const uint8_t*)_byte+1);	\
-  }
-
-#define PprzTransportPut4ByteByAddr(_byte) { \
-    PprzTransportPut2ByteByAddr(_byte);	\
-    PprzTransportPut2ByteByAddr((const uint8_t*)_byte+2);	\
-  }
-
-#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */
-#define PprzTransportPutDoubleByAddr(_byte) { \
-    PprzTransportPut4ByteByAddr((const uint8_t*)_byte+4);	\
-    PprzTransportPut4ByteByAddr((const uint8_t*)_byte);	\
-  }
-#else
-#define PprzTransportPutDoubleByAddr(_byte) { \
-    PprzTransportPut4ByteByAddr((const uint8_t*)_byte);	\
-    PprzTransportPut4ByteByAddr((const uint8_t*)_byte+4);	\
-  }
-#endif
-
-
-#define PprzTransportPutInt8ByAddr(_x) PprzTransportPut1ByteByAddr(_x)
-#define PprzTransportPutUint8ByAddr(_x) PprzTransportPut1ByteByAddr((const uint8_t*)_x)
-#define PprzTransportPutInt16ByAddr(_x) PprzTransportPut2ByteByAddr((const uint8_t*)_x)
-#define PprzTransportPutUint16ByAddr(_x) PprzTransportPut2ByteByAddr((const uint8_t*)_x)
-#define PprzTransportPutInt32ByAddr(_x) PprzTransportPut4ByteByAddr((const uint8_t*)_x)
-#define PprzTransportPutUint32ByAddr(_x) PprzTransportPut4ByteByAddr((const uint8_t*)_x)
-#define PprzTransportPutFloatByAddr(_x) PprzTransportPut4ByteByAddr((const uint8_t*)_x)
-
-#define PprzTransportPutArray(_put, _n, _x) { \
-  uint8_t _i; \
-  PprzTransportPutUint8(_n); \
-  for(_i = 0; _i < _n; _i++) { \
-    _put(&_x[_i]); \
-  } \
-}
-
-#define PprzTransportPutFloatArray(_n, _x) PprzTransportPutArray(PprzTransportPutFloatByAddr, _n, _x)
-#define PprzTransportPutDoubleArray(_n, _x) PprzTransportPutArray(PprzTransportPutDoubleByAddr, _n, _x)
-
-#define PprzTransportPutInt16Array(_n, _x) PprzTransportPutArray(PprzTransportPutInt16ByAddr, _n, _x)
-#define PprzTransportPutUint16Array(_n, _x) PprzTransportPutArray(PprzTransportPutUint16ByAddr, _n, _x)
-
-#define PprzTransportPutInt32Array(_n, _x) PprzTransportPutArray(PprzTransportPutInt32ByAddr, _n, _x)
-#define PprzTransportPutUint32Array(_n, _x) PprzTransportPutArray(PprzTransportPutUint32ByAddr, _n, _x)
-
-#define PprzTransportPutUint8Array(_n, _x) PprzTransportPutArray(PprzTransportPutUint8ByAddr, _n, _x)
-
-
-/** Receiving pprz messages */
-
-#define UNINIT 0
-#define GOT_STX 1
-#define GOT_LENGTH 2
-#define GOT_PAYLOAD 3
-#define GOT_CRC1 4
-
-
-#define PPRZ_PAYLOAD_LEN 256
-extern uint8_t pprz_payload[PPRZ_PAYLOAD_LEN];
-
-extern volatile bool_t pprz_msg_received;
-extern uint8_t pprz_ovrn, pprz_error;
-extern volatile uint8_t pprz_payload_len;
-
-static inline void parse_pprz( uint8_t c ) {
-  static uint8_t pprz_status = UNINIT;
-  static uint8_t _ck_a, _ck_b, payload_idx;
-
-  switch (pprz_status) {
-  case UNINIT:
-    if (c == STX)
-      pprz_status++;
-    break;
-  case GOT_STX:
-    if (pprz_msg_received) {
-      pprz_ovrn++;
-      goto error;
-    }
-    pprz_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
-    _ck_a = _ck_b = c;
-    pprz_status++;
-    payload_idx = 0;
-    break;
-  case GOT_LENGTH:
-    pprz_payload[payload_idx] = c;
-    _ck_a += c; _ck_b += _ck_a;
-    payload_idx++;
-    if (payload_idx == pprz_payload_len)
-      pprz_status++;
-    break;
-  case GOT_PAYLOAD:
-    if (c != _ck_a)
-      goto error;
-    pprz_status++;
-    break;
-  case GOT_CRC1:
-    if (c != _ck_b)
-      goto error;
-    pprz_msg_received = TRUE;
-    goto restart;
-  default:
-    goto error;
-  }
-  return;
- error:
-  pprz_error++;
- restart:
-  pprz_status = UNINIT;
-  return;
-}
-
-static inline void pprz_parse_payload(void) {
-  uint8_t i;
-  for(i = 0; i < pprz_payload_len; i++)
-    dl_buffer[i] = pprz_payload[i];
-  dl_msg_available = TRUE;
-}
-
-#define __PprzLink(dev, _x) dev##_x
-#define _PprzLink(dev, _x)  __PprzLink(dev, _x)
-#define PprzLink(_x) _PprzLink(PPRZ_UART, _x)
-
-#define PprzBuffer() PprzLink(ChAvailable())
-#define ReadPprzBuffer() { while (PprzLink(ChAvailable())&&!pprz_msg_received) parse_pprz(PprzLink(Getch())); }
-
-
-#endif /* PPRZ_TRANSPORT_H */
-
diff --git a/sw/airborne/sd_card/main.c b/sw/airborne/sd_card/main.c
index 5ab9371101..c20aee6910 100644
--- a/sw/airborne/sd_card/main.c
+++ b/sw/airborne/sd_card/main.c
@@ -5,9 +5,9 @@
 #include "mcu_periph/uart.h"
 
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
-#include "datalink.h"
+#include "subsystems/datalink/datalink.h"
 #include "generated/settings.h"
 #include "dl_protocol.h"
 
diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h
index 5a30258d63..6dceb2ffe4 100644
--- a/sw/airborne/subsystems/ahrs.h
+++ b/sw/airborne/subsystems/ahrs.h
@@ -19,6 +19,10 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/** \file ahrs.h
+ * \brief Attitude and Heading Reference System interface
+ */
+
 #ifndef AHRS_H
 #define AHRS_H
 
@@ -34,41 +38,44 @@
 #include AHRS_TYPE_H
 #endif
 
-
+/** Attitude and Heading Reference System state (fixed point version) */
 struct Ahrs {
 
-    struct Int32Quat   ltp_to_imu_quat;
-    struct Int32Eulers ltp_to_imu_euler;
-    struct Int32RMat   ltp_to_imu_rmat;
-    struct Int32Rates  imu_rate;
+  struct Int32Quat   ltp_to_imu_quat;  ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion
+  struct Int32Eulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles
+  struct Int32RMat   ltp_to_imu_rmat;  ///< Rotation from LocalTangentPlane to IMU frame as Rotation Matrix
+  struct Int32Rates  imu_rate;         ///< Rotational velocity in IMU frame
 
-    struct Int32Quat   ltp_to_body_quat;
-    struct Int32Eulers ltp_to_body_euler;
-    struct Int32RMat   ltp_to_body_rmat;
-    struct Int32Rates  body_rate;
+  struct Int32Quat   ltp_to_body_quat;  ///< Rotation from LocalTangentPlane to body frame as unit quaternion
+  struct Int32Eulers ltp_to_body_euler; ///< Rotation from LocalTangentPlane to body frame as Euler angles
+  struct Int32RMat   ltp_to_body_rmat;  ///< Rotation from LocalTangentPlane to body frame as Rotation Matrix
+  struct Int32Rates  body_rate;         ///< Rotational velocity in body frame
 
-    uint8_t status;
+  uint8_t status; ///< status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
 };
 
+/** Attitude and Heading Reference System state (floating point version) */
 struct AhrsFloat {
-  struct FloatQuat   ltp_to_imu_quat;
-  struct FloatEulers ltp_to_imu_euler;
-  struct FloatRMat   ltp_to_imu_rmat;
-  struct FloatRates  imu_rate;
+  struct FloatQuat   ltp_to_imu_quat;  ///< Rotation from LocalTangentPlane to IMU frame as unit quaternion
+  struct FloatEulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles
+  struct FloatRMat   ltp_to_imu_rmat;  ///< Rotation from LocalTangentPlane to IMU frame as Rotation Matrix
+  struct FloatRates  imu_rate;         ///< Rotational velocity in IMU frame
   struct FloatRates  imu_rate_previous;
   struct FloatRates  imu_rate_d;
 
-  struct FloatQuat   ltp_to_body_quat;
-  struct FloatEulers ltp_to_body_euler;
-  struct FloatRMat   ltp_to_body_rmat;
-  struct FloatRates  body_rate;
+  struct FloatQuat   ltp_to_body_quat;  ///< Rotation from LocalTangentPlane to body frame as unit quaternion
+  struct FloatEulers ltp_to_body_euler; ///< Rotation from LocalTangentPlane to body frame as Euler angles
+  struct FloatRMat   ltp_to_body_rmat;  ///< Rotation from LocalTangentPlane to body frame as Rotation Matrix
+  struct FloatRates  body_rate;         ///< Rotational velocity in body frame
   struct FloatRates  body_rate_d;
 
   // always use status from fixed point ahrs struct for now
   //uint8_t status;
 };
 
+/** global AHRS state (fixed point version) */
 extern struct Ahrs ahrs;
+/** global AHRS state (floating point version) */
 extern struct AhrsFloat ahrs_float;
 
 extern float ahrs_mag_offset;
@@ -86,10 +93,34 @@ extern float ahrs_mag_offset;
     RATES_BFP_OF_REAL(ahrs.body_rate, ahrs_float.body_rate);                   \
   }
 
+/** AHRS initialization. Called at startup.
+ *  Needs to be implemented by each AHRS algorithm.
+ */
 extern void ahrs_init(void);
+
+/** Aligns the AHRS. Called after ahrs_aligner has run to set initial attitude and biases.
+ *  Must set the ahrs status to AHRS_RUNNING.
+ *  Needs to be implemented by each AHRS algorithm.
+ */
 extern void ahrs_align(void);
+
+/** Propagation. Usually integrates the gyro rates to angles.
+ *  Reads the global #imu data struct.
+ *  Needs to be implemented by each AHRS algorithm.
+ */
 extern void ahrs_propagate(void);
+
+/** Update AHRS state with accerleration measurements.
+ *  Reads the global #imu data struct.
+ *  Needs to be implemented by each AHRS algorithm.
+ */
 extern void ahrs_update_accel(void);
+
+/** Update AHRS state with magnetometer measurements.
+ *  Reads the global #imu data struct.
+ *  Needs to be implemented by each AHRS algorithm.
+ */
 extern void ahrs_update_mag(void);
+extern void ahrs_update_gps(void);
 
 #endif /* AHRS_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c
index 1209262542..63199645d2 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c
@@ -28,9 +28,20 @@
 #include "math/pprz_algebra_int.h"
 #include "math/pprz_simple_matrix.h"
 #include "generated/airframe.h"
+#if USE_GPS
+#include "subsystems/gps.h"
+#endif
 
 
-#include "../../test/pprz_algebra_print.h"
+//#include "../../test/pprz_algebra_print.h"
+
+#if !defined AHRS_PROPAGATE_RMAT && !defined AHRS_PROPAGATE_QUAT
+#error "You have to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT"
+#endif
+
+#ifdef AHRS_MAG_UPDATE_YAW_ONLY
+#warning "AHRS_MAG_UPDATE_YAW_ONLY is deprecated, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw."
+#endif
 
 void ahrs_update_mag_full(void);
 void ahrs_update_mag_2d(void);
@@ -45,6 +56,7 @@ struct AhrsFloatCmplRmat ahrs_impl;
 
 void ahrs_init(void) {
   ahrs.status = AHRS_UNINIT;
+  ahrs_impl.ltp_vel_norm_valid = FALSE;
 
   /* Initialises IMU alignement */
   struct FloatEulers body_to_imu_euler =
@@ -65,6 +77,12 @@ void ahrs_init(void) {
 
   FLOAT_RATES_ZERO(ahrs_float.imu_rate);
 
+#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
+  ahrs_impl.correct_gravity = TRUE;
+#else
+  ahrs_impl.correct_gravity = FALSE;
+#endif
+
 }
 
 
@@ -126,49 +144,67 @@ void ahrs_propagate(void) {
 
 void ahrs_update_accel(void) {
 
-  struct FloatVect3 accel_float;
-  ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
-
-#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
-  float v = FLOAT_VECT3_NORM(ahrs_impl.est_ltp_speed);
-  accel_float.y -=  v * ahrs_float.imu_rate.r;
-  accel_float.z -= -v * ahrs_float.imu_rate.q;
-#endif
-
+  /* last column of roation matrix = ltp z-axis in imu-frame */
   struct FloatVect3  c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2),
-			    RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2),
-			    RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)};
+                            RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2),
+                            RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)};
+
+  struct FloatVect3 imu_accel_float;
+  ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel);
+
   struct FloatVect3 residual;
-  FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, c2);
+
+  if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
+    /*
+     * centrifugal acceleration in body frame
+     * a_c_body = omega x (omega x r)
+     * (omega x r) = tangential velocity in body frame
+     * a_c_body = omega x vel_tangential_body
+     * assumption: tangential velocity only along body x-axis
+     */
+    const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0};
+    struct FloatVect3 acc_c_body;
+    VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs_float.body_rate, vel_tangential_body);
+
+    /* convert centrifucal acceleration from body to imu frame */
+    struct FloatVect3 acc_c_imu;
+    FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body);
+
+    /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
+    struct FloatVect3 corrected_gravity;
+    VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu);
+
+    /* compute the residual of gravity vector in imu frame */
+    FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
+  } else {
+    FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2);
+  }
+
 #ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
   /* heuristic on acceleration norm */
-  const float acc_norm = FLOAT_VECT3_NORM(accel_float);
+  const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float);
   const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
 #else
   const float weight = 1.;
 #endif
+
   /* compute correction */
   const float gravity_rate_update_gain = -5e-2; // -5e-2
   FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
-#if 1
+
   const float gravity_bias_update_gain = 1e-5; // -5e-6
   FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
-#else
-  const float alpha = 5e-4;
-  FLOAT_RATES_SCALE(ahrs_impl.gyro_bias, 1.-alpha);
-  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, alpha);
-#endif
+
   /* FIXME: saturate bias */
 
 }
 
 
 void ahrs_update_mag(void) {
-#ifdef AHRS_MAG_UPDATE_YAW_ONLY
-  ahrs_update_mag_2d();
-  //  ahrs_update_mag_2d_dumb();
-#else
+#if AHRS_MAG_UPDATE_ALL_AXES
   ahrs_update_mag_full();
+#else
+  ahrs_update_mag_2d();
 #endif
 }
 
@@ -223,7 +259,7 @@ void ahrs_update_mag_2d(void) {
 }
 
 
-void ahrs_update_mag_full_2d_dumb(void) {
+void ahrs_update_mag_2d_dumb(void) {
 
   /* project mag on local tangeant plane */
   struct FloatVect3 magf;
@@ -236,19 +272,26 @@ void ahrs_update_mag_full_2d_dumb(void) {
   const float me =     0. * magf.x + cphi       *magf.y - sphi       *magf.z;
 
   const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn;
-  struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
+  const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0),
+                                RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,1),
+                                RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)};
   const float mag_rate_update_gain = 2.5;
-  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm));
+  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm));
   const float mag_bias_update_gain = -2.5e-4;
-  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm));
+  FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, r2, (mag_bias_update_gain*res_norm));
 
 }
 
-
-
-
-
-
+void ahrs_update_gps(void) {
+#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
+  if (gps.fix == GPS_FIX_3D) {
+    ahrs_impl.ltp_vel_norm = gps.speed_3d / 100.;
+    ahrs_impl.ltp_vel_norm_valid = TRUE;
+  } else {
+    ahrs_impl.ltp_vel_norm_valid = FALSE;
+  }
+#endif
+}
 
 /*
  * Compute ltp to imu rotation in euler angles and quaternion representations
@@ -271,12 +314,36 @@ static inline void compute_imu_rmat_and_euler_from_quat(void) {
  * Compute body orientation and rates from imu orientation and rates
  */
 static inline void compute_body_orientation_and_rates(void) {
-
   FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat,
-		      ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
+                      ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
   FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat,
-		      ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
+                      ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
   FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat);
   FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate);
-
 }
+
+
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// TODO use ahrs result directly
+#include "estimator.h"
+// remotely settable
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+void ahrs_update_fw_estimator(void)
+{
+  // export results to estimator
+  estimator_phi   = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral;
+  estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral;
+  estimator_psi   = ahrs_float.ltp_to_body_euler.psi;
+
+  estimator_p = ahrs_float.body_rate.p;
+  estimator_q = ahrs_float.body_rate.q;
+  estimator_r = ahrs_float.body_rate.r;
+}
+#endif //AHRS_UPDATE_FW_ESTIMATOR
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h
index ecfa367ccf..56aabf2a38 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h
@@ -24,11 +24,15 @@
 #ifndef AHRS_FLOAT_CMPL_RMAT
 #define AHRS_FLOAT_CMPL_RMAT
 
+#include "std.h"
+
 struct AhrsFloatCmplRmat {
   struct FloatRates gyro_bias;
   struct FloatRates rate_correction;
   /* for gravity correction during coordinated turns */
-  struct FloatVect3 est_ltp_speed;
+  float ltp_vel_norm;
+  bool_t ltp_vel_norm_valid;
+  bool_t correct_gravity;
 
   /*
      Holds float version of IMU alignement
@@ -42,4 +46,12 @@ struct AhrsFloatCmplRmat {
 extern struct AhrsFloatCmplRmat ahrs_impl;
 
 
+#ifdef AHRS_UPDATE_FW_ESTIMATOR
+// TODO copy ahrs to state instead of estimator
+void ahrs_update_fw_estimator(void);
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+#endif
+
+
 #endif /* AHRS_FLOAT_CMPL_RMAT */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 86d5ee77b5..9470897f63 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -29,7 +29,7 @@
 #include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
 #include "math/pprz_algebra_float.h"
 
-#ifdef USE_GPS
+#if USE_GPS
 #include "subsystems/gps.h"
 #endif
 
@@ -45,7 +45,7 @@
 #endif
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 
 struct AhrsFloatDCM ahrs_impl;
@@ -73,7 +73,7 @@ float DCM_Matrix[3][3]       = {{1,0,0},{0,1,0},{0,0,1}};
 float Update_Matrix[3][3]    = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
 float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
 
-#ifdef USE_MAGNETOMETER
+#if USE_MAGNETOMETER
 float MAG_Heading_X = 1;
 float MAG_Heading_Y = 0;
 #endif
@@ -91,7 +91,7 @@ int renorm_blowup_count = 0;
 float imu_health = 0.;
 #endif
 
-#ifdef USE_HIGH_ACCEL_FLAG
+#if USE_HIGH_ACCEL_FLAG
 // High Accel Flag
 #define HIGH_ACCEL_LOW_SPEED 15.0
 #define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
@@ -144,7 +144,7 @@ void ahrs_update_fw_estimator( void )
   estimator_q = ahrs_float.body_rate.q;
   estimator_r = ahrs_float.body_rate.r;
 /*
-  RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel,
+  RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
     &(DCM_Matrix[0][0]),
     &(DCM_Matrix[0][1]),
     &(DCM_Matrix[0][2]),
@@ -189,7 +189,7 @@ void ahrs_init(void) {
   /* set inital filter dcm */
   set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
 
-#ifdef USE_HIGH_ACCEL_FLAG
+#if USE_HIGH_ACCEL_FLAG
   high_accel_done = FALSE;
   high_accel_flag = FALSE;
 #endif
@@ -261,7 +261,7 @@ void ahrs_update_accel(void)
   accel_float.z = -accel_float.z;
 
 
-#ifdef USE_GPS
+#if USE_GPS
   if (gps.fix == GPS_FIX_3D) {    //Remove centrifugal acceleration.
     accel_float.y += gps.speed_3d/100. * Omega[2];  // Centrifugal force on Acc_y = GPS_speed*GyroZ
     accel_float.z -= gps.speed_3d/100. * Omega[1];  // Centrifugal force on Acc_z = GPS_speed*GyroY
@@ -274,7 +274,7 @@ void ahrs_update_accel(void)
 
 void ahrs_update_mag(void)
 {
-#ifdef USE_MAGNETOMETER
+#if USE_MAGNETOMETER
 #warning MAGNETOMETER FEEDBACK NOT TESTED YET
 
   float cos_roll;
@@ -318,13 +318,17 @@ void ahrs_update_mag(void)
   ltp_mag.y = MAG_Heading_Y;
 
   // Downlink
-  RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
+  RunOnceEvery(10,DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice, &ltp_mag.x, &ltp_mag.y, &ltp_mag.z));
 
   // Magnetic Heading
   // MAG_Heading = atan2(imu.mag.y, -imu.mag.x);
 #endif
 }
 
+void ahrs_update_gps(void) {
+
+}
+
 void Normalize(void)
 {
   float error=0;
@@ -429,7 +433,7 @@ void Drift_correction(void)
   // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
   Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1);  //
 
-#ifdef USE_HIGH_ACCEL_FLAG
+#if USE_HIGH_ACCEL_FLAG
   // Test for high acceleration:
   //  - low speed
   //  - high thrust
@@ -465,7 +469,7 @@ void Drift_correction(void)
 
   //*****YAW***************
 
-#ifdef USE_MAGNETOMETER
+#if USE_MAGNETOMETER
   // We make the gyro YAW drift correction based on compass magnetic heading
 //  float mag_heading_x = cos(MAG_Heading);
 //  float mag_heading_y = sin(MAG_Heading);
@@ -479,7 +483,7 @@ void Drift_correction(void)
   Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
 
-#elif defined USE_GPS // Use GPS Ground course to correct yaw gyro drift
+#elif USE_GPS // Use GPS Ground course to correct yaw gyro drift
 
   if(gps.fix == GPS_FIX_3D && gps.gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s
     float ground_course = ((float)gps.course)/1.e7 - M_PI; //This is the runaway direction of you "plane" in rad
diff --git a/sw/airborne/subsystems/ahrs/ahrs_infrared.h b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
index 872a56dfd3..43a2b3b340 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_infrared.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_infrared.h
@@ -31,7 +31,6 @@
 extern float ins_roll_neutral;
 extern float ins_pitch_neutral;
 
-extern void ahrs_update_gps(void);
 extern void ahrs_update_infrared(void);
 
 // TODO copy ahrs to state instead of estimator
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
index 4060f3ca8d..f25d78149b 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
@@ -23,7 +23,6 @@
 // TODO
 //
 // gravity heuristic
-// gps based gravity correction
 // gps update for yaw on fixed wing ?
 //
 
@@ -32,6 +31,9 @@
 #include "subsystems/ahrs/ahrs_int_utils.h"
 
 #include "subsystems/imu.h"
+#if USE_GPS
+#include "subsystems/gps.h"
+#endif
 #include "math/pprz_trig_int.h"
 #include "math/pprz_algebra_int.h"
 
@@ -42,31 +44,34 @@
 static inline void ahrs_update_mag_full(void);
 static inline void ahrs_update_mag_2d(void);
 
+#ifdef AHRS_MAG_UPDATE_YAW_ONLY
+#warning "AHRS_MAG_UPDATE_YAW_ONLY is deprecated, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw."
+#endif
 
 /* in place quaternion first order integration with constante rotational velocity */
 /*  */
-#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) {			\
-    _hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz;	\
-    _hr.qx +=  _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz;	\
-    _hr.qy +=  _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz;	\
-    _hr.qz +=  _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy;	\
-									\
-    ldiv_t _div = ldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2));		\
-    _q.qi+= _div.quot;							\
-    _hr.qi = _div.rem;							\
-									\
-    _div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2));			\
-    _q.qx+= _div.quot;							\
-    _hr.qx = _div.rem;							\
-									\
-    _div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2));			\
-    _q.qy+= _div.quot;							\
-    _hr.qy = _div.rem;							\
-									\
-    _div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2));			\
-    _q.qz+= _div.quot;							\
-    _hr.qz = _div.rem;							\
-									\
+#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) {              \
+    _hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz;    \
+    _hr.qx +=  _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz;    \
+    _hr.qy +=  _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz;    \
+    _hr.qz +=  _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy;    \
+                                                                    \
+    ldiv_t _div = ldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2));        \
+    _q.qi+= _div.quot;                                              \
+    _hr.qi = _div.rem;                                              \
+                                                                    \
+    _div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2));               \
+    _q.qx+= _div.quot;                                              \
+    _hr.qx = _div.rem;                                              \
+                                                                    \
+    _div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2));               \
+    _q.qy+= _div.quot;                                              \
+    _hr.qy = _div.rem;                                              \
+                                                                    \
+    _div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2));               \
+    _q.qz+= _div.quot;                                              \
+    _hr.qz = _div.rem;                                              \
+                                                                    \
   }
 
 
@@ -80,6 +85,7 @@ static inline void compute_body_orientation(void);
 void ahrs_init(void) {
 
   ahrs.status = AHRS_UNINIT;
+  ahrs_impl.ltp_vel_norm_valid = FALSE;
 
   /* set ltp_to_body to zero */
   INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
@@ -97,6 +103,12 @@ void ahrs_init(void) {
   INT_RATES_ZERO(ahrs_impl.rate_correction);
   INT_RATES_ZERO(ahrs_impl.high_rez_bias);
 
+#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
+  ahrs_impl.correct_gravity = TRUE;
+#else
+  ahrs_impl.correct_gravity = FALSE;
+#endif
+
 }
 
 void ahrs_align(void) {
@@ -159,25 +171,40 @@ void ahrs_propagate(void) {
 
 void ahrs_update_accel(void) {
 
+  // c2 = ltp z-axis in imu-frame
   struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
-			   RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
-			   RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
+                           RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
+                           RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
   struct Int32Vect3 residual;
-#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
-  // FIXME: check overflow ?
-  const struct Int32Vect3 Xdd_imu = {
-    0,
-     ((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.r)
-    >>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC),
-    -((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.q)
-    >>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC)
-  };
-  struct Int32Vect3 corrected_gravity;
-  VECT3_DIFF(corrected_gravity, imu.accel, Xdd_imu);
-  INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
-#else
-  INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2);
-#endif
+
+  if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) {
+    /*
+     * centrifugal acceleration in body frame
+     * a_c_body = omega x (omega x r)
+     * (omega x r) = tangential velocity in body frame
+     * a_c_body = omega x vel_tangential_body
+     * assumption: tangential velocity only along body x-axis
+     */
+
+    // FIXME: check overflows !
+    const struct Int32Vect3 vel_tangential_body = {(ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC), 0.0, 0.0};
+    struct Int32Vect3 acc_c_body;
+    VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body);
+    INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC);
+
+    /* convert centrifucal acceleration from body to imu frame */
+    struct Int32Vect3 acc_c_imu;
+    INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body);
+
+    /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */
+    struct Int32Vect3 corrected_gravity;
+    INT32_VECT3_DIFF(corrected_gravity, imu.accel, acc_c_imu);
+
+    /* compute the residual of gravity vector in imu frame */
+    INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
+  } else {
+    INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2);
+  }
 
   // residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
   // rate_correction FRAC = RATE_FRAC = 12
@@ -206,18 +233,18 @@ void ahrs_update_accel(void) {
 }
 
 void ahrs_update_mag(void) {
-#ifdef AHRS_MAG_UPDATE_YAW_ONLY
-  ahrs_update_mag_2d();
-#else
+#if AHRS_MAG_UPDATE_ALL_AXES
   ahrs_update_mag_full();
+#else
+  ahrs_update_mag_2d();
 #endif
 }
 
 
 static inline void ahrs_update_mag_full(void) {
   const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
-					  MAG_BFP_OF_REAL(AHRS_H_Y),
-					  MAG_BFP_OF_REAL(AHRS_H_Z)};
+                                          MAG_BFP_OF_REAL(AHRS_H_Y),
+                                          MAG_BFP_OF_REAL(AHRS_H_Z)};
   struct Int32Vect3 expected_imu;
   INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
 
@@ -229,9 +256,9 @@ static inline void ahrs_update_mag_full(void) {
   ahrs_impl.rate_correction.r += residual.z/32/16;
 
 
-  ahrs_impl.high_rez_bias.p += -residual.x/32*1024;
-  ahrs_impl.high_rez_bias.q += -residual.y/32*1024;
-  ahrs_impl.high_rez_bias.r += -residual.z/32*1024;
+  ahrs_impl.high_rez_bias.p -= residual.x/32*1024;
+  ahrs_impl.high_rez_bias.q -= residual.y/32*1024;
+  ahrs_impl.high_rez_bias.r -= residual.z/32*1024;
 
 
   INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
@@ -242,7 +269,7 @@ static inline void ahrs_update_mag_full(void) {
 static inline void ahrs_update_mag_2d(void) {
 
   const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
-					  MAG_BFP_OF_REAL(AHRS_H_Y)};
+                                          MAG_BFP_OF_REAL(AHRS_H_Y)};
 
   struct Int32Vect3 measured_ltp;
   INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
@@ -285,6 +312,16 @@ static inline void ahrs_update_mag_2d(void) {
 
 }
 
+void ahrs_update_gps(void) {
+#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN && USE_GPS
+  if (gps.fix == GPS_FIX_3D) {
+    ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(gps.speed_3d / 100.);
+    ahrs_impl.ltp_vel_norm_valid = TRUE;
+  } else {
+    ahrs_impl.ltp_vel_norm_valid = FALSE;
+  }
+#endif
+}
 
 /* Compute ltp to imu rotation in quaternion and rotation matrice representation
    from the euler angle representation */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
index faabe79bb6..7878e232ce 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
@@ -31,9 +31,9 @@ struct AhrsIntCmpl {
   struct Int32Rates  rate_correction;
   struct Int64Quat   high_rez_quat;
   struct Int64Rates  high_rez_bias;
-#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
   int32_t ltp_vel_norm;
-#endif
+  bool_t ltp_vel_norm_valid;
+  bool_t correct_gravity;
 };
 
 extern struct AhrsIntCmpl ahrs_impl;
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
index b07c4a81c6..4718311da1 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
@@ -44,9 +44,9 @@ static inline void compute_body_orientation(void);
 
 #define PI_INTEG_EULER     (INT32_ANGLE_PI * F_UPDATE)
 #define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
-#define INTEG_EULER_NORMALIZE(_a) {				\
-    while (_a >  PI_INTEG_EULER)  _a -= TWO_PI_INTEG_EULER;	\
-    while (_a < -PI_INTEG_EULER)  _a += TWO_PI_INTEG_EULER;	\
+#define INTEG_EULER_NORMALIZE(_a) {                         \
+    while (_a >  PI_INTEG_EULER)  _a -= TWO_PI_INTEG_EULER; \
+    while (_a < -PI_INTEG_EULER)  _a += TWO_PI_INTEG_EULER; \
   }
 
 void ahrs_init(void) {
@@ -78,7 +78,7 @@ void ahrs_align(void) {
 
   get_phi_theta_measurement_fom_accel(&ahrs_impl.hi_res_euler.phi, &ahrs_impl.hi_res_euler.theta, ahrs_aligner.lp_accel);
   get_psi_measurement_from_mag(&ahrs_impl.hi_res_euler.psi,
-			       ahrs_impl.hi_res_euler.phi/F_UPDATE, ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag);
+                               ahrs_impl.hi_res_euler.phi/F_UPDATE, ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag);
 
   EULERS_COPY(ahrs_impl.measure, ahrs_impl.hi_res_euler);
   EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler);
@@ -99,7 +99,7 @@ void ahrs_align(void) {
 //#define USE_NOISE_FILTER 1
 #define NOISE_FILTER_GAIN 50
 
-#ifdef USE_NOISE_CUT
+#if USE_NOISE_CUT
 #include "led.h"
 static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) {
   struct Int32Rates diff;
@@ -150,19 +150,19 @@ void ahrs_propagate(void) {
   /* unbias gyro             */
   struct Int32Rates uf_rate;
   RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias);
-#ifdef USE_NOISE_CUT
+#if USE_NOISE_CUT
   static struct Int32Rates last_uf_rate = { 0, 0, 0 };
   if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
 #endif
     /* low pass rate */
-#ifdef USE_NOISE_FILTER
+#if USE_NOISE_FILTER
     RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN);
     RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1);
 #else
     RATES_ADD(ahrs.imu_rate, uf_rate);
     RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
 #endif
-#ifdef USE_NOISE_CUT
+#if USE_NOISE_CUT
   }
   RATES_COPY(last_uf_rate, uf_rate);
 #endif
@@ -199,18 +199,18 @@ void ahrs_propagate(void) {
 
 void ahrs_update_accel(void) {
 
-#if defined(USE_NOISE_CUT) || defined(USE_NOISE_FILTER)
+#if USE_NOISE_CUT || USE_NOISE_FILTER
   static struct Int32Vect3 last_accel = { 0, 0, 0 };
 #endif
-#ifdef USE_NOISE_CUT
+#if USE_NOISE_CUT
   if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) {
 #endif
-#ifdef USE_NOISE_FILTER
+#if USE_NOISE_FILTER
     VECT3_SUM_SCALED(imu.accel, imu.accel, last_accel, NOISE_FILTER_GAIN);
     VECT3_SDIV(imu.accel, imu.accel, NOISE_FILTER_GAIN+1);
 #endif
     get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel);
-#ifdef USE_NOISE_CUT
+#if USE_NOISE_CUT
   }
   VECT3_COPY(last_accel, imu.accel);
 #endif
@@ -224,6 +224,10 @@ void ahrs_update_mag(void) {
 
 }
 
+void ahrs_update_gps(void) {
+
+}
+
 /* measures phi and theta assuming no dynamic acceleration ?!! */
 __attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) {
 
@@ -261,7 +265,7 @@ __attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag(
   //  sphi_ctheta * imu.mag.y +
   //  cphi_ctheta * imu.mag.z;
   float m_psi = -atan2(me, mn);
-  *psi_meas = ((m_psi - RadOfDeg(ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
+  *psi_meas = ((m_psi - ahrs_mag_offset)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
 
 }
 
diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c
index af06cf0704..290602091b 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_sim.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_sim.c
@@ -100,6 +100,10 @@ void ahrs_update_accel(void) {
 void ahrs_update_mag(void) {
 }
 
+void ahrs_update_gps(void) {
+
+}
+
 
 /*
  * Compute body orientation and rates from imu orientation and rates
diff --git a/sw/airborne/modem.h b/sw/airborne/subsystems/datalink/audio_telemetry.h
similarity index 71%
rename from sw/airborne/modem.h
rename to sw/airborne/subsystems/datalink/audio_telemetry.h
index ff76baf053..7829673eb5 100644
--- a/sw/airborne/modem.h
+++ b/sw/airborne/subsystems/datalink/audio_telemetry.h
@@ -1,7 +1,7 @@
 /*
  * $Id$
  *
- * Paparazzi mcu0 cmx469 modem functions
+ * Paparazzi telemetry via audio channel modem functions
  *
  * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
  *
@@ -24,17 +24,15 @@
  *
  */
 
-#ifndef MODEM_H
-#define MODEM_H
+#ifndef AUDIO_TELEMETRY_H
+#define AUDIO_TELEMETRY_H
 
 #include "inttypes.h"
 
-extern uint8_t modem_nb_ovrn;
-
-#ifdef MODEM
+extern uint8_t audio_telemetry_nb_ovrn;
 
 #include "generated/airframe.h"
-#include "modem_hw.h"
+#include "audio_telemetry_hw.h"
 
 
 #define TX_BUF_SIZE     255
@@ -46,7 +44,7 @@ extern uint8_t    tx_byte;
 extern uint8_t    tx_byte_idx;
 
 
-#define ModemSendMessage() MODEM_CHECK_RUNNING()
+#define AudioTelemetrySendMessage() AUDIO_TELEMETRY_CHECK_RUNNING()
 
 #if TX_BUF_SIZE == 256
 #define UPDATE_HEAD() {			   \
@@ -59,14 +57,14 @@ extern uint8_t    tx_byte_idx;
 }
 #endif
 
-#define ModemCheckFreeSpace(_space) (tx_head>=tx_tail? _space < (TX_BUF_SIZE - (tx_head - tx_tail)) : _space < (tx_tail - tx_head))
+#define AudioTelemetryCheckFreeSpace(_space) (tx_head>=tx_tail? _space < (TX_BUF_SIZE - (tx_head - tx_tail)) : _space < (tx_tail - tx_head))
 
-#define ModemPut1Byte(_byte) { \
+#define AudioTelemetryPut1Byte(_byte) { \
   tx_buf[tx_head] = _byte;	  \
   UPDATE_HEAD();		  \
 }
 
-#define MODEM_LOAD_NEXT_BYTE() { \
+#define AUDIO_TELEMETRY_LOAD_NEXT_BYTE() { \
   tx_byte = tx_buf[tx_tail]; \
   tx_byte_idx = 0; \
   tx_tail++; \
@@ -74,8 +72,6 @@ extern uint8_t    tx_byte_idx;
     tx_tail = 0; \
 }
 
-#define ModemTransmit(_x) ModemPut1Byte(_x)
+#define AudioTelemetryTransmit(_x) Audio_TelemetryPut1Byte(_x)
 
-#endif // MODEM
-
-#endif /* MODEM_H */
+#endif /* AUDIO_TELEMETRY_H */
diff --git a/sw/airborne/datalink.h b/sw/airborne/subsystems/datalink/datalink.h
similarity index 75%
rename from sw/airborne/datalink.h
rename to sw/airborne/subsystems/datalink/datalink.h
index 9baa824a92..455bb66ec3 100644
--- a/sw/airborne/datalink.h
+++ b/sw/airborne/subsystems/datalink/datalink.h
@@ -59,36 +59,26 @@ EXTERN uint8_t dl_buffer[MSG_SIZE]  __attribute__ ((aligned));
 EXTERN void dl_parse_msg(void);
 /** Should be called when chars are available in dl_buffer */
 
+/** Check for new message and parse */
+#define DlCheckAndParse() {   \
+  if (dl_msg_available) {			\
+    dl_parse_msg();				    \
+    dl_msg_available = FALSE;	\
+  }						                \
+}
+
 #if DATALINK == PPRZ
 
-#define DatalinkEvent() {			\
-  if (PprzBuffer()) {				\
-    ReadPprzBuffer();				\
-    if (pprz_msg_received) {			\
-      pprz_parse_payload();			\
-      pprz_msg_received = FALSE;		\
-    }						\
-  }						\
-  if (dl_msg_available) {			\
-    dl_parse_msg();				\
-    dl_msg_available = FALSE;			\
-  }						\
+#define DatalinkEvent() {			            \
+  PprzCheckAndParse(PPRZ_UART, pprz_tp);  \
+  DlCheckAndParse();                      \
 }
 
 #elif DATALINK == XBEE
 
-#define DatalinkEvent() {			\
-  if (XBeeBuffer()) {				\
-    ReadXBeeBuffer();				\
-    if (xbee_msg_received) {			\
-      xbee_parse_payload();			\
-      xbee_msg_received = FALSE;		\
-    }						\
-  }						\
-  if (dl_msg_available) {			\
-    dl_parse_msg();				\
-    dl_msg_available = FALSE;			\
-  }						\
+#define DatalinkEvent() {			            \
+  XBeeCheckAndParse(XBEE_UART, xbee_tp);  \
+  DlCheckAndParse();                      \
 }
 
 #else
diff --git a/sw/airborne/downlink.c b/sw/airborne/subsystems/datalink/downlink.c
similarity index 77%
rename from sw/airborne/downlink.c
rename to sw/airborne/subsystems/datalink/downlink.c
index 003b816beb..5249a92f2a 100644
--- a/sw/airborne/downlink.c
+++ b/sw/airborne/subsystems/datalink/downlink.c
@@ -31,20 +31,6 @@
 #include "std.h"
 #include "generated/airframe.h"
 
-#ifdef FBW
-#ifndef TELEMETRY_MODE_FBW
-#define TELEMETRY_MODE_FBW 0
-#endif
-uint8_t telemetry_mode_Fbw_DefaultChannel = TELEMETRY_MODE_FBW;
-#endif /** FBW */
-
-#ifdef AP
-#ifndef TELEMETRY_MODE_AP
-#define TELEMETRY_MODE_AP 0
-#endif
-uint8_t telemetry_mode_Ap_DefaultChannel = TELEMETRY_MODE_AP;
-#endif /** AP */
-
 uint8_t downlink_nb_ovrn;
 uint16_t downlink_nb_bytes;
 uint16_t downlink_nb_msgs;
diff --git a/sw/airborne/subsystems/datalink/downlink.h b/sw/airborne/subsystems/datalink/downlink.h
new file mode 100644
index 0000000000..cb81e4851d
--- /dev/null
+++ b/sw/airborne/subsystems/datalink/downlink.h
@@ -0,0 +1,122 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2003-2006  Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file downlink.h
+ *  \brief Common code for AP and FBW telemetry
+ *
+ */
+
+#ifndef DOWNLINK_H
+#define DOWNLINK_H
+
+#include <inttypes.h>
+
+#include "generated/modules.h"
+#include "messages.h"
+#include "generated/airframe.h" // AC_ID is required
+
+#if defined SITL
+
+#ifdef SIM_UART
+#include "sim_uart.h"
+#include "subsystems/datalink/pprz_transport.h"
+#include "subsystems/datalink/xbee.h"
+#else /* SIM_UART */
+/** Software In The Loop simulation uses IVY bus directly as the transport layer */
+#include "ivy_transport.h"
+#endif
+
+#else /** SITL */
+#include "subsystems/datalink/pprz_transport.h"
+#include "subsystems/datalink/xbee.h"
+#if USE_AUDIO_TELEMETRY
+#include "subsystems/datalink/audio_telemetry.h"
+#endif
+#ifdef USE_USB_SERIAL
+#include "mcu_periph/usb_serial.h"
+#endif
+#endif /** !SITL */
+
+#ifndef DefaultChannel
+#define DefaultChannel DOWNLINK_TRANSPORT
+#endif
+
+#ifndef DefaultDevice
+#define DefaultDevice DOWNLINK_DEVICE
+#endif
+
+/** Counter of messages not sent because of unavailibity of the output buffer*/
+extern uint8_t downlink_nb_ovrn;
+extern uint16_t downlink_nb_bytes;
+extern uint16_t downlink_nb_msgs;
+
+/* Transport macros
+ *
+ * call transport functions from channel
+ */
+#define __Transport(dev, _x) dev##_x
+#define _Transport(dev, _x) __Transport(dev, _x)
+#define Transport(_chan, _fun) _Transport(_chan, _fun)
+
+
+/** Set of macros for generated code (messages.h) from messages.xml */
+/** 2 = ac_id + msg_id */
+#define DownlinkIDsSize(_trans, _dev, _x) (_x+2)
+#define DownlinkSizeOf(_trans, _dev, _x) Transport(_trans, SizeOf(_dev, DownlinkIDsSize(_trans, _dev, _x)))
+
+#define DownlinkCheckFreeSpace(_trans, _dev, _x) Transport(_trans, CheckFreeSpace(_dev, (uint8_t)(_x)))
+
+#define DownlinkPutUint8(_trans, _dev, _x) Transport(_trans, PutUint8(_dev, _x))
+
+#define DownlinkPutInt8ByAddr(_trans, _dev, _x) Transport(_trans, PutInt8ByAddr(_dev, _x))
+#define DownlinkPutUint8ByAddr(_trans, _dev, _x) Transport(_trans, PutUint8ByAddr(_dev, _x))
+#define DownlinkPutInt16ByAddr(_trans, _dev, _x) Transport(_trans, PutInt16ByAddr(_dev, _x))
+#define DownlinkPutUint16ByAddr(_trans, _dev, _x) Transport(_trans, PutUint16ByAddr(_dev, _x))
+#define DownlinkPutInt32ByAddr(_trans, _dev, _x) Transport(_trans, PutInt32ByAddr(_dev, _x))
+#define DownlinkPutUint32ByAddr(_trans, _dev, _x) Transport(_trans, PutUint32ByAddr(_dev, _x))
+#define DownlinkPutFloatByAddr(_trans, _dev, _x) Transport(_trans, PutFloatByAddr(_dev, _x))
+
+#define DownlinkPutDoubleByAddr(_trans, _dev, _x) Transport(_trans, PutDoubleByAddr(_dev, _x))
+
+#define DownlinkPutFloatArray(_trans, _dev, _n, _x) Transport(_trans, PutFloatArray(_dev, _n, _x))
+#define DownlinkPutDoubleArray(_trans, _dev, _n, _x) Transport(_trans, PutDoubleArray(_dev, _n, _x))
+#define DownlinkPutInt16Array(_trans, _dev, _n, _x) Transport(_trans, PutInt16Array(_dev, _n, _x))
+#define DownlinkPutUint16Array(_trans, _dev, _n, _x) Transport(_trans, PutUint16Array(_dev, _n, _x))
+#define DownlinkPutInt32Array(_trans, _dev, _n, _x) Transport(_trans, PutInt32Array(_dev, _n, _x))
+#define DownlinkPutUint32Array(_trans, _dev, _n, _x) Transport(_trans, PutUint32Array(_dev, _n, _x))
+#define DownlinkPutUint8Array(_trans, _dev, _n, _x) Transport(_trans, PutUint8Array(_dev, _n, _x))
+
+#define DownlinkOverrun(_trans, _dev) downlink_nb_ovrn++;
+#define DownlinkCountBytes(_trans, _dev, _n) downlink_nb_bytes += _n;
+
+#define DownlinkStartMessage(_trans, _dev, _name, msg_id, payload_len) { \
+  downlink_nb_msgs++; \
+  Transport(_trans, Header(_dev, DownlinkIDsSize(_trans, _dev, payload_len))); \
+  Transport(_trans, PutUint8(_dev, AC_ID)); \
+  Transport(_trans, PutNamedUint8(_dev, _name, msg_id)); \
+}
+
+#define DownlinkEndMessage(_trans, _dev) Transport(_trans, Trailer(_dev))
+
+#endif /* DOWNLINK_H */
diff --git a/sw/airborne/downlink_transport.h b/sw/airborne/subsystems/datalink/downlink_transport.h
similarity index 100%
rename from sw/airborne/downlink_transport.h
rename to sw/airborne/subsystems/datalink/downlink_transport.h
diff --git a/sw/airborne/pprz_transport.c b/sw/airborne/subsystems/datalink/pprz_transport.c
similarity index 83%
rename from sw/airborne/pprz_transport.c
rename to sw/airborne/subsystems/datalink/pprz_transport.c
index 90b4a93e06..6c234a47de 100644
--- a/sw/airborne/pprz_transport.c
+++ b/sw/airborne/subsystems/datalink/pprz_transport.c
@@ -23,11 +23,9 @@
  */
 
 #include <inttypes.h>
-#include "pprz_transport.h"
+#include "subsystems/datalink/pprz_transport.h"
 #include "mcu_periph/uart.h"
 
 uint8_t ck_a, ck_b;
-volatile bool_t pprz_msg_received = FALSE;
-uint8_t pprz_ovrn, pprz_error;
-volatile uint8_t pprz_payload_len;
-uint8_t pprz_payload[PPRZ_PAYLOAD_LEN];
+
+struct pprz_transport pprz_tp;
diff --git a/sw/airborne/subsystems/datalink/pprz_transport.h b/sw/airborne/subsystems/datalink/pprz_transport.h
new file mode 100644
index 0000000000..b65cd59a33
--- /dev/null
+++ b/sw/airborne/subsystems/datalink/pprz_transport.h
@@ -0,0 +1,223 @@
+/*
+ * Paparazzi $Id$
+ *
+ * Copyright (C) 2003  Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file pprz_transport.h
+ *  \brief Building and parsing Paparazzi frames
+ *
+ *  Pprz frame:
+ *
+ *   |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B|
+ *
+ *   where checksum is computed over length and payload:
+ *     ck_A = ck_B = length
+ *     for each byte b in payload
+ *       ck_A += b; ck_b += ck_A
+ */
+
+#ifndef PPRZ_TRANSPORT_H
+#define PPRZ_TRANSPORT_H
+
+#include <inttypes.h>
+#include "std.h"
+#include "subsystems/datalink/datalink.h"
+#include "subsystems/datalink/transport.h"
+
+/* PPRZ Transport
+ * downlink macros
+ */
+extern uint8_t ck_a, ck_b;
+
+#define STX  0x99
+
+/** 4 = STX + len + ck_a + ck_b */
+#define PprzTransportSizeOf(_dev, _payload) (_payload+4)
+
+#define PprzTransportCheckFreeSpace(_dev, _x) TransportLink(_dev, CheckFreeSpace(_x))
+#define PprzTransportPut1Byte(_dev, _x) TransportLink(_dev, Transmit(_x))
+#define PprzTransportSendMessage(_dev) TransportLink(_dev, SendMessage())
+
+#define PprzTransportHeader(_dev, payload_len) { \
+  PprzTransportPut1Byte(_dev, STX);				\
+  uint8_t msg_len = PprzTransportSizeOf(_dev, payload_len);	\
+  PprzTransportPut1Byte(_dev, msg_len);			\
+  ck_a = msg_len; ck_b = msg_len;			\
+}
+
+#define PprzTransportTrailer(_dev) { \
+  PprzTransportPut1Byte(_dev, ck_a);	\
+  PprzTransportPut1Byte(_dev, ck_b);	\
+  PprzTransportSendMessage(_dev) \
+}
+
+#define PprzTransportPutUint8(_dev, _byte) { \
+    ck_a += _byte;			  \
+    ck_b += ck_a;			  \
+    PprzTransportPut1Byte(_dev, _byte);		  \
+ }
+
+#define PprzTransportPutNamedUint8(_dev, _name, _byte) PprzTransportPutUint8(_dev, _byte)
+
+#define PprzTransportPut1ByteByAddr(_dev, _byte) {	 \
+    uint8_t _x = *(_byte);		 \
+    PprzTransportPutUint8(_dev, _x);	 \
+  }
+
+#define PprzTransportPut2ByteByAddr(_dev, _byte) { \
+    PprzTransportPut1ByteByAddr(_dev, _byte);	\
+    PprzTransportPut1ByteByAddr(_dev, (const uint8_t*)_byte+1);	\
+  }
+
+#define PprzTransportPut4ByteByAddr(_dev, _byte) { \
+    PprzTransportPut2ByteByAddr(_dev, _byte);	\
+    PprzTransportPut2ByteByAddr(_dev, (const uint8_t*)_byte+2);	\
+  }
+
+#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */
+#define PprzTransportPutDoubleByAddr(_dev, _byte) { \
+    PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4);	\
+    PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte);	\
+  }
+#else
+#define PprzTransportPutDoubleByAddr(_dev, _byte) { \
+    PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte);	\
+    PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4);	\
+  }
+#endif
+
+
+#define PprzTransportPutInt8ByAddr(_dev, _x) PprzTransportPut1ByteByAddr(_dev, _x)
+#define PprzTransportPutUint8ByAddr(_dev, _x) PprzTransportPut1ByteByAddr(_dev, (const uint8_t*)_x)
+#define PprzTransportPutInt16ByAddr(_dev, _x) PprzTransportPut2ByteByAddr(_dev, (const uint8_t*)_x)
+#define PprzTransportPutUint16ByAddr(_dev, _x) PprzTransportPut2ByteByAddr(_dev, (const uint8_t*)_x)
+#define PprzTransportPutInt32ByAddr(_dev, _x) PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+#define PprzTransportPutUint32ByAddr(_dev, _x) PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+#define PprzTransportPutFloatByAddr(_dev, _x) PprzTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+
+#define PprzTransportPutArray(_dev, _put, _n, _x) { \
+  uint8_t _i; \
+  PprzTransportPutUint8(_dev, _n); \
+  for(_i = 0; _i < _n; _i++) { \
+    _put(_dev, &_x[_i]); \
+  } \
+}
+
+#define PprzTransportPutFloatArray(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutFloatByAddr, _n, _x)
+#define PprzTransportPutDoubleArray(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutDoubleByAddr, _n, _x)
+
+#define PprzTransportPutInt16Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutInt16ByAddr, _n, _x)
+#define PprzTransportPutUint16Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutUint16ByAddr, _n, _x)
+
+#define PprzTransportPutInt32Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutInt32ByAddr, _n, _x)
+#define PprzTransportPutUint32Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutUint32ByAddr, _n, _x)
+
+#define PprzTransportPutUint8Array(_dev, _n, _x) PprzTransportPutArray(_dev, PprzTransportPutUint8ByAddr, _n, _x)
+
+
+/** Receiving pprz messages */
+
+// PPRZ parsing state machine
+#define UNINIT      0
+#define GOT_STX     1
+#define GOT_LENGTH  2
+#define GOT_PAYLOAD 3
+#define GOT_CRC1    4
+
+struct pprz_transport {
+  // generic interface
+  struct transport trans;
+  // specific pprz transport variables
+  uint8_t status;
+  uint8_t payload_idx;
+  uint8_t ck_a, ck_b;
+};
+
+extern struct pprz_transport pprz_tp;
+
+static inline void parse_pprz(struct pprz_transport * t, uint8_t c ) {
+  switch (t->status) {
+  case UNINIT:
+    if (c == STX)
+      t->status++;
+    break;
+  case GOT_STX:
+    if (t->trans.msg_received) {
+      t->trans.ovrn++;
+      goto error;
+    }
+    t->trans.payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
+    t->ck_a = t->ck_b = c;
+    t->status++;
+    t->payload_idx = 0;
+    break;
+  case GOT_LENGTH:
+    t->trans.payload[t->payload_idx] = c;
+    t->ck_a += c; t->ck_b += t->ck_a;
+    t->payload_idx++;
+    if (t->payload_idx == t->trans.payload_len)
+      t->status++;
+    break;
+  case GOT_PAYLOAD:
+    if (c != t->ck_a)
+      goto error;
+    t->status++;
+    break;
+  case GOT_CRC1:
+    if (c != t->ck_b)
+      goto error;
+    t->trans.msg_received = TRUE;
+    goto restart;
+  default:
+    goto error;
+  }
+  return;
+ error:
+  t->trans.error++;
+ restart:
+  t->status = UNINIT;
+  return;
+}
+
+static inline void pprz_parse_payload(struct pprz_transport * t) {
+  uint8_t i;
+  for(i = 0; i < t->trans.payload_len; i++)
+    dl_buffer[i] = t->trans.payload[i];
+  dl_msg_available = TRUE;
+}
+
+
+#define PprzBuffer(_dev) TransportLink(_dev,ChAvailable())
+#define ReadPprzBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received)) parse_pprz(&(_trans),TransportLink(_dev,Getch())); }
+#define PprzCheckAndParse(_dev,_trans) {  \
+  if (PprzBuffer(_dev)) {                 \
+    ReadPprzBuffer(_dev,_trans);          \
+    if (_trans.trans.msg_received) {      \
+      pprz_parse_payload(&(_trans));      \
+      _trans.trans.msg_received = FALSE;  \
+    }                                     \
+  }                                       \
+}
+
+
+#endif /* PPRZ_TRANSPORT_H */
+
diff --git a/sw/airborne/subsystems/datalink/transport.h b/sw/airborne/subsystems/datalink/transport.h
new file mode 100644
index 0000000000..09d42ab980
--- /dev/null
+++ b/sw/airborne/subsystems/datalink/transport.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2011 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file transport.h
+ *  generic transport header
+ */
+
+#ifndef TRANSPORT_H
+#define TRANSPORT_H
+
+#include <inttypes.h>
+#include "std.h"
+
+#ifndef TRANSPORT_PAYLOAD_LEN
+#define TRANSPORT_PAYLOAD_LEN 256
+#endif
+
+/** Generic transport header
+ */
+struct transport {
+  // payload buffer
+  uint8_t payload[TRANSPORT_PAYLOAD_LEN];
+  // payload length
+  volatile uint8_t payload_len;
+  // message received flag
+  volatile bool_t msg_received;
+  // overrun and error flags
+  uint8_t ovrn, error;
+};
+
+/** Transport link macros
+ *
+ * make the link between the transport layer
+ * and the device layer
+ */
+#define __TransportLink(dev, _x) dev##_x
+#define _TransportLink(dev, _x)  __TransportLink(dev, _x)
+#define TransportLink(_dev, _x) _TransportLink(_dev, _x)
+
+#endif /* TRANSPORT_H */
+
diff --git a/sw/airborne/xbee.c b/sw/airborne/subsystems/datalink/xbee.c
similarity index 77%
rename from sw/airborne/xbee.c
rename to sw/airborne/subsystems/datalink/xbee.c
index dac33d7b48..904b4ba366 100644
--- a/sw/airborne/xbee.c
+++ b/sw/airborne/subsystems/datalink/xbee.c
@@ -24,19 +24,16 @@
 
 #include "sys_time.h"
 #include "print.h"
-#include "xbee.h"
+#include "subsystems/datalink/xbee.h"
 
 #ifdef SIM_UART
 #include "sim_uart.h"
 #endif
 
 uint8_t xbee_cs;
-uint8_t xbee_payload[XBEE_PAYLOAD_LEN];
-volatile bool_t xbee_msg_received;
-volatile uint8_t xbee_payload_len;
 uint8_t xbee_rssi;
-uint8_t xbee_ovrn, xbee_error;
 
+struct xbee_transport xbee_tp;
 
 #define AT_COMMAND_SEQUENCE "+++"
 #define AT_INIT_PERIOD_US 2000000
@@ -46,29 +43,32 @@ uint8_t xbee_ovrn, xbee_error;
 
 
 void xbee_init( void ) {
+  xbee_tp.status = XBEE_UNINIT;
+  xbee_tp.trans.msg_received = FALSE;
+
 #ifndef NO_XBEE_API_INIT
   /** - busy wait 1.25s */
   sys_time_usleep(1250000);
 
   /** Switching to AT mode (FIXME: busy waiting) */
-  XBeePrintString(AT_COMMAND_SEQUENCE);
+  XBeePrintString(XBEE_UART,AT_COMMAND_SEQUENCE);
 
   /** - busy wait 1.25s */
   sys_time_usleep(1250000);
 
   /** Setting my address */
-  XBeePrintString(AT_SET_MY);
+  XBeePrintString(XBEE_UART,AT_SET_MY);
   uint16_t addr = XBEE_MY_ADDR;
-  XBeePrintHex16(addr);
-  XBeePrintString("\r");
+  XBeePrintHex16(XBEE_UART,addr);
+  XBeePrintString(XBEE_UART,"\r");
 
-  XBeePrintString(AT_AP_MODE);
+  XBeePrintString(XBEE_UART,AT_AP_MODE);
 
 #ifdef XBEE_INIT
-  XBeePrintString(XBEE_INIT);
+  XBeePrintString(XBEE_UART,XBEE_INIT);
 #endif
 
   /** Switching back to normal mode */
-  XBeePrintString(AT_EXIT);
+  XBeePrintString(XBEE_UART,AT_EXIT);
 #endif
 }
diff --git a/sw/airborne/subsystems/datalink/xbee.h b/sw/airborne/subsystems/datalink/xbee.h
new file mode 100644
index 0000000000..4163cb9060
--- /dev/null
+++ b/sw/airborne/subsystems/datalink/xbee.h
@@ -0,0 +1,238 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2006  Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/* Maxstream XBee serial input and output */
+
+#ifndef XBEE_H
+#define XBEE_H
+
+#include "subsystems/datalink/datalink.h"
+#include "generated/airframe.h"
+
+#ifdef XBEE868
+#include "subsystems/datalink/xbee868.h"
+#else /* Not 868 */
+#include "subsystems/datalink/xbee24.h"
+#endif
+#include "subsystems/datalink/transport.h"
+
+/** Constants for the API protocol */
+#define XBEE_START 0x7e
+#define TX_OPTIONS 0x00
+#define NO_FRAME_ID 0
+
+/** Ground station address */
+#define GROUND_STATION_ADDR 0x100
+
+extern uint8_t xbee_cs;
+extern uint8_t xbee_rssi;
+
+/** Initialisation in API mode and setting of the local address
+ * FIXME: busy wait */
+#define XBEE_MY_ADDR AC_ID
+void xbee_init( void );
+
+/* 5 = Start + len_msb + len_lsb + API_id + checksum */
+#define XBeeAPISizeOf(_dev, _x) (_x+5)
+
+#define XBeeTransportCheckFreeSpace(_dev, x) TransportLink(_dev, CheckFreeSpace(x))
+#define XBeeTransportPut1Byte(_dev, x) TransportLink(_dev, Transmit(x))
+#define XBeeTransportSendMessage(_dev) TransportLink(_dev, SendMessage())
+
+#define XBeeTransportPutUint8(_dev, _x) { \
+  xbee_cs += _x; \
+  XBeeTransportPut1Byte(_dev, _x); \
+}
+
+#define XBeeTransportPut1ByteByAddr(_dev, _byte) { \
+  uint8_t _x = *(_byte);	\
+  XBeeTransportPutUint8(_dev, _x);	 \
+ }
+
+#define XBeeTransportPut2Bytes(_dev, _x) { \
+  uint16_t x16 = _x; \
+  XBeeTransportPut1Byte(_dev, x16>>8); \
+  XBeeTransportPut1Byte(_dev, x16 & 0xff); \
+}
+
+#define XBeeTransportPut2ByteByAddr(_dev, _byte) { \
+    XBeeTransportPut1ByteByAddr(_dev, _byte);	\
+    XBeeTransportPut1ByteByAddr(_dev, (const uint8_t*)_byte+1);	\
+  }
+
+#define XBeeTransportPut4ByteByAddr(_dev, _byte) { \
+    XBeeTransportPut2ByteByAddr(_dev, _byte);	\
+    XBeeTransportPut2ByteByAddr(_dev, (const uint8_t*)_byte+2); \
+  }
+
+#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */
+#define XBeeTransportPutDoubleByAddr(_dev, _byte) { \
+    XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4);	\
+    XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte);	\
+  }
+#else
+#define XBeeTransportPutDoubleByAddr(_dev, _byte) { \
+    XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte);	\
+    XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_byte+4);	\
+  }
+#endif
+
+
+#define XBeeTransportPutInt8ByAddr(_dev, _x) XBeeTransportPut1ByteByAddr(_dev, _x)
+#define XBeeTransportPutUint8ByAddr(_dev, _x) XBeeTransportPut1ByteByAddr(_dev, (const uint8_t*)_x)
+#define XBeeTransportPutInt16ByAddr(_dev, _x) XBeeTransportPut2ByteByAddr(_dev, (const uint8_t*)_x)
+#define XBeeTransportPutUint16ByAddr(_dev, _x) XBeeTransportPut2ByteByAddr(_dev, (const uint8_t*)_x)
+#define XBeeTransportPutInt32ByAddr(_dev, _x) XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+#define XBeeTransportPutUint32ByAddr(_dev, _x) XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+#define XBeeTransportPutFloatByAddr(_dev, _x) XBeeTransportPut4ByteByAddr(_dev, (const uint8_t*)_x)
+#define XBeeTransportPutNamedUint8(_dev, _name, _byte) XBeeTransportPutUint8(_dev, _byte)
+
+#define XBeeTransportPutArray(_dev, _put, _n, _x) { \
+  uint8_t _i; \
+  XBeeTransportPutUint8(_dev, _n); \
+  for(_i = 0; _i < _n; _i++) { \
+    _put(_dev, &_x[_i]); \
+  } \
+}
+
+#define XBeeTransportPutInt16Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutInt16ByAddr, _n, _x)
+
+#define XBeeTransportPutUint16Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutUint16ByAddr, _n, _x)
+#define XBeeTransportPutUint8Array(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutUint8ByAddr, _n, _x)
+#define XBeeTransportPutFloatArray(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutFloatByAddr, _n, _x)
+#define XBeeTransportPutDoubleArray(_dev, _n, _x) XBeeTransportPutArray(_dev, XBeeTransportPutDoubleByAddr, _n, _x)
+
+
+
+#define XBeeTransportHeader(_dev, _len) { \
+  XBeeTransportPut1Byte(_dev, XBEE_START); \
+  uint8_t payload_len = XBeeAPISizeOf(_dev, _len); \
+  XBeeTransportPut2Bytes(_dev, payload_len); \
+  xbee_cs = 0; \
+  XBeeTransportPutTXHeader(_dev); \
+}
+
+#define XBeeTransportTrailer(_dev) { \
+  xbee_cs = 0xff - xbee_cs; \
+  XBeeTransportPut1Byte(_dev, xbee_cs); \
+  XBeeTransportSendMessage(_dev) \
+}
+
+
+
+/** Status of the API packet receiver automata */
+#define XBEE_UNINIT         0
+#define XBEE_GOT_START      1
+#define XBEE_GOT_LENGTH_MSB 2
+#define XBEE_GOT_LENGTH_LSB 3
+#define XBEE_GOT_PAYLOAD    4
+
+struct xbee_transport {
+  // generic interface
+  struct transport trans;
+  // specific pprz transport variables
+  uint8_t status;
+  uint8_t payload_idx;
+  uint8_t cs;
+};
+
+extern struct xbee_transport xbee_tp;
+
+/** Parsing a XBee API frame */
+static inline void parse_xbee( struct xbee_transport * t, uint8_t c ) {
+  switch (t->status) {
+  case XBEE_UNINIT:
+    if (c == XBEE_START)
+      t->status++;
+    break;
+  case XBEE_GOT_START:
+    if (t->trans.msg_received) {
+      t->trans.ovrn++;
+      goto error;
+    }
+    t->trans.payload_len = c<<8;
+    t->status++;
+    break;
+  case XBEE_GOT_LENGTH_MSB:
+    t->trans.payload_len |= c;
+    t->status++;
+    t->payload_idx = 0;
+    t->cs=0;
+    break;
+  case XBEE_GOT_LENGTH_LSB:
+    t->trans.payload[t->payload_idx] = c;
+    t->cs += c;
+    t->payload_idx++;
+    if (t->payload_idx == t->trans.payload_len)
+      t->status++;
+    break;
+  case XBEE_GOT_PAYLOAD:
+    if (c + t->cs != 0xff)
+      goto error;
+    t->trans.msg_received = TRUE;
+    goto restart;
+    break;
+  default:
+    goto error;
+  }
+  return;
+ error:
+  t->trans.error++;
+ restart:
+  t->status = XBEE_UNINIT;
+  return;
+}
+
+/** Parsing a frame data and copy the payload to the datalink buffer */
+static inline void xbee_parse_payload(struct xbee_transport * t) {
+  switch (t->trans.payload[0]) {
+  case XBEE_RX_ID:
+  case XBEE_TX_ID: /* Useful if A/C is connected to the PC with a cable */
+    XbeeGetRSSI(t->trans.payload);
+    uint8_t i;
+    for(i = XBEE_RFDATA_OFFSET; i < t->trans.payload_len; i++)
+      dl_buffer[i-XBEE_RFDATA_OFFSET] = t->trans.payload[i];
+    dl_msg_available = TRUE;
+    break;
+  default:
+    return;
+  }
+}
+
+#define XBeeBuffer(_dev) TransportLink(_dev,ChAvailable())
+#define ReadXBeeBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received)) parse_xbee(&(_trans),TransportLink(_dev,Getch())); }
+#define XBeeCheckAndParse(_dev,_trans) {  \
+  if (XBeeBuffer(_dev)) {                 \
+    ReadXBeeBuffer(_dev,_trans);          \
+    if (_trans.trans.msg_received) {      \
+      xbee_parse_payload(&(_trans));      \
+      _trans.trans.msg_received = FALSE;  \
+    }                                     \
+  }                                       \
+}
+
+#define XBeePrintString(_dev, s) TransportLink(_dev,PrintString(s))
+#define XBeePrintHex16(_dev, x) TransportLink(_dev,PrintHex16(x))
+
+#endif /* XBEE_H */
diff --git a/sw/airborne/xbee24.h b/sw/airborne/subsystems/datalink/xbee24.h
similarity index 72%
rename from sw/airborne/xbee24.h
rename to sw/airborne/subsystems/datalink/xbee24.h
index c217776f80..8b108f0f15 100644
--- a/sw/airborne/xbee24.h
+++ b/sw/airborne/subsystems/datalink/xbee24.h
@@ -31,17 +31,17 @@
 #define XBEE_RX_ID 0x81 /* 16 bits address */
 #define XBEE_RFDATA_OFFSET 5
 
-#define XBeeTransportPutTXHeader() { \
-  XBeeTransportPutUint8(XBEE_TX_ID); \
-  XBeeTransportPutUint8(NO_FRAME_ID); \
-  XBeeTransportPutUint8(GROUND_STATION_ADDR >> 8); \
-  XBeeTransportPutUint8(GROUND_STATION_ADDR & 0xff); \
-  XBeeTransportPutUint8(TX_OPTIONS); \
+#define XBeeTransportPutTXHeader(_dev) { \
+  XBeeTransportPutUint8(_dev, XBEE_TX_ID); \
+  XBeeTransportPutUint8(_dev, NO_FRAME_ID); \
+  XBeeTransportPutUint8(_dev, GROUND_STATION_ADDR >> 8); \
+  XBeeTransportPutUint8(_dev, GROUND_STATION_ADDR & 0xff); \
+  XBeeTransportPutUint8(_dev, TX_OPTIONS); \
 }
 
 /* 4 = frame_id + addr_msb + addr_lsb + options */
-#define XBeeTransportSizeOf(_x) XBeeAPISizeOf(_x+4)
+#define XBeeTransportSizeOf(_dev, _x) XBeeAPISizeOf(_dev, _x+4)
 
-#define XbeeGetRSSI() { xbee_rssi = xbee_payload[3]; }
+#define XbeeGetRSSI(_payload) { xbee_rssi = _payload[3]; }
 
 #endif // XBEE24_H
diff --git a/sw/airborne/xbee868.h b/sw/airborne/subsystems/datalink/xbee868.h
similarity index 58%
rename from sw/airborne/xbee868.h
rename to sw/airborne/subsystems/datalink/xbee868.h
index 4cf3ecb479..eb95e7b504 100644
--- a/sw/airborne/xbee868.h
+++ b/sw/airborne/subsystems/datalink/xbee868.h
@@ -31,26 +31,26 @@
 #define XBEE_RX_ID 0x90
 #define XBEE_RFDATA_OFFSET 12
 
-#define XBeeTransportPutTXHeader()  { \
-  XBeeTransportPutUint8(XBEE_TX_ID); \
-  XBeeTransportPutUint8(NO_FRAME_ID); \
-  XBeeTransportPutUint8(0x00); \
-  XBeeTransportPutUint8(0x00); \
-  XBeeTransportPutUint8(0x00); \
-  XBeeTransportPutUint8(0x00); \
-  XBeeTransportPutUint8(0x00); \
-  XBeeTransportPutUint8(0x00); \
-  XBeeTransportPutUint8(GROUND_STATION_ADDR >> 8); \
-  XBeeTransportPutUint8(GROUND_STATION_ADDR & 0xff); \
-  XBeeTransportPutUint8(0xff); \
-  XBeeTransportPutUint8(0xfe); \
-  XBeeTransportPutUint8(0x00); \
-  XBeeTransportPutUint8(TX_OPTIONS); \
+#define XBeeTransportPutTXHeader(_dev)  { \
+  XBeeTransportPutUint8(_dev, XBEE_TX_ID); \
+  XBeeTransportPutUint8(_dev, NO_FRAME_ID); \
+  XBeeTransportPutUint8(_dev, 0x00); \
+  XBeeTransportPutUint8(_dev, 0x00); \
+  XBeeTransportPutUint8(_dev, 0x00); \
+  XBeeTransportPutUint8(_dev, 0x00); \
+  XBeeTransportPutUint8(_dev, 0x00); \
+  XBeeTransportPutUint8(_dev, 0x00); \
+  XBeeTransportPutUint8(_dev, GROUND_STATION_ADDR >> 8); \
+  XBeeTransportPutUint8(_dev, GROUND_STATION_ADDR & 0xff); \
+  XBeeTransportPutUint8(_dev, 0xff); \
+  XBeeTransportPutUint8(_dev, 0xfe); \
+  XBeeTransportPutUint8(_dev, 0x00); \
+  XBeeTransportPutUint8(_dev, TX_OPTIONS); \
 }
 
 /* 13 = frame_id + addr==8 + 3 + options */
-#define XBeeTransportSizeOf(_x) XBeeAPISizeOf(_x+13)
+#define XBeeTransportSizeOf(_dev, _x) XBeeAPISizeOf(_dev, _x+13)
 
-#define XbeeGetRSSI() {}
+#define XbeeGetRSSI(_payload) {}
 
 #endif // XBEE868_H
diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c
index 4d50a634dc..0c827a85f2 100644
--- a/sw/airborne/subsystems/gps.c
+++ b/sw/airborne/subsystems/gps.c
@@ -19,6 +19,11 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/** @file gps.c
+ *  @brief Device independent GPS code
+ *
+ */
+
 #include "subsystems/gps.h"
 
 #include "led.h"
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
index 2d46b16c0d..64370bb209 100644
--- a/sw/airborne/subsystems/gps.h
+++ b/sw/airborne/subsystems/gps.h
@@ -20,7 +20,7 @@
  */
 
 /** @file gps.h
- *  @brief Device independent GPS code
+ *  @brief Device independent GPS code (interface)
  *
  */
 
@@ -48,16 +48,17 @@
 #define GPS_NB_CHANNELS 1
 #endif
 
-/** Space Vehicle Information */
+/** data structure for Space Vehicle Information of a single satellite */
 struct SVinfo {
-  uint8_t svid;
-  uint8_t flags;
-  uint8_t qi;
-  uint8_t cno;
-  int8_t elev;  ///< deg
-  int16_t azim; ///< deg
+  uint8_t svid;  ///< Satellite ID
+  uint8_t flags; ///< bitfield with GPS receiver specific flags
+  uint8_t qi;    ///< quality bitfield (GPS receiver specific)
+  uint8_t cno;   ///< Carrier to Noise Ratio (Signal Strength) in dbHz
+  int8_t elev;   ///< elevation in deg
+  int16_t azim;  ///< azimuth in deg
 };
 
+/** data structure for GPS information */
 struct GpsState {
   struct EcefCoor_i ecef_pos;    ///< position in ECEF in cm
   struct LlaCoor_i lla_pos;      ///< position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
@@ -77,7 +78,7 @@ struct GpsState {
   uint32_t tow;                  ///< time of week in ms
 
   uint8_t nb_channels;           ///< Number of scanned satellites
-  struct SVinfo svinfos[GPS_NB_CHANNELS];
+  struct SVinfo svinfos[GPS_NB_CHANNELS]; ///< holds information from the Space Vehicles (Satellites)
 
   uint32_t last_fix_ticks;       ///< cpu time in ticks at last valid fix
   uint16_t last_fix_time;        ///< cpu time in sec at last valid fix
@@ -90,6 +91,7 @@ struct GpsTimeSync {
   uint32_t t0;          ///< for time sync: hw clock ticks when GPS message is received
 };
 
+/** global GPS state */
 extern struct GpsState gps;
 
 
diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c
index aaa96859e6..79a157db86 100644
--- a/sw/airborne/subsystems/gps/gps_mtk.c
+++ b/sw/airborne/subsystems/gps/gps_mtk.c
@@ -386,6 +386,8 @@ static bool_t user_gps_configure(bool_t cpt) {
   case 1:
     MtkSend_CFG(MTK_DIY_OUTPUT_RATE);
     return FALSE;
+  default:
+    break;
   }
   return TRUE; /* Continue, except for the last case */
 }
diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
index d22f1643a6..f172a64275 100644
--- a/sw/airborne/subsystems/imu.c
+++ b/sw/airborne/subsystems/imu.c
@@ -21,6 +21,10 @@
  * Boston, MA 02111-1307, USA.
  */
 
+/** \file imu.c
+ * \brief Inertial Measurement Unit interface
+ */
+
 #include "subsystems/imu.h"
 
 struct Imu imu;
diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h
index 52f0a9afa2..fb3512e365 100644
--- a/sw/airborne/subsystems/imu.h
+++ b/sw/airborne/subsystems/imu.h
@@ -20,6 +20,9 @@
  * the Free Software Foundation, 59 Temple Place - Suite 330,
  * Boston, MA 02111-1307, USA.
  */
+/** \file imu.h
+ * \brief Inertial Measurement Unit interface
+ */
 
 #ifndef IMU_H
 #define IMU_H
@@ -32,23 +35,24 @@
 extern void imu_impl_init(void);
 extern void imu_periodic(void);
 
+/** abstract IMU interface providing fixed point interface  */
 struct Imu {
-  struct Int32Rates gyro;
-  struct Int32Vect3 accel;
-  struct Int32Vect3 mag;
-  struct Int32Rates gyro_prev;
-  struct Int32Vect3 accel_prev;
-  struct Int32Rates gyro_neutral;
-  struct Int32Vect3 accel_neutral;
-  struct Int32Vect3 mag_neutral;
-  struct Int32Rates gyro_unscaled;
-  struct Int32Vect3 accel_unscaled;
-  struct Int32Vect3 mag_unscaled;
-  struct Int32Quat  body_to_imu_quat;
-  struct Int32RMat  body_to_imu_rmat;
+  struct Int32Rates gyro;             ///< gyroscope measurements
+  struct Int32Vect3 accel;            ///< accelerometer measurements
+  struct Int32Vect3 mag;              ///< magnetometer measurements
+  struct Int32Rates gyro_prev;        ///< previous gyroscope measurements
+  struct Int32Vect3 accel_prev;       ///< previous accelerometer measurements
+  struct Int32Rates gyro_neutral;     ///< gyroscope bias
+  struct Int32Vect3 accel_neutral;    ///< accelerometer bias
+  struct Int32Vect3 mag_neutral;      ///< magnetometer neutral readings (bias)
+  struct Int32Rates gyro_unscaled;    ///< unscaled gyroscope measurements
+  struct Int32Vect3 accel_unscaled;   ///< unscaled accelerometer measurements
+  struct Int32Vect3 mag_unscaled;     ///< unscaled magnetometer measurements
+  struct Int32Quat  body_to_imu_quat; ///< rotation from body to imu frame as a unit quaternion
+  struct Int32RMat  body_to_imu_rmat; ///< rotation from body to imu frame as a rotation matrix
 };
 
-/* abstract IMU interface providing floating point interface  */
+/** abstract IMU interface providing floating point interface  */
 struct ImuFloat {
   struct FloatRates   gyro;
   struct FloatVect3   accel;
@@ -61,6 +65,8 @@ struct ImuFloat {
 };
 
 extern void imu_float_init(struct ImuFloat* imuf);
+
+/** global IMU state */
 extern struct Imu imu;
 
 /* underlying hardware */
diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h
index f14a8f0bc7..a33b2b17c5 100644
--- a/sw/airborne/subsystems/imu/imu_aspirin.h
+++ b/sw/airborne/subsystems/imu/imu_aspirin.h
@@ -32,9 +32,27 @@
 #include "peripherals/hmc5843.h"
 #include "peripherals/adxl345.h"
 
+#ifdef IMU_ASPIRIN_VERSION_1_0
 #define IMU_MAG_X_CHAN 0
 #define IMU_MAG_Y_CHAN 1
 #define IMU_MAG_Z_CHAN 2
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN 1
+#define IMU_MAG_Z_SIGN 1
+#endif
+#endif
+
+#ifdef IMU_ASPIRIN_VERSION_1_5
+#define IMU_MAG_X_CHAN 2
+#define IMU_MAG_Y_CHAN 0
+#define IMU_MAG_Z_CHAN 1
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN -1
+#define IMU_MAG_Z_SIGN 1
+#endif
+#endif
 
 #if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
 #define IMU_GYRO_P_SIGN   1
@@ -46,11 +64,58 @@
 #define IMU_ACCEL_Y_SIGN  1
 #define IMU_ACCEL_Z_SIGN  1
 #endif
-#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
-#define IMU_MAG_X_SIGN    1
-#define IMU_MAG_Y_SIGN    1
-#define IMU_MAG_Z_SIGN    1
+
+/** default gyro sensitivy and neutral from the datasheet
+ * IMU-3000 has 16.4 LSB/(deg/s) at 2000deg/s range
+ * sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
+ * sens = 1/16.4 * pi/180 * 4096 = 4.359066229
+ */
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+#define IMU_GYRO_P_SENS 4.359
+#define IMU_GYRO_P_SENS_NUM 4359
+#define IMU_GYRO_P_SENS_DEN 1000
+#define IMU_GYRO_Q_SENS 4.359
+#define IMU_GYRO_Q_SENS_NUM 4359
+#define IMU_GYRO_Q_SENS_DEN 1000
+#define IMU_GYRO_R_SENS 4.359
+#define IMU_GYRO_R_SENS_NUM 4359
+#define IMU_GYRO_R_SENS_DEN 1000
 #endif
+#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
+#define IMU_GYRO_P_NEUTRAL 0
+#define IMU_GYRO_Q_NEUTRAL 0
+#define IMU_GYRO_R_NEUTRAL 0
+#endif
+
+
+/** default accel sensitivy from the ADXL345 datasheet
+ * sensitivity of x & y axes depends on supply voltage:
+ *   - 256 LSB/g @ 2.5V
+ *   - 265 LSB/g @ 3.3V
+ * z sensitivity stays at 256 LSB/g
+ * fixed point sens: 9.81 [m/s^2] / 256 [LSB/g] * 2^INT32_ACCEL_FRAC
+ * x/y sens = 9.81 / 265 * 1024 = 37.91
+ * z sens   = 9.81 / 256 * 1024 = 39.24
+ *
+ * what about the offset at 3.3V?
+ */
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+#define IMU_ACCEL_X_SENS 37.91
+#define IMU_ACCEL_X_SENS_NUM 3791
+#define IMU_ACCEL_X_SENS_DEN 100
+#define IMU_ACCEL_Y_SENS 37.91
+#define IMU_ACCEL_Y_SENS_NUM 3791
+#define IMU_ACCEL_Y_SENS_DEN 100
+#define IMU_ACCEL_Z_SENS 39.24
+#define IMU_ACCEL_Z_SENS_NUM 39.24
+#define IMU_ACCEL_Z_SENS_DEN 100
+#endif
+#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
+#define IMU_ACCEL_X_NEUTRAL 0
+#define IMU_ACCEL_Y_NEUTRAL 0
+#define IMU_ACCEL_Z_NEUTRAL 0
+#endif
+
 
 enum AspirinStatus
   { AspirinStatusUninit,
diff --git a/sw/airborne/subsystems/imu/imu_b2.c b/sw/airborne/subsystems/imu/imu_b2.c
index 5d4bf680a6..a41c6e9c37 100644
--- a/sw/airborne/subsystems/imu/imu_b2.c
+++ b/sw/airborne/subsystems/imu/imu_b2.c
@@ -34,6 +34,8 @@ void imu_impl_init(void) {
   ami601_init();
 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
   hmc5843_init();
+#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
+  hmc58xx_init();
 #endif
 
 }
diff --git a/sw/airborne/subsystems/imu/imu_b2.h b/sw/airborne/subsystems/imu/imu_b2.h
index 2c076d03ad..1860501be9 100644
--- a/sw/airborne/subsystems/imu/imu_b2.h
+++ b/sw/airborne/subsystems/imu/imu_b2.h
@@ -33,6 +33,8 @@
 #define IMU_B2_MAG_NONE   0
 #define IMU_B2_MAG_MS2100 1
 #define IMU_B2_MAG_AMI601 2
+#define IMU_B2_MAG_HMC5843 3
+#define IMU_B2_MAG_HMC58XX 4
 
 
 #ifdef IMU_B2_VERSION_1_0
@@ -167,7 +169,7 @@
     }									\
   }
 #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
-#include "peripherals/hmc5843.h"
+include "peripherals/hmc5843.h"
 #define foo_handler() {}
 #define ImuMagEvent(_mag_handler) {					\
     MagEvent(foo_handler);                          \
@@ -179,6 +181,19 @@
       hmc5843.data_available = FALSE;		\
     }									\
   }
+#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
+#include "peripherals/hmc58xx.h"
+#define foo_handler() {}
+#define ImuMagEvent(_mag_handler) {					\
+    MagEvent(foo_handler);                          \
+    if (hmc58xx_data_available) {			\
+      imu.mag_unscaled.x = hmc58xx_data.x;		\
+      imu.mag_unscaled.y = hmc58xx_data.y;		\
+      imu.mag_unscaled.z = hmc58xx_data.z;		\
+      _mag_handler();							\
+      hmc58xx_data_available = FALSE;		\
+    }									\
+  }
 #else
 #define ImuMagEvent(_mag_handler) {}
 #endif
diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c
index d25b367a84..63eb8e54ae 100644
--- a/sw/airborne/subsystems/ins.c
+++ b/sw/airborne/subsystems/ins.c
@@ -31,11 +31,11 @@
 
 #include "subsystems/ahrs.h"
 
-#ifdef USE_VFF
+#if USE_VFF
 #include "subsystems/ins/vf_float.h"
 #endif
 
-#ifdef USE_HFF
+#if USE_HFF
 #include "subsystems/ins/hf_float.h"
 #endif
 
@@ -58,7 +58,7 @@ struct LtpDef_i  ins_ltp_def;
          bool_t  ins_ltp_initialised;
 struct NedCoor_i ins_gps_pos_cm_ned;
 struct NedCoor_i ins_gps_speed_cm_s_ned;
-#ifdef USE_HFF
+#if USE_HFF
 /* horizontal gps transformed to NED in meters as float */
 struct FloatVect2 ins_gps_pos_m_ned;
 struct FloatVect2 ins_gps_speed_m_s_ned;
@@ -66,11 +66,11 @@ struct FloatVect2 ins_gps_speed_m_s_ned;
 bool_t ins_hf_realign;
 
 /* barometer                   */
-#ifdef USE_VFF
+#if USE_VFF
 int32_t ins_qfe;
 bool_t  ins_baro_initialised;
 int32_t ins_baro_alt;
-#ifdef USE_SONAR
+#if USE_SONAR
 bool_t  ins_update_on_agl;
 int32_t ins_sonar_offset;
 #endif
@@ -87,25 +87,25 @@ struct EnuCoor_i ins_enu_accel;
 
 
 void ins_init() {
-#ifdef USE_INS_NAV_INIT
+#if USE_INS_NAV_INIT
   ins_ltp_initialised = TRUE;
 
   /** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
-  struct LlaCoor_i llh; /* Height above the ellipsoid */
-  llh.lat = INT32_RAD_OF_DEG(NAV_LAT0);
-  llh.lon = INT32_RAD_OF_DEG(NAV_LON0);
+  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
+  llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
+  llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
   /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
-  llh.alt = NAV_ALT0 + NAV_MSL0;
+  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
 
-  struct EcefCoor_i nav_init;
-  ecef_of_lla_i(&nav_init, &llh);
+  struct EcefCoor_i ecef_nav0;
+  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
 
-  ltp_def_from_ecef_i(&ins_ltp_def, &nav_init);
+  ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
   ins_ltp_def.hmsl = NAV_ALT0;
 #else
   ins_ltp_initialised  = FALSE;
 #endif
-#ifdef USE_VFF
+#if USE_VFF
   ins_baro_initialised = FALSE;
 #ifdef BOOZ2_SONAR
   ins_update_on_agl = FALSE;
@@ -114,7 +114,7 @@ void ins_init() {
 #endif
   ins_vf_realign = FALSE;
   ins_hf_realign = FALSE;
-#ifdef USE_HFF
+#if USE_HFF
   b2_hff_init(0., 0., 0., 0.);
 #endif
   INT32_VECT3_ZERO(ins_ltp_pos);
@@ -128,7 +128,7 @@ void ins_init() {
 void ins_periodic( void ) {
 }
 
-#ifdef USE_HFF
+#if USE_HFF
 void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
   b2_hff_realign(pos, speed);
 }
@@ -138,7 +138,7 @@ void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatV
 
 
 void ins_realign_v(float z) {
-#ifdef USE_VFF
+#if USE_VFF
   vff_realign(z);
 #endif
 }
@@ -151,7 +151,7 @@ void ins_propagate() {
   INT32_RMAT_TRANSP_VMULT(accel_ltp, ahrs.ltp_to_body_rmat, accel_body);
   float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
 
-#ifdef USE_VFF
+#if USE_VFF
   if (baro.status == BS_RUNNING && ins_baro_initialised) {
     vff_propagate(z_accel_float);
     ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
@@ -165,7 +165,7 @@ void ins_propagate() {
   ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
 #endif /* USE_VFF */
 
-#ifdef USE_HFF
+#if USE_HFF
   /* propagate horizontal filter */
   b2_hff_propagate();
 #else
@@ -179,7 +179,7 @@ void ins_propagate() {
 }
 
 void ins_update_baro() {
-#ifdef USE_VFF
+#if USE_VFF
   if (baro.status == BS_RUNNING) {
     if (!ins_baro_initialised) {
       ins_qfe = baro.absolute;
@@ -190,7 +190,7 @@ void ins_update_baro() {
     if (ins_vf_realign) {
       ins_vf_realign = FALSE;
       ins_qfe = baro.absolute;
-#ifdef USE_SONAR
+#if USE_SONAR
       ins_sonar_offset = sonar_meas;
 #endif
       vff_realign(0.);
@@ -208,7 +208,7 @@ void ins_update_baro() {
 
 
 void ins_update_gps(void) {
-#ifdef USE_GPS
+#if USE_GPS
   if (gps.fix == GPS_FIX_3D) {
     if (!ins_ltp_initialised) {
       ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
@@ -218,7 +218,7 @@ void ins_update_gps(void) {
     }
     ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
     ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
-#ifdef USE_HFF
+#if USE_HFF
     VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
     VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
     VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
@@ -236,15 +236,15 @@ void ins_update_gps(void) {
 #endif
     }
     b2_hff_update_gps();
-#ifndef USE_VFF /* vff not used */
+#if !USE_VFF /* vff not used */
     ins_ltp_pos.z =  (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
     ins_ltp_speed.z =  (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
 #endif /* vff not used */
 #endif /* hff used */
 
 
-#ifndef USE_HFF /* hff not used */
-#ifndef USE_VFF /* neither hf nor vf used */
+#if !USE_HFF /* hff not used */
+#if !USE_VFF /* neither hf nor vf used */
     INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
     INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
 #else /* only vff used */
@@ -252,7 +252,7 @@ void ins_update_gps(void) {
     INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
 #endif
 
-#ifdef USE_GPS_LAG_HACK
+#if USE_GPS_LAG_HACK
     VECT2_COPY(d_pos, ins_ltp_speed);
     INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
     VECT2_ADD(ins_ltp_pos, d_pos);
@@ -267,7 +267,7 @@ void ins_update_gps(void) {
 }
 
 void ins_update_sonar() {
-#if defined USE_SONAR && defined USE_VFF
+#if USE_SONAR && USE_VFF
   static int32_t sonar_filtered = 0;
   sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
   /* update baro_qfe assuming a flat ground */
diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h
index 6353bbb553..83bb470096 100644
--- a/sw/airborne/subsystems/ins.h
+++ b/sw/airborne/subsystems/ins.h
@@ -33,11 +33,11 @@ extern struct NedCoor_i ins_gps_pos_cm_ned;
 extern struct NedCoor_i ins_gps_speed_cm_s_ned;
 
 /* barometer                   */
-#ifdef USE_VFF
+#if USE_VFF
 extern int32_t ins_baro_alt;
 extern int32_t ins_qfe;
 extern bool_t  ins_baro_initialised;
-#ifdef USE_SONAR
+#if USE_SONAR
 extern bool_t  ins_update_on_agl; /* use sonar to update agl if available */
 extern int32_t ins_sonar_offset;
 #endif
@@ -51,7 +51,7 @@ extern struct NedCoor_i ins_ltp_accel;
 extern struct EnuCoor_i ins_enu_pos;
 extern struct EnuCoor_i ins_enu_speed;
 extern struct EnuCoor_i ins_enu_accel;
-#ifdef USE_HFF
+#if USE_HFF
 /* horizontal gps transformed to NED in meters as float */
 extern struct FloatVect2 ins_gps_pos_m_ned;
 extern struct FloatVect2 ins_gps_speed_m_s_ned;
diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c
index 88e2c65857..f9cdcc78bc 100644
--- a/sw/airborne/subsystems/ins/hf_float.c
+++ b/sw/airborne/subsystems/ins/hf_float.c
@@ -487,7 +487,7 @@ void b2_hff_propagate(void) {
 void b2_hff_update_gps(void) {
   b2_hff_lost_counter = 0;
 
-#ifdef USE_GPS_ACC4R
+#if USE_GPS_ACC4R
   Rgps_pos = (float) gps.pacc / 100.;
   if (Rgps_pos < HFF_R_POS_MIN)
     Rgps_pos = HFF_R_POS_MIN;
diff --git a/sw/airborne/subsystems/navigation/border_line.c b/sw/airborne/subsystems/navigation/border_line.c
new file mode 100644
index 0000000000..5dd7d42c3c
--- /dev/null
+++ b/sw/airborne/subsystems/navigation/border_line.c
@@ -0,0 +1,138 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2012 Tobias Muench
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+
+//modified nav_linie by Anton Kochevar, ENAC
+
+/**
+ * @file subsystems/navigation/border_line.c
+ * @brief navigate along a border line (line 1-2) with turns in the same direction
+ *
+ * you can use this function to navigate along a border if it is essetial not to cross it
+ * navigation is along line p1, p2 with turns in the same direction to make sure you dont cross the line
+ * take care youre navigation radius is not to small in strong wind conditions!
+*/
+
+#include "subsystems/navigation/border_line.h"
+#include "generated/airframe.h"
+#include "subsystems/nav.h"
+
+
+enum border_line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 };
+static enum border_line_status border_line_status;
+
+bool_t border_line_init( void ) {
+  border_line_status = LR12;
+  return FALSE;
+}
+
+bool_t border_line(uint8_t l1, uint8_t l2, float radius) {
+  radius = fabs(radius);
+  float alt = waypoints[l1].a;
+  waypoints[l2].a = alt;
+
+  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
+  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
+  float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
+
+  float u_x = l2_l1_x / d;
+  float u_y = l2_l1_y / d;
+
+  float angle = atan2((WaypointY(l1)-WaypointY(l2)),(WaypointX(l2)-WaypointX(l1)));
+
+  struct point l2_c1 = { WaypointX(l1) - sin(angle)*radius,
+       WaypointY(l1) - cos(angle)*radius,
+       alt  };
+  struct point l2_c2 = { l2_c1.x + 2*radius*cos(angle) ,
+       l2_c1.y - 2*radius*sin(angle),
+       alt  };
+
+
+
+  struct point l1_c2 = { WaypointX(l2) - sin(angle)*radius,
+       WaypointY(l2) - cos(angle)*radius,
+       alt  };
+  struct point l1_c3 = { l1_c2.x - 2*radius*cos(angle) ,
+       l1_c2.y + 2*radius*sin(angle),
+       alt  };
+
+  float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
+  float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
+  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
+
+
+  NavVerticalAutoThrottleMode(0);
+  NavVerticalAltitudeMode(WaypointAlt(l1), 0.);
+
+  switch (border_line_status) {
+  case LR12:
+    NavSegment(l2, l1);
+    if (NavApproachingFrom(l1, l2, CARROT)) {
+      border_line_status = LQC21;
+      nav_init_stage();
+    }
+    break;
+  case LQC21:
+    nav_circle_XY(l2_c1.x, l2_c1.y, -radius);
+    if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)-10)) {
+      border_line_status = LTC2;
+      nav_init_stage();
+    }
+    break;
+  case LTC2:
+    nav_circle_XY(l2_c2.x, l2_c2.y, radius);
+    if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
+      border_line_status = LR21;
+      nav_init_stage();
+    }
+    break;
+
+  case LR21:
+    NavSegment(l1, l2);
+    if (NavApproachingFrom(l2, l1, CARROT)) {
+      border_line_status = LTC1;
+      nav_init_stage();
+    }
+    break;
+
+  case LTC1:
+    nav_circle_XY(l1_c2.x, l1_c2.y, radius);
+    if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) + 10)) {
+      border_line_status = LQC11;
+      nav_init_stage();
+    }
+    break;
+  case LQC11:
+    nav_circle_XY(l1_c3.x, l1_c3.y, -radius);
+    if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI + 10))) {
+      border_line_status = LR12;
+      nav_init_stage();
+    }
+    break;
+
+  default: /* Should not occur !!! End the pattern */
+    return FALSE;
+  }
+  return TRUE; /* This pattern never ends */
+}
diff --git a/sw/airborne/obsolete/booz_gps.c b/sw/airborne/subsystems/navigation/border_line.h
similarity index 66%
rename from sw/airborne/obsolete/booz_gps.c
rename to sw/airborne/subsystems/navigation/border_line.h
index 9aa4970e22..7c8094b26a 100644
--- a/sw/airborne/obsolete/booz_gps.c
+++ b/sw/airborne/subsystems/navigation/border_line.h
@@ -1,7 +1,7 @@
 /*
  * $Id$
  *
- * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2012, Tobias Muench
  *
  * This file is part of paparazzi.
  *
@@ -19,29 +19,22 @@
  * along with paparazzi; see the file COPYING.  If not, write to
  * the Free Software Foundation, 59 Temple Place - Suite 330,
  * Boston, MA 02111-1307, USA.
+ *
  */
 
-#include "booz_gps.h"
+/**
+ * @file subsystems/navigation/border_line.h
+ * @brief navigate along a border line (line 1-2) with turns in the same direction
+ */
 
-#include "led.h"
+#ifndef BORDER_LINE_H
+#define BORDER_LINE_H
 
-struct Booz_gps_state booz_gps_state;
+#include "std.h"
 
+extern bool_t border_line_init( void );
+extern bool_t border_line(uint8_t wp1, uint8_t wp2, float radius);
 
-#ifdef SITL
-bool_t booz_gps_available;
 #endif
 
-
-void booz_gps_init(void) {
-#ifdef SITL
-  booz_gps_available = FALSE;
-#endif
-  booz_gps_state.fix = BOOZ2_GPS_FIX_NONE;
-#ifdef GPS_LED
-  LED_OFF(GPS_LED);
-#endif
-#ifdef BOOZ_GPS_TYPE_H
-  booz_gps_impl_init();
-#endif
-}
+/* BORDER_LINE_H */
diff --git a/sw/airborne/subsystems/navigation/gls.c b/sw/airborne/subsystems/navigation/gls.c
new file mode 100644
index 0000000000..8f22c9548a
--- /dev/null
+++ b/sw/airborne/subsystems/navigation/gls.c
@@ -0,0 +1,176 @@
+/*
+ *
+ * Copyright (C) 2012, Tobias Münch
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file subsystems/navigation/gls.c
+ * @brief gps landing system
+ *
+ * gps landing system
+ * -automatic calculation of top of decent for const app angle
+ * -smooth intercept posible
+ * -landing direction is set by app fix / also possible in flight!!!
+ *
+ * in airframe.xml
+ * it is possible to define
+ *
+ * 1. target_speed
+ * 2. app_angle
+ * 3. app_intercept_af_tod
+ *
+ * 1 - only efective with useairspeed flag
+ * 2 - defauld is a approach angle of 5 degree which should be fine for most planes
+ * 3 - distance between approach fix and top of decent
+*/
+
+
+
+#include "generated/airframe.h"
+#include "estimator.h"
+#include "subsystems/navigation/gls.h"
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+
+
+
+float target_speed;
+float app_angle;
+float app_intercept_af_tod;
+
+bool_t init = TRUE;
+
+#ifndef APP_TARGET_SPEED
+#define APP_TARGET_SPEED V_CTL_AUTO_AIRSPEED_SETPOINT;
+#endif
+
+#ifndef APP_ANGLE
+#define APP_ANGLE RadOfDeg(5);
+#endif
+
+#ifndef APP_INTERCEPT_AF_TOD
+#define APP_INTERCEPT_AF_TOD 100
+#endif
+
+
+static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _tod, uint8_t _td) {
+
+  if ((WaypointX(_af)==WaypointX(_td))&&(WaypointY(_af)==WaypointY(_td))){
+  WaypointX(_af)=WaypointX(_td)-1;
+  }
+
+  float td_af_x = WaypointX(_af) - WaypointX(_td);
+  float td_af_y = WaypointY(_af) - WaypointY(_td);
+  float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
+  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tan(app_angle));
+
+  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
+  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
+  WaypointAlt(_tod) = WaypointAlt(_af);
+
+  if (td_tod > td_af) {
+    WaypointX(_af) = WaypointX(_tod) + td_af_x / td_af * app_intercept_af_tod;
+    WaypointY(_af) = WaypointY(_tod) + td_af_y / td_af * app_intercept_af_tod;
+  }
+  return FALSE;
+}	/* end of gls_copute_TOD */
+
+
+//###############################################################################################
+
+bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td) {
+
+  init = TRUE;
+
+#if USE_AIRSPEED
+  //float wind_additional = sqrt(wind_east*wind_east + wind_north*wind_north); // should be gusts only!
+  //Bound(wind_additional, 0, 0.5);
+  //target_speed = FL_ENVE_V_S * 1.3 + wind_additional; FIXME
+  target_speed =  APP_TARGET_SPEED; //  ok for now!
+#endif
+
+  app_angle = APP_ANGLE;
+  app_intercept_af_tod = APP_INTERCEPT_AF_TOD;
+  Bound(app_intercept_af_tod,0,200);
+
+
+  gls_compute_TOD(_af, _tod, _td);	// calculate Top Of Decent
+
+  return FALSE;
+}  /* end of gls_init() */
+
+
+//###############################################################################################
+
+
+bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
+
+
+  if (init){
+
+#if USE_AIRSPEED
+    v_ctl_auto_airspeed_setpoint = target_speed;			// set target speed for approach
+#endif
+    init = FALSE;
+  }
+
+
+  float final_x = WaypointX(_td) - WaypointX(_tod);
+  float final_y = WaypointY(_td) - WaypointY(_tod);
+  float final2 = Max(final_x * final_x + final_y * final_y, 1.);
+
+  float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
+  Bound(nav_final_progress,-1,1);
+  float nav_final_length = sqrt(final2);
+
+  float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
+  Bound(pre_climb, -5, 0.);
+
+  float start_alt = WaypointAlt(_tod);
+  float diff_alt = WaypointAlt(_td) - start_alt;
+  float alt = start_alt + nav_final_progress * diff_alt;
+  Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(v_ctl_altitude_pgain))) // to prevent climbing before intercept
+
+
+
+
+  if(nav_final_progress < -0.5) {			// for smooth intercept
+
+    NavVerticalAltitudeMode(WaypointAlt(_tod), 0);	// vertical mode (fly straigt and intercept glideslope)
+
+    NavVerticalAutoThrottleMode(0);		// throttle mode
+
+    NavSegment(_af, _td);				// horizontal mode (stay on localiser)
+  }
+
+  else {
+
+    NavVerticalAltitudeMode(alt, pre_climb);	// vertical mode (folow glideslope)
+
+    NavVerticalAutoThrottleMode(0);		// throttle mode
+
+    NavSegment(_af, _td);				// horizontal mode (stay on localiser)
+  }
+
+
+return TRUE;
+
+}	// end of gls()
diff --git a/sw/airborne/csc/csc_throttle.h b/sw/airborne/subsystems/navigation/gls.h
similarity index 69%
rename from sw/airborne/csc/csc_throttle.h
rename to sw/airborne/subsystems/navigation/gls.h
index 08d32e5ba6..f6f7ea71b8 100644
--- a/sw/airborne/csc/csc_throttle.h
+++ b/sw/airborne/subsystems/navigation/gls.h
@@ -1,6 +1,8 @@
 /*
- * Copyright (C) 2008 Joby Energy
  *
+ * Copyright (C) 2012, Tobias Muench
+ *
+ * This file is part of paparazzi.
  *
  * paparazzi is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -19,19 +21,20 @@
  *
  */
 
-/** \file uart_throttle.h
+/**
+ * @file subsystems/navigation/gls.h
+ * @brief gps landing system
  */
 
-#ifndef __CSC_THROTTLE_H__
-#define __CSC_THROTTLE_H__
+#ifndef NAV_GLS_H
+#define NAV_GLS_H
 
-#include "types.h"
 #include "std.h"
+#include "paparazzi.h"
 
 
-void csc_throttle_init( void );
-void csc_throttle_event_task( void );
-void csc_throttle_send_msg(uint8_t throttle_id, uint8_t cmd_id, uint16_t arg1, uint16_t arg2);
 
+extern bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td);
+extern bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td);
 
 #endif
diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c
index e05b26df5e..745ae53a8e 100644
--- a/sw/airborne/subsystems/navigation/spiral.c
+++ b/sw/airborne/subsystems/navigation/spiral.c
@@ -90,13 +90,9 @@ bool_t SpiralNav(void)
   TransCurrentY = estimator_y - WaypointY(Center);
   DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
 
-  bool_t InCircle = TRUE;
   float DistanceStartEstim;
   float CircleAlpha;
 
-  if(DistanceFromCenter > Spiralradius)
-	InCircle = FALSE;
-
   switch(CSpiralStatus)
 	{
 	case Outside:
diff --git a/sw/airborne/subsystems/sensors/infrared.c b/sw/airborne/subsystems/sensors/infrared.c
index caa00a1b5d..89e39bd693 100644
--- a/sw/airborne/subsystems/sensors/infrared.c
+++ b/sw/airborne/subsystems/sensors/infrared.c
@@ -38,8 +38,8 @@ struct Infrared infrared;
 /** \brief Initialisation of \a ir structure
  */
 void infrared_struct_init(void) {
-  infrared.roll_neutral = RadOfDeg(IR_ROLL_NEUTRAL_DEFAULT);
-  infrared.pitch_neutral = RadOfDeg(IR_PITCH_NEUTRAL_DEFAULT);
+  infrared.roll_neutral = IR_ROLL_NEUTRAL_DEFAULT;
+  infrared.pitch_neutral = IR_PITCH_NEUTRAL_DEFAULT;
 
   infrared.correction_left = IR_CORRECTION_LEFT;
   infrared.correction_right = IR_CORRECTION_RIGHT;
diff --git a/sw/airborne/subsystems/sensors/infrared_i2c.h b/sw/airborne/subsystems/sensors/infrared_i2c.h
index 855e186424..fec70ef777 100644
--- a/sw/airborne/subsystems/sensors/infrared_i2c.h
+++ b/sw/airborne/subsystems/sensors/infrared_i2c.h
@@ -49,7 +49,7 @@ extern void infrared_i2c_ver_event( void );
   if (irv_trans.status == I2CTransSuccess) infrared_i2c_ver_event(); \
 }
 
-#define infrared_i2cDownlink() DOWNLINK_SEND_DEBUG_IR_I2C(DefaultChannel, &ir_i2c.ir1, &ir_i2c.ir2, &ir_i2c.ir3)
+#define infrared_i2cDownlink() DOWNLINK_SEND_DEBUG_IR_I2C(DefaultChannel, DefaultDevice, &ir_i2c.ir1, &ir_i2c.ir2, &ir_i2c.ir3)
 
 #define infrared_i2c_SetConfWord(_v) { \
   ir_i2c_conf_hor_done = FALSE; \
diff --git a/sw/airborne/subsystems/settings.c b/sw/airborne/subsystems/settings.c
index 3f8447efb7..e32c194722 100644
--- a/sw/airborne/subsystems/settings.c
+++ b/sw/airborne/subsystems/settings.c
@@ -6,7 +6,7 @@ bool_t settings_store_now;
 
 
 void settings_init(void) {
-#ifdef USE_PERSISTENT_SETTINGS
+#if USE_PERSISTENT_SETTINGS
   if (persistent_read((uint32_t)&pers_settings, sizeof(struct PersistentSettings)))
     return; // return -1 ?
   persistent_settings_load();
@@ -15,7 +15,7 @@ void settings_init(void) {
 
 
 void settings_store(void) {
-#ifdef USE_PERSISTENT_SETTINGS
+#if USE_PERSISTENT_SETTINGS
   persistent_settings_store();
   persistent_write((uint32_t)&pers_settings, sizeof(struct PersistentSettings));
 #endif
diff --git a/sw/airborne/sys_time.h b/sw/airborne/sys_time.h
index a665e88c5c..0c9298d433 100644
--- a/sw/airborne/sys_time.h
+++ b/sw/airborne/sys_time.h
@@ -23,7 +23,7 @@
  */
 
 /*
- *\brief architecture independant timing functions
+ *\brief architecture independent timing functions
  *
  */
 
diff --git a/sw/airborne/test/ahrs/ahrs_on_synth.c b/sw/airborne/test/ahrs/ahrs_on_synth.c
index e3f9e97889..4ebf8127e7 100644
--- a/sw/airborne/test/ahrs/ahrs_on_synth.c
+++ b/sw/airborne/test/ahrs/ahrs_on_synth.c
@@ -178,9 +178,12 @@ void aos_compute_sensors(void) {
   MAGS_BFP_OF_REAL(imu.mag, h_imu);
 
 #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
-#if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
+#if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
   VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel);
 #endif
+#if AHRS_TYPE == AHRS_TYPE_FCR2
+  ahrs_impl.ltp_vel_norm = FLOAT_VECT3_NORM(aos.ltp_vel);
+#endif
 #if AHRS_TYPE == AHRS_TYPE_ICQ
   ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel));
 #endif
diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c
index bb04a7bfdf..a1acd5a394 100644
--- a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c
+++ b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c
@@ -40,7 +40,7 @@ gboolean timeout_callback(gpointer data) {
 #endif
 
 #if AHRS_TYPE == AHRS_TYPE_ICQ
-    IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
+    IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d",
 	       ahrs_impl.gyro_bias.p,
 	       ahrs_impl.gyro_bias.q,
 	       ahrs_impl.gyro_bias.r);
@@ -48,7 +48,7 @@ gboolean timeout_callback(gpointer data) {
 #if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2
     struct Int32Rates bias_i;
     RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias);
-    IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
+    IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d",
 	       bias_i.p,
 	       bias_i.q,
 	       bias_i.r);
diff --git a/sw/airborne/test/nova_test_imu.c b/sw/airborne/test/nova_test_imu.c
index 35f6a89d5e..743a589cfa 100644
--- a/sw/airborne/test/nova_test_imu.c
+++ b/sw/airborne/test/nova_test_imu.c
@@ -6,7 +6,7 @@
 
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "ADS8344.h"
 
diff --git a/sw/airborne/test/peripherals/test_ami601.c b/sw/airborne/test/peripherals/test_ami601.c
index 557e3c529d..1810ea3463 100644
--- a/sw/airborne/test/peripherals/test_ami601.c
+++ b/sw/airborne/test/peripherals/test_ami601.c
@@ -29,7 +29,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "mcu_periph/i2c.h"
 #include "peripherals/ami601.h"
@@ -64,7 +64,7 @@ static inline void main_init( void ) {
 }
 
 static inline void main_periodic_task( void ) {
-  //  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  //  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
 
   RunOnceEvery(10, { ami601_read();});
 }
@@ -79,6 +79,6 @@ static inline void on_mag(void) {
   LED_TOGGLE(4);
   ami601_status = AMI601_IDLE;
   struct Int32Vect3 bla = {ami601_values[0], ami601_values[1], ami601_values[2]};
-  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, &bla.x, &bla.y, &bla.z);
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &bla.x, &bla.y, &bla.z);
 
 }
diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c
index b8f90776f4..4d7c1a9c5c 100644
--- a/sw/airborne/test/subsystems/test_ahrs.c
+++ b/sw/airborne/test/subsystems/test_ahrs.c
@@ -29,7 +29,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/imu.h"
 #include "subsystems/ahrs.h"
@@ -118,49 +118,49 @@ static inline void main_report(void) {
 
   PeriodicPrescaleBy10(
 		       {
-			 DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
+			 DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
 						     &imu.accel_unscaled.x,
 						     &imu.accel_unscaled.y,
 						     &imu.accel_unscaled.z);
 		       },
 		       {
-			 DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
+			 DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
 						    &imu.gyro_unscaled.p,
 						    &imu.gyro_unscaled.q,
 						    &imu.gyro_unscaled.r);
 		       },
 		       {
-			 DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+			 DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
 						   &imu.mag_unscaled.x,
 						   &imu.mag_unscaled.y,
 						   &imu.mag_unscaled.z);
 		       },
 		       {
-			 DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
+			 DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice,
 						   &imu.accel.x,
 						   &imu.accel.y,
 						   &imu.accel.z);
 		       },
 		       {
-			 DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel,
+			 DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice,
 						  &imu.gyro.p,
 						  &imu.gyro.q,
 						  &imu.gyro.r);
 		       },
 
 		       {
-			 DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel,
+			 DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice,
 						 &imu.mag.x,
 						 &imu.mag.y,
 						 &imu.mag.z);
 		       },
 
 		       {
-			 DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+			 DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
 		       },
 		       {
 #ifdef USE_I2C2
-			 DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+			 DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
 						  &i2c2_errors.ack_fail_cnt,
 						  &i2c2_errors.miss_start_stop_cnt,
 						  &i2c2_errors.arb_lost_cnt,
@@ -173,7 +173,7 @@ static inline void main_report(void) {
 #endif
 		       },
 		       {
-			 DOWNLINK_SEND_BOOZ2_AHRS_EULER(DefaultChannel,
+			 DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
 							&ahrs.ltp_to_imu_euler.phi,
 							&ahrs.ltp_to_imu_euler.theta,
 							&ahrs.ltp_to_imu_euler.psi,
@@ -182,7 +182,7 @@ static inline void main_report(void) {
 							&ahrs.ltp_to_body_euler.psi);
 		       },
 		       {
-			 DOWNLINK_SEND_BOOZ_AHRS_BIAS(DefaultChannel,
+			 DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice,
 						      &ahrs_impl.gyro_bias.p,
 						      &ahrs_impl.gyro_bias.q,
 						      &ahrs_impl.gyro_bias.r);
diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c
index 9591438b63..5e1dd683bc 100644
--- a/sw/airborne/test/subsystems/test_imu.c
+++ b/sw/airborne/test/subsystems/test_imu.c
@@ -29,7 +29,7 @@
 #include "led.h"
 #include "mcu_periph/uart.h"
 #include "messages.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/imu.h"
 
@@ -69,11 +69,11 @@ static inline void main_init( void ) {
 static inline void main_periodic_task( void ) {
   RunOnceEvery(100, {
       //      LED_TOGGLE(3);
-      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
     });
 #ifdef USE_I2C2
   RunOnceEvery(111, {
-      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice,
                    &i2c2_errors.ack_fail_cnt,
                    &i2c2_errors.miss_start_stop_cnt,
                    &i2c2_errors.arb_lost_cnt,
@@ -102,13 +102,13 @@ static inline void on_accel_event(void) {
   cnt++;
   if (cnt > 15) cnt = 0;
   if (cnt == 0) {
-    DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
+    DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,
                 &imu.accel_unscaled.x,
                 &imu.accel_unscaled.y,
                 &imu.accel_unscaled.z);
   }
   else if (cnt == 7) {
-    DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
+    DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice,
                   &imu.accel.x,
                   &imu.accel.y,
                   &imu.accel.z);
@@ -123,13 +123,13 @@ static inline void on_gyro_accel_event(void) {
   if (cnt > 15) cnt = 0;
 
   if (cnt == 0) {
-    DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
+    DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,
                    &imu.gyro_unscaled.p,
                    &imu.gyro_unscaled.q,
                    &imu.gyro_unscaled.r);
   }
   else if (cnt == 7) {
-    DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel,
+    DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice,
                  &imu.gyro.p,
                  &imu.gyro.q,
                  &imu.gyro.r);
@@ -144,13 +144,13 @@ static inline void on_mag_event(void) {
   if (cnt > 10) cnt = 0;
 
   if (cnt == 0) {
-    DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel,
+    DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice,
                 &imu.mag.x,
                 &imu.mag.y,
                 &imu.mag.z);
   }
   else if (cnt == 5) {
-    DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
+    DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
                   &imu.mag_unscaled.x,
                   &imu.mag_unscaled.y,
                   &imu.mag_unscaled.z);
diff --git a/sw/airborne/test/subsystems/test_radio_control.c b/sw/airborne/test/subsystems/test_radio_control.c
index 9daf3a9afc..a0da36ee80 100644
--- a/sw/airborne/test/subsystems/test_radio_control.c
+++ b/sw/airborne/test/subsystems/test_radio_control.c
@@ -28,7 +28,7 @@
 #include "interrupt_hw.h"
 #include "mcu_periph/uart.h"
 
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 #include "subsystems/radio_control.h"
 
@@ -62,14 +62,14 @@ static inline void main_periodic_task( void ) {
   RunOnceEvery(51, {
     /*LED_TOGGLE(2);*/
     uint32_t blaaa= cpu_time_sec;
-    DOWNLINK_SEND_TIME(DefaultChannel, &blaaa);
+    DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &blaaa);
   });
 
   RunOnceEvery(10, {radio_control_periodic_task();});
 
   int16_t foo = 0;//RC_PPM_SIGNED_TICS_OF_USEC(2050-1500);
   RunOnceEvery(10,
-    {DOWNLINK_SEND_BOOZ2_RADIO_CONTROL(DefaultChannel,	\
+    {DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(DefaultChannel, DefaultDevice,	\
 				       &radio_control.values[RADIO_ROLL], \
 				       &radio_control.values[RADIO_PITCH], \
 				       &radio_control.values[RADIO_YAW], \
@@ -79,7 +79,7 @@ static inline void main_periodic_task( void ) {
 				       &radio_control.status);});
 #ifdef RADIO_CONTROL_TYPE_PPM
   RunOnceEvery(10,
-	       {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel,&blaa, 8, booz_radio_control_ppm_pulses);});
+	       {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice,&blaa, 8, booz_radio_control_ppm_pulses);});
 #endif
 
   LED_PERIODIC();
diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c
index 1f33f4aad1..9f9e496e9e 100644
--- a/sw/airborne/test/subsystems/test_settings.c
+++ b/sw/airborne/test/subsystems/test_settings.c
@@ -27,8 +27,8 @@
 #include "std.h"
 #include "mcu.h"
 #include "sys_time.h"
-#include "downlink.h"
-#include "datalink.h"
+#include "subsystems/datalink/downlink.h"
+#include "subsystems/datalink/datalink.h"
 #include "subsystems/settings.h"
 
 #include "mcu_periph/uart.h"
@@ -75,7 +75,7 @@ static inline void main_init( void ) {
 static inline void main_periodic( void ) {
 
   RunOnceEvery(100, {
-      DOWNLINK_SEND_ALIVE(DefaultChannel,  16, MD5SUM);
+      DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice,  16, MD5SUM);
       PeriodicSendDlValue(DefaultChannel);
     });
 
@@ -101,7 +101,7 @@ void dl_parse_msg(void) {
       uint8_t i = DL_SETTING_index(dl_buffer);
       float val = DL_SETTING_value(dl_buffer);
       DlSetting(i, val);
-      DOWNLINK_SEND_DL_VALUE(DefaultChannel, &i, &val);
+      DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
     }
     break;
   default:
diff --git a/sw/airborne/test/test_adcs.c b/sw/airborne/test/test_adcs.c
index 0bfd29a3b3..11eb994388 100644
--- a/sw/airborne/test/test_adcs.c
+++ b/sw/airborne/test/test_adcs.c
@@ -10,7 +10,7 @@
 #include "messages.h"
 #include "led.h"
 #include "mcu_periph/uart.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 
 
 #define NB_ADC 8
diff --git a/sw/airborne/test/test_telemetry.c b/sw/airborne/test/test_telemetry.c
index bd2760fc7f..4f51dad0e4 100644
--- a/sw/airborne/test/test_telemetry.c
+++ b/sw/airborne/test/test_telemetry.c
@@ -26,7 +26,7 @@
 #include "mcu_periph/uart.h"
 #include "interrupt_hw.h"
 #include "sys_time.h"
-#include "downlink.h"
+#include "subsystems/datalink/downlink.h"
 static inline void main_init( void );
 static inline void main_periodic( void );
 
@@ -48,7 +48,7 @@ static inline void main_init( void ) {
 }
 
 static inline void main_periodic( void ) {
-  RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
   LED_PERIODIC();
 }
 
diff --git a/sw/airborne/xbee.h b/sw/airborne/xbee.h
deleted file mode 100644
index 28b8493047..0000000000
--- a/sw/airborne/xbee.h
+++ /dev/null
@@ -1,233 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2006  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/* Maxstream XBee serial input and output */
-
-#ifndef XBEE_H
-#define XBEE_H
-
-#include "datalink.h"
-#include "generated/airframe.h"
-
-#ifdef XBEE868
-#include "xbee868.h"
-#else /* Not 868 */
-#include "xbee24.h"
-#endif
-
-/** Constants for the API protocol */
-#define XBEE_START 0x7e
-#define TX_OPTIONS 0x00
-#define NO_FRAME_ID 0
-
-#define GROUND_STATION_ADDR 0x100
-
-extern uint8_t xbee_cs;
-#define XBEE_PAYLOAD_LEN 256
-extern uint8_t xbee_payload[XBEE_PAYLOAD_LEN];
-
-extern volatile bool_t xbee_msg_received;
-extern volatile uint8_t xbee_payload_len;
-extern uint8_t xbee_rssi;
-extern uint8_t xbee_ovrn, xbee_error;
-
-/** Initialisation in API mode and setting of the local address
- * FIXME: busy wait */
-#define XBEE_MY_ADDR AC_ID
-void xbee_init( void );
-
-#define __Link(dev, _x) dev##_x
-#define _Link(dev, _x)  __Link(dev, _x)
-#define Link(_x) _Link(DOWNLINK_DEVICE, _x)
-
-#define XBeeTransportPut1Byte(x) Link(Transmit(x))
-#define XBeeTransportCheckFreeSpace(x) Link(CheckFreeSpace(x))
-/* 5 = Start + len_msb + len_lsb + API_id + checksum */
-#define XBeeAPISizeOf(_x) (_x+5)
-#define XBeeTransportSendMessage() Link(SendMessage())
-
-#define XBeeTransportPutUint8(_x) { \
-  xbee_cs += _x; \
-  XBeeTransportPut1Byte(_x); \
-}
-
-#define XBeeTransportPut1ByteByAddr(_byte) { \
-  uint8_t _x = *(_byte);	\
-  XBeeTransportPutUint8(_x);	 \
- }
-
-#define XBeeTransportPut2Bytes(_x) { \
-  uint16_t x16 = _x; \
-  XBeeTransportPut1Byte(x16>>8); \
-  XBeeTransportPut1Byte(x16 & 0xff); \
-}
-
-#define XBeeTransportPut2ByteByAddr(_byte) { \
-    XBeeTransportPut1ByteByAddr(_byte);	\
-    XBeeTransportPut1ByteByAddr((const uint8_t*)_byte+1);	\
-  }
-
-#define XBeeTransportPut4ByteByAddr(_byte) { \
-    XBeeTransportPut2ByteByAddr(_byte);	\
-    XBeeTransportPut2ByteByAddr((const uint8_t*)_byte+2); \
-  }
-
-#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */
-#define XBeeTransportPutDoubleByAddr(_byte) { \
-    XBeeTransportPut4ByteByAddr((const uint8_t*)_byte+4);	\
-    XBeeTransportPut4ByteByAddr((const uint8_t*)_byte);	\
-  }
-#else
-#define XBeeTransportPutDoubleByAddr(_byte) { \
-    XBeeTransportPut4ByteByAddr((const uint8_t*)_byte);	\
-    XBeeTransportPut4ByteByAddr((const uint8_t*)_byte+4);	\
-  }
-#endif
-
-
-#define XBeeTransportPutInt8ByAddr(_x) XBeeTransportPut1ByteByAddr(_x)
-#define XBeeTransportPutUint8ByAddr(_x) XBeeTransportPut1ByteByAddr((const uint8_t*)_x)
-#define XBeeTransportPutInt16ByAddr(_x) XBeeTransportPut2ByteByAddr((const uint8_t*)_x)
-#define XBeeTransportPutUint16ByAddr(_x) XBeeTransportPut2ByteByAddr((const uint8_t*)_x)
-#define XBeeTransportPutInt32ByAddr(_x) XBeeTransportPut4ByteByAddr((const uint8_t*)_x)
-#define XBeeTransportPutUint32ByAddr(_x) XBeeTransportPut4ByteByAddr((const uint8_t*)_x)
-#define XBeeTransportPutFloatByAddr(_x) XBeeTransportPut4ByteByAddr((const uint8_t*)_x)
-#define XBeeTransportPutNamedUint8(_name, _byte) XBeeTransportPutUint8(_byte)
-
-#define XBeeTransportPutArray(_put, _n, _x) { \
-  uint8_t _i; \
-  XBeeTransportPutUint8(_n); \
-  for(_i = 0; _i < _n; _i++) { \
-    _put(&_x[_i]); \
-  } \
-}
-
-#define XBeeTransportPutInt16Array(_n, _x) XBeeTransportPutArray(XBeeTransportPutInt16ByAddr, _n, _x)
-
-#define XBeeTransportPutUint16Array(_n, _x) XBeeTransportPutArray(XBeeTransportPutUint16ByAddr, _n, _x)
-#define XBeeTransportPutUint8Array(_n, _x) XBeeTransportPutArray(XBeeTransportPutUint8ByAddr, _n, _x)
-#define XBeeTransportPutFloatArray(_n, _x) XBeeTransportPutArray(XBeeTransportPutFloatByAddr, _n, _x)
-#define XBeeTransportPutDoubleArray(_n, _x) XBeeTransportPutArray(XBeeTransportPutDoubleByAddr, _n, _x)
-
-
-
-#define XBeeTransportHeader(_len) { \
-  XBeeTransportPut1Byte(XBEE_START); \
-  uint8_t payload_len = XBeeAPISizeOf(_len); \
-  XBeeTransportPut2Bytes(payload_len); \
-  xbee_cs = 0; \
-  XBeeTransportPutTXHeader(); \
-}
-
-#define XBeeTransportTrailer() { \
-  xbee_cs = 0xff - xbee_cs; \
-  XBeeTransportPut1Byte(xbee_cs); \
-  XBeeTransportSendMessage() \
-}
-
-
-
-/** Status of the API packet receiver automata */
-#define XBEE_UNINIT 0
-#define XBEE_GOT_START 1
-#define XBEE_GOT_LENGTH_MSB 2
-#define XBEE_GOT_LENGTH_LSB 3
-#define XBEE_GOT_PAYLOAD 4
-
-
-/** Parsing a XBee API frame */
-static inline void parse_xbee( uint8_t c ) {
-  static uint8_t xbee_status = XBEE_UNINIT;
-  static uint8_t cs, payload_idx;
-
-  switch (xbee_status) {
-  case XBEE_UNINIT:
-    if (c == XBEE_START)
-      xbee_status++;
-    break;
-  case XBEE_GOT_START:
-    if (xbee_msg_received) {
-      xbee_ovrn++;
-      goto error;
-    }
-    xbee_payload_len = c<<8;
-    xbee_status++;
-    break;
-  case XBEE_GOT_LENGTH_MSB:
-    xbee_payload_len |= c;
-    xbee_status++;
-    payload_idx = 0;
-    cs=0;
-    break;
-  case XBEE_GOT_LENGTH_LSB:
-    xbee_payload[payload_idx] = c;
-    cs += c;
-    payload_idx++;
-    if (payload_idx == xbee_payload_len)
-      xbee_status++;
-    break;
-  case XBEE_GOT_PAYLOAD:
-    if (c + cs != 0xff)
-      goto error;
-    xbee_msg_received = TRUE;
-    goto restart;
-    break;
-  default:
-    goto error;
-  }
-  return;
- error:
-  xbee_error++;
- restart:
-  xbee_status = XBEE_UNINIT;
-  return;
-}
-
-/** Parsing a frame data and copy the payload to the datalink buffer */
-static inline void xbee_parse_payload(void) {
-  switch (xbee_payload[0]) {
-  case XBEE_RX_ID:
-  case XBEE_TX_ID: /* Useful if A/C is connected to the PC with a cable */
-    XbeeGetRSSI();
-    uint8_t i;
-    for(i = XBEE_RFDATA_OFFSET; i < xbee_payload_len; i++)
-      dl_buffer[i-XBEE_RFDATA_OFFSET] = xbee_payload[i];
-    dl_msg_available = TRUE;
-    break;
-  default:
-    return;
-  }
-}
-
-#define __XBeeLink(dev, _x) dev##_x
-#define _XBeeLink(dev, _x)  __XBeeLink(dev, _x)
-#define XBeeLink(_x) _XBeeLink(XBEE_UART, _x)
-
-#define XBeeBuffer() XBeeLink(ChAvailable())
-#define ReadXBeeBuffer() { while (XBeeLink(ChAvailable())&&!xbee_msg_received) parse_xbee(XBeeLink(Getch())); }
-
-#define XBeePrintString(s) XBeeLink(PrintString(s))
-#define XBeePrintHex16(x) XBeeLink(PrintHex16(x))
-
-#endif /* XBEE_H */
diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml
index 6f832fa8a0..488aeb7f30 100644
--- a/sw/ground_segment/cockpit/gcs.ml
+++ b/sw/ground_segment/cockpit/gcs.ml
@@ -347,7 +347,7 @@ let options =
    "-center_ac", Arg.Set auto_center_new_ac, "Centers the map on any new A/C";
    "-edit", Arg.Unit (fun () -> edit := true; layout_file := "editor.xml"), "Flight plan editor";
    "-fullscreen", Arg.Set fullscreen, "Fullscreen window";
-   "-google_fill", Arg.Set GM.auto, "Google maps auto fill";
+   "-maps_fill", Arg.Set GM.auto, "Automatically start loading background maps";
    "-ign", Arg.String (fun s -> ign:=true; IGN.data_path := s), "IGN tiles path";
    "-lambertIIe", Arg.Unit (fun () -> projection:=G.LambertIIe),"Switch to LambertIIe projection";
    "-layout", Arg.Set_string layout_file, (sprintf "<XML layout specification> GUI layout. Default: %s" !layout_file);
@@ -356,12 +356,12 @@ let options =
    "-mercator", Arg.Unit (fun () -> projection:=G.Mercator),"Switch to (Google Maps) Mercator projection, default";
    "-mplayer", Arg.Set_string mplayer, "Launch mplayer with the given argument as X plugin";
    "-no_alarm", Arg.Set no_alarm, "Disables alarm page";
-   "-no_google_http", Arg.Unit (fun () -> Gm.set_policy Gm.NoHttp), "Switch off Google Maps downloading";
+   "-maps_no_http", Arg.Unit (fun () -> Gm.set_policy Gm.NoHttp), "Switch off downloading of maps, always use cached maps";
    "-ortho", Arg.Set_string get_bdortho, "IGN tiles path";
    "-osm", Arg.Unit (fun () -> Gm.set_maps_source Gm.OSM), "Use OpenStreetMap database (default is Google)";
    "-ms", Arg.Unit (fun () -> Gm.set_maps_source Gm.MS), "Use Microsoft maps database (default is Google)";
    "-particules", Arg.Set display_particules, "Display particules";
-    "-plugin", Arg.Set_string  plugin_window, "External X application (launched with the id of the plugin window as argument)";
+   "-plugin", Arg.Set_string  plugin_window, "External X application (launched with the id of the plugin window as argument)";
    "-ref", Arg.Set_string geo_ref, "Geographic ref (e.g. 'WGS84 43.605 1.443')";
    "-speech", Arg.Set Speech.active, "Enable vocal messages";
    "-srtm", Arg.Set srtm, "Enable SRTM elevation display";
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index 9018f8cb2a..e4cfaba693 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -1254,7 +1254,7 @@ let message_request = Ground_Pprz.message_req
 
 let get_ts = fun _sender vs ->
   let ac = get_ac vs in
-  let t = Pprz.float_assoc "time_since_last_bat_msg" vs in
+  let t = Pprz.float_assoc "time_since_last_msg" vs in
   ac.strip#set_label "telemetry_status" (if t > 2. then sprintf "%.0f" t else "   ");
   ac.strip#set_color "telemetry_status" (if t > 5. then alert_color else ok_color)
 
diff --git a/sw/ground_segment/cockpit/page_settings.ml b/sw/ground_segment/cockpit/page_settings.ml
index d4817d43dd..ace96e1217 100644
--- a/sw/ground_segment/cockpit/page_settings.ml
+++ b/sw/ground_segment/cockpit/page_settings.ml
@@ -30,26 +30,26 @@ let (//) = Filename.concat
 
 
 class setting = fun (i:int) (xml:Xml.xml) (current_value:GMisc.label) set_default ->
-  object
-    method index = i
-    method xml = xml
-    method current_value =
-      let auc = Pprz.alt_unit_coef_of_xml xml in
-      let (alt_a, alt_b) = Ocaml_tools.affine_transform auc in
-      (float_of_string current_value#text -. alt_b) /. alt_a
-    method update = fun s ->
-      if current_value#text <> s then begin
-	current_value#set_text s;
-	try set_default (float_of_string s) with Failure "float_of_string" -> ()
-      end
-  end
+object
+  method index = i
+  method xml = xml
+  method current_value =
+    let auc = Pprz.alt_unit_coef_of_xml xml in
+    let (alt_a, alt_b) = Ocaml_tools.affine_transform auc in
+    (float_of_string current_value#text -. alt_b) /. alt_a
+  method update = fun s ->
+    if current_value#text <> s then begin
+      current_value#set_text s;
+      try set_default (float_of_string s) with Failure "float_of_string" -> ()
+    end
+end
 
 let pipe_regexp = Str.regexp "|"
 let values_of_dl_setting = fun dl_setting ->
   try
     Array.of_list (Str.split pipe_regexp (Xml.attrib dl_setting "values"))
   with
-    _ -> [||]
+      _ -> [||]
 
 
 (* Look for the index of a value in a array. May raise Not_found *)
@@ -101,39 +101,39 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) packing dl_settin
       let value = ref lower in
       let callback = fun () -> do_change i !value in
       let update_value = fun index ->
-	modified := true;
-	value := float index;
-	if auto_but#active then callback () in
+        modified := true;
+        value := float index;
+        if auto_but#active then callback () in
       if Array.length values > 2 then (* Combo box *)
-	let strings = Array.to_list values in
-	let combo = Gtk_tools.combo strings hbox in
+        let strings = Array.to_list values in
+        let combo = Gtk_tools.combo strings hbox in
 
-	let update_string = fun string ->
-	  try
-	    update_value ((search_index string values) + truncate lower)
-	  with
-	    Not_found -> failwith (sprintf "Internal error: Settings, %s not found" string) in
-	Gtk_tools.combo_connect combo update_string;
+        let update_string = fun string ->
+          try
+            update_value ((search_index string values) + truncate lower)
+          with
+              Not_found -> failwith (sprintf "Internal error: Settings, %s not found" string) in
+        Gtk_tools.combo_connect combo update_string;
 
-	(callback, fun j -> try Gtk_tools.select_in_combo combo values.(truncate j) with _ -> ())
+        (callback, fun j -> try Gtk_tools.select_in_combo combo values.(truncate j) with _ -> ())
       else (* radio buttons *)
-	let ilower = truncate lower
-	and iupper = truncate upper in
-	let callback = fun _ -> do_change i !value in
-	let group = (GButton.radio_button ())#group in (* Group shared by the buttons *)
-	let buttons = Array.init (iupper-ilower+1)
-	    (fun j ->
-	      (* Build the button *)
-	      let label =
-		if Array.length values = 0
-		then Printf.sprintf "%d" (ilower + j)
-		else values.(j) in
-	      let b = GButton.radio_button ~group ~label ~packing:hbox#add () in
+        let ilower = truncate lower
+        and iupper = truncate upper in
+        let callback = fun _ -> do_change i !value in
+        let group = (GButton.radio_button ())#group in (* Group shared by the buttons *)
+        let buttons = Array.init (iupper-ilower+1)
+          (fun j ->
+              (* Build the button *)
+            let label =
+              if Array.length values = 0
+              then Printf.sprintf "%d" (ilower + j)
+              else values.(j) in
+            let b = GButton.radio_button ~group ~label ~packing:hbox#add () in
 
-	      (* Connect the event *)
-	      ignore (b#connect#pressed (fun () -> update_value (ilower + j)));
-	      b) in
-	(callback, fun j -> try buttons.(truncate j - ilower)#set_active true with _ -> ())
+              (* Connect the event *)
+            ignore (b#connect#pressed (fun () -> update_value (ilower + j)));
+            b) in
+        (callback, fun j -> try buttons.(truncate j - ilower)#set_active true with _ -> ())
     else (* slider *)
       let value = (lower +. upper) /. 2. in
       let adj = GData.adjustment ~value ~lower ~upper:(upper+.step_incr) ~step_incr ~page_incr ~page_size () in
@@ -173,49 +173,49 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) packing dl_settin
   let _icon = GMisc.image ~stock:`UNDO ~packing:undo_but#add () in
   let callback = fun _ ->
     match !prev_value with
-      None -> ()
-    | Some v -> do_change i v in
+        None -> ()
+      | Some v -> do_change i v in
   ignore (undo_but#connect#clicked ~callback);
   tooltips#set_tip undo_but#coerce ~text:"Undo";
 
   ignore (auto_but#connect#toggled
-    (fun () ->
-      commit_but#misc#set_sensitive (not auto_but#active);
-      undo_but#misc#set_sensitive (not auto_but#active)));
+            (fun () ->
+              commit_but#misc#set_sensitive (not auto_but#active);
+              undo_but#misc#set_sensitive (not auto_but#active)));
 
   (** Insert the related buttons in the strip and prepare the papgets DnD *)
   List.iter (fun x ->
     match String.lowercase (Xml.tag x) with
-      "strip_button" ->
-	let label = ExtXml.attrib x "name"
-	and sp_value = ExtXml.float_attrib x "value"
-        and group = ExtXml.attrib_or_default x "group" "" in
-	let b =
-	  try (* Is it an icon ? *)
-	    let icon = Xml.attrib x "icon" in
-	    let b = GButton.button () in
-	    let pixbuf = GdkPixbuf.from_file (Env.gcs_icons_path // icon) in
-	    ignore (GMisc.image ~pixbuf ~packing:b#add ());
+        "strip_button" ->
+          let label = ExtXml.attrib x "name"
+          and sp_value = ExtXml.float_attrib x "value"
+          and group = ExtXml.attrib_or_default x "group" "" in
+          let b =
+            try (* Is it an icon ? *)
+              let icon = Xml.attrib x "icon" in
+              let b = GButton.button () in
+              let pixbuf = GdkPixbuf.from_file (Env.gcs_icons_path // icon) in
+              ignore (GMisc.image ~pixbuf ~packing:b#add ());
 
-	    (* Drag for Drop *)
-	    let papget = Papget_common.xml "variable_setting" "button"
-		["variable", varname;
-		 "value", ExtXml.attrib x "value";
-		 "icon", icon] in
-	    Papget_common.dnd_source b#coerce papget;
+            (* Drag for Drop *)
+              let papget = Papget_common.xml "variable_setting" "button"
+                ["variable", varname;
+                 "value", ExtXml.attrib x "value";
+                 "icon", icon] in
+              Papget_common.dnd_source b#coerce papget;
 
             (* Associates the label as a tooltip *)
-	    tooltips#set_tip b#coerce ~text:label;
-	    b
-	  with
-	    Xml.No_attribute "icon" -> GButton.button ~label ()
-	  | exc ->
-	      prerr_endline (Printexc.to_string exc);
-	      GButton.button ~label () in
-	(strip group b#coerce: unit);
-	ignore (b#connect#clicked (fun _ -> do_change i sp_value))
-    | "key_press" -> add_key x (do_change i) keys
-    | t -> failwith (sprintf "Page_settings.one_setting, Unexpected tag: '%s'" t))
+              tooltips#set_tip b#coerce ~text:label;
+              b
+            with
+                Xml.No_attribute "icon" -> GButton.button ~label ()
+              | exc ->
+                prerr_endline (Printexc.to_string exc);
+                GButton.button ~label () in
+          (strip group b#coerce: unit);
+          ignore (b#connect#clicked (fun _ -> do_change i sp_value))
+      | "key_press" -> add_key x (do_change i) keys
+      | t -> failwith (sprintf "Page_settings.one_setting, Unexpected tag: '%s'" t))
     (Xml.children dl_setting);
 
   new setting i dl_setting current_value set_default
@@ -223,38 +223,38 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) packing dl_settin
 
 
 let same_tag_for_all = function
-    [] -> failwith "Page_settings: unreachable, empty dl_settings element"
+[] -> failwith "Page_settings: unreachable, empty dl_settings element"
   | x::xs ->
-      let tag_first = Xml.tag x in
-      List.iter (fun y -> assert(ExtXml.tag_is y tag_first)) xs;
-      String.lowercase tag_first
+    let tag_first = Xml.tag x in
+    List.iter (fun y -> assert(ExtXml.tag_is y tag_first)) xs;
+    String.lowercase tag_first
 
 
 (** Build the tree of settings *)
 let rec build_settings = fun do_change i flat_list keys xml_settings packing tooltips strip ->
   match same_tag_for_all xml_settings with
-    "dl_setting" ->
-      List.iter
-	(fun dl_setting ->
-	  let label_value = one_setting !i do_change packing dl_setting tooltips strip keys in
-	  flat_list := label_value :: !flat_list;
-	  incr i)
-	xml_settings
-  | "dl_settings" ->
+      "dl_setting" ->
+        List.iter
+          (fun dl_setting ->
+            let label_value = one_setting !i do_change packing dl_setting tooltips strip keys in
+            flat_list := label_value :: !flat_list;
+            incr i)
+          xml_settings
+    | "dl_settings" ->
       let n = GPack.notebook ~packing () in
 
       List.iter (fun dl_settings ->
-	let text = ExtXml.attrib dl_settings "name" in
-	let _sw = GBin.scrolled_window ~hpolicy:`AUTOMATIC ~vpolicy:`AUTOMATIC () in
-	let vbox = GPack.vbox  () in
+        let text = ExtXml.attrib dl_settings "name" in
+        let _sw = GBin.scrolled_window ~hpolicy:`AUTOMATIC ~vpolicy:`AUTOMATIC () in
+        let vbox = GPack.vbox  () in
 
-	let tab_label = (GMisc.label ~text ())#coerce in
-	ignore (n#append_page ~tab_label vbox#coerce);
+        let tab_label = (GMisc.label ~text ())#coerce in
+        ignore (n#append_page ~tab_label vbox#coerce);
 
-	let children = Xml.children dl_settings in
-	build_settings do_change i flat_list keys children vbox#pack tooltips strip)
-	xml_settings
-  | tag -> failwith (sprintf "Page_settings.build_settings, unexpected tag '%s'" tag)
+        let children = Xml.children dl_settings in
+        build_settings do_change i flat_list keys children vbox#pack tooltips strip)
+        xml_settings
+    | tag -> failwith (sprintf "Page_settings.build_settings, unexpected tag '%s'" tag)
 
 
 class settings = fun ?(visible = fun _ -> true) xml_settings do_change strip ->
@@ -269,31 +269,30 @@ class settings = fun ?(visible = fun _ -> true) xml_settings do_change strip ->
   let length = Array.length variables in
   let assocs =
     List.map (fun setting -> (ExtXml.attrib setting#xml "var", setting#index)) ordered_list in
-  object (self)
-    method widget = sw#coerce
-    method length = length
-    method keys = !keys
-    method set = fun i v ->
-      if visible self#widget then
-	let setting = variables.(i) in
-	let auc = Pprz.alt_unit_coef_of_xml setting#xml in
-	let (alt_a, alt_b) = Ocaml_tools.affine_transform auc in
-	let v = alt_a *. v +. alt_b in
-	let s = string_of_float v in
-	if i < 0 || i >= Array.length variables then
-	  failwith (sprintf "Pages.settings#set: %d out of bounnds (length=%d)" i (Array.length variables));
-	let s =
-	  let values = values_of_dl_setting setting#xml in
-	  try
-      let lower = int_of_string (ExtXml.attrib setting#xml "min") in
-	    values.(truncate v - lower)
-	  with
-	    _ -> s in
-	setting#update s
-    method assoc var = List.assoc var assocs
-    method save = fun airframe_filename ->
-      let settings = Array.fold_right (fun setting r -> try (setting#index, setting#xml, setting#current_value)::r with _ -> r) variables [] in
-      SaveSettings.popup airframe_filename (Array.of_list settings) do_change
-  end
-
+object (self)
+  method widget = sw#coerce
+  method length = length
+  method keys = !keys
+  method set = fun i v ->
+    if visible self#widget then
+      let setting = variables.(i) in
+      let auc = Pprz.alt_unit_coef_of_xml setting#xml in
+      let (alt_a, alt_b) = Ocaml_tools.affine_transform auc in
+      let v = alt_a *. v +. alt_b in
+      let s = string_of_float v in
+      if i < 0 || i >= Array.length variables then
+        failwith (sprintf "Pages.settings#set: %d out of bounnds (length=%d)" i (Array.length variables));
+      let s =
+        let values = values_of_dl_setting setting#xml in
+        try
+          let lower = int_of_string (ExtXml.attrib setting#xml "min") in
+          values.(truncate v - lower)
+        with
+            _ -> s in
+      setting#update s
+  method assoc var = List.assoc var assocs
+  method save = fun airframe_filename ->
+    let settings = Array.fold_right (fun setting r -> try (setting#index, setting#xml, setting#current_value)::r with _ -> r) variables [] in
+    SaveSettings.popup airframe_filename (Array.of_list settings) do_change
+end
 
diff --git a/sw/ground_segment/cockpit/saveSettings.ml b/sw/ground_segment/cockpit/saveSettings.ml
index 91138dc714..7c06daa0b1 100644
--- a/sw/ground_segment/cockpit/saveSettings.ml
+++ b/sw/ground_segment/cockpit/saveSettings.ml
@@ -43,21 +43,12 @@ let floats_not_equal = fun f1 f2 ->
   let r = abs_float (f1 /. f2) in
   r < 0.999 || r > 1.001
 
-(* Unit conversions *)
-let scale_of_units = fun u1 u2  ->
-  match u1, u2 with
-    "deg", "rad" -> 180. /. Latlong.pi
-  | "rad", "deg" -> Latlong.pi /. 180.
-  | u1, u2 when u1 = u2 -> 1.
-  | _ -> invalid_arg (Printf.sprintf "SaveSettings.scale_of_units %s %s" u1 u2)
-
-
 
 (** The save file dialog box *)
 let save_airframe = fun w filename save ->
   match GToolbox.select_file ~title:"Save Airframe" ~filename () with
-    None -> ()
-  | Some file ->
+      None -> ()
+    | Some file ->
       save file;
       w#save_settings#destroy ()
 
@@ -77,7 +68,7 @@ let display_columns = fun w model ->
     let renderer = GTree.cell_renderer_text [`XALIGN 0.] in
     let vc = GTree.view_column ~title ~renderer:(renderer, ["text", col]) () in
     vc#set_clickable true;
-  ignore (w#treeview_settings#append_column vc))
+    ignore (w#treeview_settings#append_column vc))
     text_columns;
   let renderer = GTree.cell_renderer_toggle [`XALIGN 0.] in
   let vc = GTree.view_column ~renderer:(renderer, ["active", col_to_save]) () in
@@ -117,13 +108,13 @@ let write_xml = fun (model:GTree.tree_store) old_file airframe_xml file ->
 
 
 let send_airframe_values = fun (model:GTree.tree_store) send_value ->
-   model#foreach (fun _path row ->
-     if model#get ~row ~column:col_to_save then begin
-       let index = model#get ~row ~column:col_index
-       and airframe_value = model#get ~row ~column:col_airframe_value in
-       send_value index airframe_value
-     end;
-     false)
+  model#foreach (fun _path row ->
+    if model#get ~row ~column:col_to_save then begin
+      let index = model#get ~row ~column:col_index
+      and airframe_value = model#get ~row ~column:col_airframe_value in
+      send_value index airframe_value
+    end;
+    false)
 
 
 
@@ -135,18 +126,28 @@ let fill_data = fun (model:GTree.tree_store) settings airframe_xml ->
       let param = attrib "param" in
       let (airframe_value, unit) = EditAirframe.get airframe_xml param in
       let scale =
-	try
-	  let unit_setting = attrib "unit"
-	  and unit_airframe =
-	    match unit with Some u -> u | None -> raise Exit in
-	  scale_of_units unit_setting unit_airframe
-	with
-	  _ -> 1. in
+        try
+          let unit_setting = attrib "unit"
+          and unit_airframe =
+            match unit with Some u -> u | None -> raise Exit in
+          Pprz.scale_of_units unit_setting unit_airframe
+        with
+            _ -> 1. in
+      let val_list = Str.split (Str.regexp "[ ()]+") airframe_value in
+      let (scale_macros, str_val) = List.partition (fun x -> Str.string_match (Str.regexp "RadOfDeg\\|DegOfRad") x 0) val_list in
+      let extra_scale =
+        try
+          match (List.hd scale_macros) with
+              "RadOfDeg" -> Latlong.pi /. 180.
+            | "DegOfRad" -> 180. /. Latlong.pi
+            | _ -> 1.
+        with
+            _ -> 1. in
       let scaled_value =
-	try
-	  float_of_string airframe_value *. scale
-	with
-	  Failure "float_of_string" -> raise (EditAirframe.No_param param)
+        try
+          float_of_string (List.hd str_val) *. scale *. extra_scale
+        with
+            Failure "float_of_string" -> raise (EditAirframe.No_param param)
       in
 
       let row = model#append () in
@@ -157,9 +158,9 @@ let fill_data = fun (model:GTree.tree_store) settings airframe_xml ->
       model#set ~row ~column:col_settings_scaled_value (value /. scale);
       model#set ~row ~column:col_to_save (floats_not_equal scaled_value value)
     with
-      Xml.No_attribute _ -> ()
-    | EditAirframe.No_param param ->
-	not_in_airframe_file := param :: !not_in_airframe_file ) (* Not savable *)
+        Xml.No_attribute _ -> ()
+      | EditAirframe.No_param param ->
+        not_in_airframe_file := param :: !not_in_airframe_file ) (* Not savable *)
     settings;
 
   (* Warning if needed *)
diff --git a/sw/ground_segment/lpc21iap/Makefile b/sw/ground_segment/lpc21iap/Makefile
index e3fab50201..dc4a954a37 100644
--- a/sw/ground_segment/lpc21iap/Makefile
+++ b/sw/ground_segment/lpc21iap/Makefile
@@ -27,8 +27,8 @@ endif
 
 UNAME = $(shell uname -s)
 ifeq ("$(UNAME)","Darwin")
-  INCLUDES=-I/opt/local/include/
-  LIBRARYS=-L/opt/local/lib	
+  LIBRARYS = -L /opt/paparazzi/lib -L /opt/local/lib
+  INCLUDES = -I /opt/paparazzi/include -I /opt/local/include/
 endif
 
 
diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
index d7cf0118fb..7870fe7e62 100644
--- a/sw/ground_segment/misc/Makefile
+++ b/sw/ground_segment/misc/Makefile
@@ -1,7 +1,8 @@
 UNAME = $(shell uname -s)
 
 ifeq ("$(UNAME)","Darwin")
-  LIBRARYS = -L/opt/local/lib
+  LIBRARYS = -L /opt/paparazzi/lib -L /opt/local/lib
+  INCLUDES = -I /opt/paparazzi/include -I /opt/local/include/
 else
   LIBRARYS = -s
 endif
@@ -19,4 +20,4 @@ kestrel2ivy: kestrel2ivy.o
 	g++ -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) -livy
 
 %.o : %.c
-	gcc -c -O2 -Wall -I /opt/local/include/  $<
+	gcc -c -O2 -Wall $(INCLUDES) $<
diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml
index 920afe7423..0661e986aa 100644
--- a/sw/ground_segment/tmtc/messages.ml
+++ b/sw/ground_segment/tmtc/messages.ml
@@ -28,7 +28,7 @@ open Printf
 
 let list_sort = fun f l -> List.sort (fun x y -> compare (f x) (f y)) l
 
-let display_delay = 500 (* Time in second between two updates *)
+let display_delay = 500 (* Time in milliseconds between two updates *)
 let led_delay = 500 (* Time in milliseconds while the green led is displayed *)
 
 let dnd_targets = [ { Gtk.target = "STRING"; flags = []; info = 0} ]
@@ -38,7 +38,7 @@ let values_of_field = fun field ->
   try
     Array.of_list (Str.split pipe_regexp (Xml.attrib field "values"))
   with
-    _ -> [||]
+      _ -> [||]
 
 (** Display one page for a message *)
 let one_page = fun sender class_name (notebook:GPack.notebook) bind m ->
@@ -54,54 +54,54 @@ let one_page = fun sender class_name (notebook:GPack.notebook) bind m ->
   let fields =
     List.fold_left
       (fun rest f ->
-	try
-	  let unit = try "("^Xml.attrib f "unit"^")" with _ -> "" in
-	  let field_name =  Xml.attrib f "name" in
-	  let type_ = ExtXml.attrib f "type" in
-	  let name = Printf.sprintf "%s %s %s: " type_ field_name unit in
-	  let h = GPack.hbox ~packing:v#pack () in
-	  let field_label = GButton.button ~label:name ~packing:h#pack () in
+        try
+          let unit = try "("^Xml.attrib f "unit"^")" with _ -> "" in
+          let field_name =  Xml.attrib f "name" in
+          let type_ = ExtXml.attrib f "type" in
+          let name = Printf.sprintf "%s %s %s: " type_ field_name unit in
+          let h = GPack.hbox ~packing:v#pack () in
+          let field_label = GButton.button ~label:name ~packing:h#pack () in
 
-	  let value = ref "XXXX" in
-	  let l = GMisc.label ~text: !value ~packing:h#pack () in
-	  let literal_values = values_of_field f in
-	  let alt_value =
-	    try
-	      let coeff = float_of_string (Pprz.alt_unit_coef_of_xml f)
-	      and unit = Xml.attrib f "alt_unit" in
-	      fun value -> sprintf "%s (%f%s)" value (coeff*.float_of_string value) unit
-	    with
-	      _ -> fun value -> value in
-	  let update = fun (_a, x) ->
-	    value :=
-	      try
-		let i = Pprz.int_of_value x in
-		sprintf "%s (%d)" literal_values.(i) i
-	      with _ ->
-		alt_value (Pprz.string_of_value x)
-	  and display_value = fun () ->
-	    if notebook#page_num v#coerce = notebook#current_page then
-	      if l#label <> !value then l#set_text !value in
+          let value = ref "XXXX" in
+          let l = GMisc.label ~text: !value ~packing:h#pack () in
+          let literal_values = values_of_field f in
+          let alt_value =
+            try
+              let coeff = float_of_string (Pprz.alt_unit_coef_of_xml f)
+              and unit = Xml.attrib f "alt_unit" in
+              fun value -> sprintf "%s (%f%s)" value (coeff*.float_of_string value) unit
+            with
+                _ -> fun value -> value in
+          let update = fun (_a, x) ->
+            value :=
+              try
+                let i = Pprz.int_of_value x in
+                sprintf "%s (%d)" literal_values.(i) i
+              with _ ->
+                alt_value (Pprz.string_of_value x)
+          and display_value = fun () ->
+            if notebook#page_num v#coerce = notebook#current_page then
+              if l#label <> !value then l#set_text !value in
 
-	  (* box dragger *)
-	  field_label#drag#source_set dnd_targets ~modi:[`BUTTON1] ~actions:[`COPY];
-	  let data_get = fun _ (sel:GObj.selection_context) ~info ~time ->
-      let scale = Pprz.alt_unit_coef_of_xml f in
-	    let field_descr =
-	      if Pprz.is_array_type type_ then
-		match GToolbox.input_string ~title:"Index of value to drag" ~text:"0" "Index in the array ?" with
-		  None -> field_name
-		| Some i -> sprintf "%s[%s]" field_name i
-	      else
-		field_name in
-	    sel#return (sprintf "%s:%s:%s:%s:%s" sender class_name id field_descr scale) in
-	  ignore (field_label#drag#connect#data_get ~callback:data_get);
+          (* box dragger *)
+          field_label#drag#source_set dnd_targets ~modi:[`BUTTON1] ~actions:[`COPY];
+          let data_get = fun _ (sel:GObj.selection_context) ~info ~time ->
+            let scale = Pprz.alt_unit_coef_of_xml f in
+            let field_descr =
+              if Pprz.is_array_type type_ then
+                match GToolbox.input_string ~title:"Index of value to drag" ~text:"0" "Index in the array ?" with
+                    None -> field_name
+                  | Some i -> sprintf "%s[%s]" field_name i
+              else
+                field_name in
+            sel#return (sprintf "%s:%s:%s:%s:%s" sender class_name id field_descr scale) in
+          ignore (field_label#drag#connect#data_get ~callback:data_get);
 
-	  (update, display_value)::rest
-	with
-	  _ ->
-	    fprintf stderr "Warning: Ignoring '%s'\n%!" (Xml.to_string f);
-	    rest
+          (update, display_value)::rest
+        with
+            _ ->
+              fprintf stderr "Warning: Ignoring '%s'\n%!" (Xml.to_string f);
+              rest
       )
       []
       (Xml.children m)
@@ -128,17 +128,17 @@ let one_page = fun sender class_name (notebook:GPack.notebook) bind m ->
       shown := true;
       (** Look for the right position in alphabetic order *)
       let rec loop = fun i ->
-	let p = notebook#get_nth_page i in
-	let t = notebook#get_tab_label p in
-	match t#misc#get_property "name" with
-	| `STRING (Some x) -> if x < id then loop (i+1) else i
-	| _ -> raise Not_found
+        let p = notebook#get_nth_page i in
+        let t = notebook#get_tab_label p in
+        match t#misc#get_property "name" with
+          | `STRING (Some x) -> if x < id then loop (i+1) else i
+          | _ -> raise Not_found
       in
       try
-	let pos = loop 0 in
-	ignore (notebook#insert_page ~pos ~tab_label:h#coerce v#coerce)
+        let pos = loop 0 in
+        ignore (notebook#insert_page ~pos ~tab_label:h#coerce v#coerce)
       with _ ->
-	ignore (notebook#append_page ~tab_label:h#coerce v#coerce)
+        ignore (notebook#append_page ~tab_label:h#coerce v#coerce)
     end;
     time_since_last := 0;
     try
@@ -147,9 +147,9 @@ let one_page = fun sender class_name (notebook:GPack.notebook) bind m ->
       eb#coerce#misc#set_state `SELECTED;
       ignore (GMain.Timeout.add led_delay (fun () -> eb#coerce#misc#set_state `NORMAL; false))
     with
-      Invalid_argument "List.iter2" ->
-	Printf.fprintf stderr "%s: expected %d args, got %d\n" id n (List.length values); flush stderr
-    | exc -> prerr_endline (Printexc.to_string exc)
+        Invalid_argument "List.iter2" ->
+          Printf.fprintf stderr "%s: expected %d args, got %d\n" id n (List.length values); flush stderr
+      | exc -> prerr_endline (Printexc.to_string exc)
   in
   bind id display
 
@@ -159,24 +159,24 @@ let rec one_class = fun (notebook:GPack.notebook) (ident, xml_class, sender) ->
   let module P = Pprz.Messages (struct let name = class_name end) in
   let senders = Hashtbl.create 5 in
   match sender with
-  | Some "*" ->
+    | Some "*" ->
       (* Waiting for a new sender in this class *)
       let get_one = fun sender _vs ->
-	if not (Hashtbl.mem senders sender) then begin
-	  Hashtbl.add senders sender ();
-	  one_class notebook (ident,  xml_class, Some sender)
-	end in
+	    if not (Hashtbl.mem senders sender) then begin
+	      Hashtbl.add senders sender ();
+	      one_class notebook (ident,  xml_class, Some sender)
+	    end in
       List.iter
-	(fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one))
-	messages
-  | _ ->
+	    (fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one))
+	    messages
+    | _ ->
       let class_notebook = GPack.notebook ~tab_border:0 ~tab_pos:`LEFT () in
       let l = match sender with None -> "" | Some s -> ":"^s in
       let label = GMisc.label ~text:(ident^l) () in
       ignore (notebook#append_page ~tab_label:label#coerce class_notebook#coerce);
       let bind, sender_name = match sender with
-	None -> (fun m cb -> (P.message_bind m cb)), "*"
-      | Some sender -> (fun m cb -> (P.message_bind ~sender m cb)), sender in
+	      None -> (fun m cb -> (P.message_bind m cb)), "*"
+        | Some sender -> (fun m cb -> (P.message_bind ~sender m cb)), sender in
 
       (** Forall messages in the class *)
       let messages = list_sort (fun x -> Xml.attrib x "name") messages in
@@ -212,14 +212,14 @@ let _ =
     let xml = Pprz.messages_xml () in
     let class_of = fun n ->
       try
-	List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml)
+	    List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml)
       with Not_found -> failwith (sprintf "Unknown messages class: %s" n) in
 
     List.map (fun x ->
       match Str.split (Str.regexp ":") x with
-	[cl; s] -> (cl, class_of cl, Some s)
-      | [cl] -> (x, class_of cl, None)
-      | _ -> failwith (sprintf "Wrong class '%s', class[:sender] expected" x))
+	      [cl; s] -> (cl, class_of cl, Some s)
+        | [cl] -> (x, class_of cl, None)
+        | _ -> failwith (sprintf "Wrong class '%s', class[:sender] expected" x))
       !classes in
 
   (* Insert the message classes in the notebook *)
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index a360146c6c..6ead1895f7 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -282,7 +282,7 @@ let send_telemetry_status = fun a ->
   try
     let vs =
       ["ac_id", Pprz.String id;
-       "time_since_last_bat_msg", Pprz.Float (U.gettimeofday () -. a.last_msg_date)] in
+       "time_since_last_msg", Pprz.Float (U.gettimeofday () -. a.last_msg_date)] in
     Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs
   with
     _exc -> ()
diff --git a/sw/ground_segment/tmtc/settings.ml b/sw/ground_segment/tmtc/settings.ml
index 5d6d464dfc..0244c11d15 100644
--- a/sw/ground_segment/tmtc/settings.ml
+++ b/sw/ground_segment/tmtc/settings.ml
@@ -49,10 +49,10 @@ let one_ac = fun (notebook:GPack.notebook) ac_name ->
 
       let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx] in
       if classify_float value = FP_normal || classify_float value =FP_zero then
-	let vs' = ("value", Pprz.Float value) :: vs in
-	Ground_Pprz.message_send "dl" "DL_SETTING" vs'
+        let vs' = ("value", Pprz.Float value) :: vs in
+        Ground_Pprz.message_send "dl" "DL_SETTING" vs'
       else
-	Ground_Pprz.message_send "dl" "GET_DL_SETTING" vs in
+        Ground_Pprz.message_send "dl" "GET_DL_SETTING" vs in
 
     (* Build the buttons and sliders *)
     let xml = Xml.parse_file xml_file in
diff --git a/sw/in_progress/log_parser/09_05_13__14_38_13.log b/sw/in_progress/log_parser/09_05_13__14_38_13.log
index f26a37f876..0769ed08fd 100644
--- a/sw/in_progress/log_parser/09_05_13__14_38_13.log
+++ b/sw/in_progress/log_parser/09_05_13__14_38_13.log
@@ -3294,7 +3294,7 @@ ap.srcs += $(SRC_ARCH)/lpcusb/examples/msc_bot.c $(SRC_ARCH)/lpcusb/examples/msc
       </message>
       <message NAME="TELEMETRY_STATUS" ID="32">
         <field NAME="ac_id" TYPE="string"/>
-        <field NAME="time_since_last_bat_msg" TYPE="float" UNIT="s"/>
+        <field NAME="time_since_last_msg" TYPE="float" UNIT="s"/>
       </message>
       <message NAME="TELEMETRY_ERROR" ID="33">
         <field NAME="ac_id" TYPE="string"/>
@@ -3331,4 +3331,4 @@ ap.srcs += $(SRC_ARCH)/lpcusb/examples/msc_bot.c $(SRC_ARCH)/lpcusb/examples/msc
       </message>
     </class>
   </protocol>
-</configuration>
\ No newline at end of file
+</configuration>
diff --git a/sw/in_progress/python/attitude_viz.py b/sw/in_progress/python/attitude_viz.py
index 72458cb197..537172b75f 100755
--- a/sw/in_progress/python/attitude_viz.py
+++ b/sw/in_progress/python/attitude_viz.py
@@ -312,7 +312,7 @@ def init():
 def run():
   global VEHICLE_QUATS, BAR_VALUES
   VEHICLE_QUATS = [ ["AHRS_REF_QUAT", 6, "Estimate", True], ["AHRS_REF_QUAT", 2, "Reference", True]]
-  BAR_VALUES = [ ["BOOZ2_RADIO_CONTROL", 5, "Throttle (%%) %i", 9600, 96 * 2, 100] ]
+  BAR_VALUES = [ ["ROTORCRAFT_RADIO_CONTROL", 5, "Throttle (%%) %i", 9600, 96 * 2, 100] ]
   window_title = "Attitude_Viz"
   try:
     opts, args = getopt.getopt(sys.argv[1:], "t:", ["title"])
diff --git a/sw/lib/ocaml/ivy/Makefile b/sw/lib/ocaml/ivy/Makefile
deleted file mode 100644
index 24613416c7..0000000000
--- a/sw/lib/ocaml/ivy/Makefile
+++ /dev/null
@@ -1,127 +0,0 @@
-# $Id: Makefile,v 1.20 2007/09/04 19:05:43 brisset Exp $
-
-DESTDIR = /
-
-DEBUG  = n
-
-
-OCAMLC = ocamlc
-OCAMLMLI = ocamlc
-OCAMLOPT = ocamlopt -unsafe
-OCAMLDEP=ocamldep
-
-ifeq ($(DEBUG),y)
-OCAMLFLAGS = -g
-else
-OCAMLFLAGS =
-endif
-
-OCAMLOPTFLAGS=
-CFLAGS+=-Wall
-OCAMLINC=-I `ocamlc -where`
-GLIBINC=`pkg-config --cflags glib-2.0`
-
-LBITS := $(shell getconf LONG_BIT)
-ifeq ($(LBITS),64)
-  FPIC=-fPIC
-endif
-
-
-
-IVY = ivy.ml ivyLoop.ml
-
-IVYCMO= $(IVY:.ml=.cmo)
-IVYCMI= $(IVY:.ml=.cmi)
-IVYMLI= $(IVY:.ml=.mli)
-IVYCMX= $(IVY:.ml=.cmx)
-
-GLIBIVY = ivy.ml glibIvy.ml
-
-GLIBIVYCMO= $(GLIBIVY:.ml=.cmo)
-GLIBIVYCMI= $(GLIBIVY:.ml=.cmi)
-GLIBIVYCMX= $(GLIBIVY:.ml=.cmx)
-
-TKIVY = ivy.ml tkIvy.ml
-
-TKIVYCMO= $(TKIVY:.ml=.cmo)
-TKIVYCMI= $(TKIVY:.ml=.cmi)
-TKIVYCMX= $(TKIVY:.ml=.cmx)
-
-UNAME = $(shell uname -s)
-
-ifeq ("$(UNAME)","Darwin")
-  LIBRARYS = -L/opt/local/lib
-endif
-
-LIBS = ivy-ocaml.cma ivy-ocaml.cmxa glibivy-ocaml.cma glibivy-ocaml.cmxa
-
-all : $(LIBS)
-
-DISTRO=`ocamlc -version`
-
-deb :
-	cp debian/changelog.$(DISTRO) debian/changelog
-	dpkg-buildpackage -rfakeroot
-
-ivy : ivy-ocaml.cma ivy-ocaml.cmxa
-glibivy : glibivy-ocaml.cma glibivy-ocaml.cmxa
-tkivy : tkivy-ocaml.cma tkivy-ocaml.cmxa
-
-INST_FILES = $(IVYCMI) $(IVYMLI) glibIvy.cmi $(LIBS) libivy-ocaml.a libglibivy-ocaml.a dllivy-ocaml.so dllglibivy-ocaml.so ivy-ocaml.a glibivy-ocaml.a
-# tkIvy.cmi  libtkivy-ocaml.a  dlltkivy-ocaml.so tkivy-ocaml.a
-
-install : $(LIBS)
-	mkdir -p $(DESTDIR)/`ocamlc -where`
-	cp $(INST_FILES) $(DESTDIR)/`ocamlc -where`
-
-desinstall :
-	cd `ocamlc -where`; rm -f $(INST_FILES)
-
-ivy-ocaml.cma : $(IVYCMO) civy.o civyloop.o
-	ocamlmklib -o ivy-ocaml $^ $(LIBRARYS)  -livy
-
-ivy-ocaml.cmxa : $(IVYCMX) civy.o civyloop.o
-	ocamlmklib -o ivy-ocaml $^ $(LIBRARYS)  -livy
-
-glibivy-ocaml.cma : $(GLIBIVYCMO) civy.o cglibivy.o
-	ocamlmklib -o glibivy-ocaml $^ $(LIBRARYS) -lglibivy  `pkg-config --libs glib-2.0` -lpcre
-
-glibivy-ocaml.cmxa : $(GLIBIVYCMX) civy.o cglibivy.o
-	ocamlmklib -o glibivy-ocaml $^ $(LIBRARYS) -lglibivy `pkg-config --libs glib-2.0` -lpcre
-
-tkivy-ocaml.cma : $(TKIVYCMO) civy.o ctkivy.o
-	ocamlmklib -o tkivy-ocaml $^ $(LIBRARYS) -livy -ltclivy
-
-tkivy-ocaml.cmxa : $(TKIVYCMX) civy.o ctkivy.o
-	ocamlmklib -o tkivy-ocaml $^ $(LIBRARYS) -livy -ltclivy
-
-.SUFFIXES:
-.SUFFIXES: .ml .mli .mly .mll .cmi .cmo .cmx .c .o .out .opt
-
-.ml.cmo :
-	$(OCAMLC) $(OCAMLFLAGS) $(INCLUDES) -c $<
-.c.o :
-
-	$(CC) -Wall -c $(FPIC) -I /opt/local/include/  $(OCAMLINC) $(GLIBINC) $<
-.mli.cmi :
-	$(OCAMLMLI) $(OCAMLFLAGS) -c $<
-.ml.cmx :
-	$(OCAMLOPT) $(OCAMLOPTFLAGS) -c $<
-.mly.ml :
-	ocamlyacc $<
-.mll.ml :
-	ocamllex $<
-.cmo.out :
-	$(OCAMLC) -custom -o $@ unix.cma -I . ivy-ocaml.cma $< -cclib -livy
-.cmx.opt :
-	$(OCAMLOPT) -o $@ unix.cmxa -I . ivy-ocaml.cmxa $< -cclib -livy
-
-clean:
-	\rm -fr *.cm* *.o *.a .depend *~ *.out *.opt .depend *.so *-stamp debian/ivy-ocaml debian/files debian/ivy-ocaml.debhelper.log debian/ivy-ocaml.substvars debian/*~
-
-.depend:
-	$(OCAMLDEP) $(INCLUDES) *.mli *.ml > .depend
-
-ifneq ($(MAKECMDGOALS),clean)
-include .depend
-endif
diff --git a/sw/lib/ocaml/ivy/README b/sw/lib/ocaml/ivy/README
deleted file mode 100644
index 5aca0e8661..0000000000
--- a/sw/lib/ocaml/ivy/README
+++ /dev/null
@@ -1,18 +0,0 @@
-OCaml (caml.org) bindings for the Ivy library (www.tls.cena.fr/products/ivy).
-
-Two libraries are provided:
- - ivy-ocaml, running with the Ivy main loop (module IvyLoop)
- - glibivy-ocaml, running with the Glib main loop (provided by lablgtk)
-
-Installation:
- - Requires the OCaml compiler and the ivy-c library
- - "make" to compile
- - "make install" to set the files (DESTDIR may be specified)
- - "examples" directory contains the ivyprobe program for both main loops
-("make ivyivyprobe.out" and "make glibivyprobe.out")
-
-Documentation:
- - The Ivy documentation (www.tls.cena.fr/products/ivy)
- - The .mli files (ivy.mli, ivyLoop.mli and glibIvy.mli)
-
-Maintainer: Pascal Brisset (pascal dot brisset at enac dot fr)
diff --git a/sw/lib/ocaml/ivy/cglibivy.c b/sw/lib/ocaml/ivy/cglibivy.c
deleted file mode 100644
index 8241b70fc2..0000000000
--- a/sw/lib/ocaml/ivy/cglibivy.c
+++ /dev/null
@@ -1,43 +0,0 @@
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <getopt.h>
-#include <Ivy/timer.h>
-#include <Ivy/ivychannel.h>
-#include <Ivy/ivyglibloop.h>
-#include <Ivy/version.h>
-#include <glib.h>
-#include <caml/mlvalues.h>
-#include <caml/fail.h>
-#include <caml/callback.h>
-#include <caml/memory.h>
-#include <caml/alloc.h>
-
-value ivy_GtkmainLoop(value unit)
-{
-  g_main_loop_run(g_main_loop_new(NULL, FALSE));
-  return Val_unit;
-}
-
-extern void cb_delete_channel(void *delete_read);
-extern void cb_read_channel(Channel ch, IVY_HANDLE fd, void *closure);
-extern void cb_write_channel(Channel ch, IVY_HANDLE fd, void *closure);
-
-value ivy_GtkchannelSetUp(value fd, value closure_name)
-{
-  Channel c;
-  value * closure = caml_named_value(String_val(closure_name));
-
-#if IVYMINOR_VERSION == 8
-  c = IvyChannelAdd((IVY_HANDLE)Int_val(fd), (void*)closure, cb_delete_channel, cb_read_channel);
-#else
-  c = IvyChannelAdd((IVY_HANDLE)Int_val(fd), (void*)closure, cb_delete_channel, cb_read_channel, cb_write_channel);
-#endif
-  return Val_int(c);
-}
-
-value ivy_GtkchannelClose(value ch)
-{
-  IvyChannelRemove((Channel)Long_val(ch));
-  return Val_unit;
-}
diff --git a/sw/lib/ocaml/ivy/civy.c b/sw/lib/ocaml/ivy/civy.c
deleted file mode 100644
index 563f7ec517..0000000000
--- a/sw/lib/ocaml/ivy/civy.c
+++ /dev/null
@@ -1,93 +0,0 @@
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <getopt.h>
-#include <Ivy/ivy.h>
-#include <Ivy/ivyloop.h>
-#include <Ivy/timer.h>
-#include <caml/mlvalues.h>
-#include <caml/fail.h>
-#include <caml/callback.h>
-#include <caml/memory.h>
-#include <caml/alloc.h>
-
-value ivy_sendMsg(value msg)
-{
-  IvySendMsg(String_val(msg));
-  return Val_unit;
-}
-
-value ivy_stop(value unit)
-{
-  IvyStop ();
-  return Val_unit;
-}
-
-
-void app_cb(IvyClientPtr app, void *user_data, IvyApplicationEvent event )
-{
-  value closure = *(value*)user_data;
-  callback2(closure, Val_int(app), Val_int(event));
-}
-
-value ivy_init(value vappName, value vready, value closure_name)
-{
-  value * closure = caml_named_value(String_val(closure_name));
-  char * appName = malloc(strlen(String_val(vappName))+1); /* Memory leak */
-  strcpy(appName, String_val(vappName));
-  char * ready = malloc(strlen(String_val(vready))+1); /* Memory leak */
-  strcpy(ready, String_val(vready));
-  IvyInit(appName, ready, app_cb, (void*)closure, 0, 0); /* When the "die callback" is called ??? */
-  return Val_unit;
-}
-
-value ivy_start(value bus)
-{
-  IvyStart(String_val(bus));
-  return Val_unit;
-}
-
-void ClosureCallback(IvyClientPtr app, void *closure, int argc, char **argv)
-{
-  char* t[argc+1];
-  int i;
-  /* Copie de argv dans t avec ajout d'un pointeur nul a la fin */
-  for(i=0; i < argc; i++) t[i] = argv[i];
-  t[argc] = (char*)0L;
-  callback2(*(value*)closure, Val_long(app), copy_string_array((char const **)t));
-}
-
-value ivy_bindMsg(value cb_name, value regexp)
-{
-  value * closure = caml_named_value(String_val(cb_name));
-  MsgRcvPtr id = IvyBindMsg(ClosureCallback, (void*)closure, String_val(regexp));
-  return Val_long(id);
-}
-
-value ivy_unbindMsg(value id)
-{
-  IvyUnbindMsg((MsgRcvPtr)Long_val(id));
-  return Val_unit;
-}
-
-value ivy_name_of_client(value c)
-{
-  return copy_string(IvyGetApplicationName((IvyClientPtr)Long_val(c)));
-}
-value ivy_host_of_client(value c)
-{
-  return copy_string(IvyGetApplicationHost((IvyClientPtr)Long_val(c)));
-}
-
-void cb_delete_channel(void *delete_read)
-{
-}
-
-void cb_write_channel(Channel ch, IVY_HANDLE fd, void *closure)
-{
-}
-
-void cb_read_channel(Channel ch, IVY_HANDLE fd, void *closure)
-{
-  callback(*(value*)closure, Val_int(ch));
-}
diff --git a/sw/lib/ocaml/ivy/civyloop.c b/sw/lib/ocaml/ivy/civyloop.c
deleted file mode 100644
index 083b104ee1..0000000000
--- a/sw/lib/ocaml/ivy/civyloop.c
+++ /dev/null
@@ -1,70 +0,0 @@
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <getopt.h>
-#include <Ivy/ivy.h>
-#include <Ivy/ivyloop.h>
-#include <Ivy/timer.h>
-#include <Ivy/version.h>
-#include <caml/mlvalues.h>
-#include <caml/fail.h>
-#include <caml/callback.h>
-#include <caml/memory.h>
-#include <caml/alloc.h>
-
-value ivy_mainLoop(value unit)
-{
-#if IVYMINOR_VERSION == 8
-  IvyMainLoop (NULL,NULL);
-#else
-  IvyMainLoop ();
-#endif
-  return Val_unit;
-}
-
-void timer_cb(TimerId id, void *data, unsigned long delta)
-{
-  value closure = *(value*)data;
-  callback(closure, Val_long(id));
-}
-
-value ivy_timerRepeatafter(value nb_ticks,value delay, value closure_name)
-{
-  value * closure = caml_named_value(String_val(closure_name));
-  TimerId id = TimerRepeatAfter(Int_val(nb_ticks), Int_val(delay), timer_cb, (void*)closure);
-  return Val_int(id);
-}
-
-/* Data associated to Channel callbacks is the couple of delete and
-read closures */
-
-extern void cb_delete_channel(void *delete_read);
-extern void cb_read_channel(Channel ch, IVY_HANDLE fd, void *closure);
-extern void cb_write_channel(Channel ch, IVY_HANDLE fd, void *closure);
-
-
-value ivy_channelSetUp(value fd, value closure_name)
-{
-  Channel c;
-  value * closure = caml_named_value(String_val(closure_name));
-
-#if IVYMINOR_VERSION == 8
-  c = IvyChannelAdd((IVY_HANDLE)Int_val(fd), (void*)closure, cb_delete_channel, cb_read_channel);
-#else
-  c = IvyChannelAdd((IVY_HANDLE)Int_val(fd), (void*)closure, cb_delete_channel, cb_read_channel, cb_write_channel);
-#endif
-  return Val_int(c);
-}
-
-value ivy_timerRemove(value t)
-{
-  TimerRemove((TimerId)Long_val(t));
-  return Val_unit;
-}
-
-
-value ivy_channelClose(value ch)
-{
-  IvyChannelRemove((Channel)Long_val(ch));
-  return Val_unit;
-}
diff --git a/sw/lib/ocaml/ivy/ctkivy.c b/sw/lib/ocaml/ivy/ctkivy.c
deleted file mode 100644
index 87b7eb617d..0000000000
--- a/sw/lib/ocaml/ivy/ctkivy.c
+++ /dev/null
@@ -1,36 +0,0 @@
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <getopt.h>
-#include <timer.h>
-#include <caml/mlvalues.h>
-#include <caml/fail.h>
-#include <caml/callback.h>
-#include <caml/memory.h>
-#include <caml/alloc.h>
-#include "ivytcl.h"
-
-extern void cb_delete_channel(void *delete_read);
-extern void cb_read_channel(Channel ch, IVY_HANDLE fd, void *closure);
-
-value ivy_TclmainLoop(value unit)
-{
-  Tk_MainLoop();
-  return Val_unit;
-}
-
-
-value ivy_TclchannelSetUp(value fd, value closure_name)
-{
-  Channel c;
-  value * closure = caml_named_value(String_val(closure_name));
-
-  c = IvyTclChannelSetUp((IVY_HANDLE)Int_val(fd), (void*)closure, cb_delete_channel, cb_read_channel);
-  return Val_int(c);
-}
-
-value ivy_TclchannelClose(value ch)
-{
-  IvyTclChannelClose((Channel)Int_val(ch));
-  return Val_unit;
-}
diff --git a/sw/lib/ocaml/ivy/debian/changelog.3.10.2 b/sw/lib/ocaml/ivy/debian/changelog.3.10.2
deleted file mode 100644
index 89afc104ff..0000000000
--- a/sw/lib/ocaml/ivy/debian/changelog.3.10.2
+++ /dev/null
@@ -1,38 +0,0 @@
-ivy-ocaml (1.1-7) unstable; urgency=low
-
-  * Updated for ocaml 3.10.2
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Fri, 23 May 2008 23:18:00 +0200
-
-ivy-ocaml (1.1-6) unstable; urgency=low
-
-  * Updated for ocaml 3.10.1
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Mon, 25 Feb 2008 10:31:49 +0100
-
-ivy-ocaml (1.1-5) unstable; urgency=low
-
-  * Updated for ocaml 3.10
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Tue, 04 Sep 2007 20:41:49 +0200
-
-ivy-ocaml (1.1-4) unstable; urgency=low
-
-  * Updated for ocaml 3.09.2-6
-
- -- Antoine Drouin (Poine) <poine@recherche.enac.fr>  Fri,  04 Aug 2006 13:40:54 +0200
-
-ivy-ocaml (1.1-3) unstable; urgency=low
-
-  * Updated for ivy 3.8
-
- -- Antoine Drouin (Poine) <poine@recherche.enac.fr>  Fri,  28 Jul 2006 13:40:54 +0200
-
-ivy-ocaml (1.0-2) unstable; urgency=low
-
-  * Updated for ocaml 3.09
-
-  * Initial Release.
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Thu,  7 Oct 2004 13:40:54 +0200
-
diff --git a/sw/lib/ocaml/ivy/debian/changelog.3.11.2 b/sw/lib/ocaml/ivy/debian/changelog.3.11.2
deleted file mode 100644
index e365e33921..0000000000
--- a/sw/lib/ocaml/ivy/debian/changelog.3.11.2
+++ /dev/null
@@ -1,56 +0,0 @@
-ivy-ocaml (1.1-12) unstable; urgency=low
-
-  * Support for ivy-c_3.11.8
-
- -- Gautier Hattenberger <gautier.hattenberger@enac.fr>  Wed, 02 Feb 2011 17:49:31 +0100
-
-ivy-ocaml (1.1-11) unstable; urgency=low
-
-  * Support of ivy-c_3.11.6, OSX and Linux 64bit
-
- -- Gautier Hattenberger <gautier.hattenberger@enac.fr>  Wed, 1 Dec 2010 10:43:29 +0100
-
-ivy-ocaml (1.1-10) unstable; urgency=low
-
-  * Updated for ocaml 3.11.2
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Thu, 25 Feb 2010 09:58:29 +0100
-
-ivy-ocaml (1.1-7) unstable; urgency=low
-
-  * Updated for ocaml 3.10.2
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Fri, 23 May 2008 23:18:00 +0200
-
-ivy-ocaml (1.1-6) unstable; urgency=low
-
-  * Updated for ocaml 3.10.1
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Mon, 25 Feb 2008 10:31:49 +0100
-
-ivy-ocaml (1.1-5) unstable; urgency=low
-
-  * Updated for ocaml 3.10
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Tue, 04 Sep 2007 20:41:49 +0200
-
-ivy-ocaml (1.1-4) unstable; urgency=low
-
-  * Updated for ocaml 3.09.2-6
-
- -- Antoine Drouin (Poine) <poine@recherche.enac.fr>  Fri,  04 Aug 2006 13:40:54 +0200
-
-ivy-ocaml (1.1-3) unstable; urgency=low
-
-  * Updated for ivy 3.8
-
- -- Antoine Drouin (Poine) <poine@recherche.enac.fr>  Fri,  28 Jul 2006 13:40:54 +0200
-
-ivy-ocaml (1.0-2) unstable; urgency=low
-
-  * Updated for ocaml 3.09
-
-  * Initial Release.
-
- -- Pascal Brisset (Hecto) <pascal.brisset@enac.fr>  Thu,  7 Oct 2004 13:40:54 +0200
-
diff --git a/sw/lib/ocaml/ivy/debian/compat b/sw/lib/ocaml/ivy/debian/compat
deleted file mode 100644
index b8626c4cff..0000000000
--- a/sw/lib/ocaml/ivy/debian/compat
+++ /dev/null
@@ -1 +0,0 @@
-4
diff --git a/sw/lib/ocaml/ivy/debian/control b/sw/lib/ocaml/ivy/debian/control
deleted file mode 100644
index 7707a3836d..0000000000
--- a/sw/lib/ocaml/ivy/debian/control
+++ /dev/null
@@ -1,13 +0,0 @@
-Source: ivy-ocaml
-Section: net
-Priority: optional
-Maintainer: Gautier Hattenberger <gautier.hattenberger@enac.fr>
-Build-Depends: debhelper (>= 4.0.0), ocaml-nox, ivy-c-dev (>=3.8), libglib2.0-dev, libpcre3-dev, ivy-c(>=3.8)
-Standards-Version: 3.6.1
-
-Package: ivy-ocaml
-Architecture: any
-Depends: ${shlibs:Depends}, ${misc:Depends}, ocaml-nox-${F:OCamlABI}, ivy-c(>= 3.8)
-Description: Ocaml binding for the Ivy software bus
- This package provides the bindings for the Ivy software bus. Standalone
- linking and with the glib mainloop are provided.
diff --git a/sw/lib/ocaml/ivy/debian/rules b/sw/lib/ocaml/ivy/debian/rules
deleted file mode 100755
index a6550e0265..0000000000
--- a/sw/lib/ocaml/ivy/debian/rules
+++ /dev/null
@@ -1,99 +0,0 @@
-#!/usr/bin/make -f
-# -*- makefile -*-
-# Sample debian/rules that uses debhelper.
-# This file was originally written by Joey Hess and Craig Small.
-# As a special exception, when this file is copied by dh-make into a
-# dh-make output file, you may use that output file without restriction.
-# This special exception was added by Craig Small in version 0.37 of dh-make.
-
-# Uncomment this to turn on verbose mode.
-#export DH_VERBOSE=1
-
-
-OCAMLABI := $(shell ocamlc -version)
-
-
-CFLAGS = -Wall -g
-
-ifneq (,$(findstring noopt,$(DEB_BUILD_OPTIONS)))
-	CFLAGS += -O0
-else
-	CFLAGS += -O2
-endif
-
-configure: configure-stamp
-configure-stamp:
-	dh_testdir
-	# Add here commands to configure the package.
-
-	touch configure-stamp
-
-
-build: build-stamp
-
-build-stamp: configure-stamp 
-	dh_testdir
-
-	# Add here commands to compile the package.
-	$(MAKE)
-	#docbook-to-man debian/ivy-ocaml.sgml > ivy-ocaml.1
-
-	touch build-stamp
-
-clean:
-	dh_testdir
-	dh_testroot
-	rm -f build-stamp configure-stamp
-
-	# Add here commands to clean up after the build process.
-	-$(MAKE) clean
-
-	dh_clean 
-
-install: build
-	dh_testdir
-	dh_testroot
-	dh_clean -k 
-	dh_installdirs
-
-	# Add here commands to install the package into debian/ivy-ocaml.
-	$(MAKE) install DESTDIR=$(CURDIR)/debian/ivy-ocaml
-
-
-# Build architecture-independent files here.
-binary-indep: build install
-# We have nothing to do by default.
-
-# Build architecture-dependent files here.
-binary-arch: build install
-	dh_testdir
-	dh_testroot
-	dh_installchangelogs 
-	dh_installdocs examples
-	dh_installexamples
-#	dh_install
-#	dh_installmenu
-#	dh_installdebconf	
-#	dh_installlogrotate
-#	dh_installemacsen
-#	dh_installpam
-#	dh_installmime
-#	dh_installinit
-#	dh_installcron
-#	dh_installinfo
-	dh_installman
-	dh_link
-	dh_strip
-	dh_compress
-	dh_fixperms
-#	dh_perl
-#	dh_python
-#	dh_makeshlibs
-	dh_installdeb
-	dh_shlibdeps
-	dh_gencontrol -- -VF:OCamlABI="$(OCAMLABI)"
-	dh_md5sums
-	dh_builddeb
-
-binary: binary-indep binary-arch
-.PHONY: build clean binary-indep binary-arch binary install configure
diff --git a/sw/lib/ocaml/ivy/debian/shlibs.local b/sw/lib/ocaml/ivy/debian/shlibs.local
deleted file mode 100644
index 85b0b9942a..0000000000
--- a/sw/lib/ocaml/ivy/debian/shlibs.local
+++ /dev/null
@@ -1 +0,0 @@
-libivy 3 ivy-c
diff --git a/sw/lib/ocaml/ivy/examples/Makefile b/sw/lib/ocaml/ivy/examples/Makefile
deleted file mode 100644
index 53fa3ae7c4..0000000000
--- a/sw/lib/ocaml/ivy/examples/Makefile
+++ /dev/null
@@ -1,31 +0,0 @@
-# $Id: Makefile,v 1.4 2006/07/28 16:38:53 poine Exp $
-
-OCAMLC = ocamlc -I .. 
-OCAMLMLI = ocamlc -I .. 
-OCAMLOPT = ocamlopt
-OCAMLDEP=ocamldep
-
-all: ivyprobe.out glibivyprobe.out
-
-ivyprobe.out : ivyprobe.cmo ivyivyprobe.cmo
-	$(OCAMLC) -custom -o $@ unix.cma ivy-ocaml.cma $^
-
-glibivyprobe.out : ivyprobe.cmo glibivyprobe.cmo
-	$(OCAMLC) -custom -o $@ unix.cma glibivy-ocaml.cma $^
-
-#tkivyprobe.out : ivyprobe.cmo tkivyprobe.cmo
-#	$(OCAMLC) -custom -o $@ unix.cma -I +labltk labltk.cma -I . $^
-#tkivyprobe.cmo : OCAMLFLAGS=-I +labltk
-
-%.cmo : %.ml
-	$(OCAMLC) -c $<
-
-clean:
-	\rm -f *.cm* *.o *.a .depend *~ *.out *.opt .depend *.so
-
-.depend:
-	$(OCAMLDEP) $(INCLUDES) *.mli *.ml > .depend
-
-ifneq ($(MAKECMDGOALS),clean)
-include .depend
-endif
diff --git a/sw/lib/ocaml/ivy/examples/glibivyprobe.ml b/sw/lib/ocaml/ivy/examples/glibivyprobe.ml
deleted file mode 100644
index 0d4251e46b..0000000000
--- a/sw/lib/ocaml/ivy/examples/glibivyprobe.ml
+++ /dev/null
@@ -1,9 +0,0 @@
-(* $Id: glibivyprobe.ml,v 1.1 2004/10/18 10:55:37 brisset Exp $ *)
-
-let _ =
-  Ivyprobe.init ();
-  try
-    ignore (GlibIvy.set_up_channel Unix.stdin Ivy.stop (fun _ -> Ivyprobe.read stdin));
-    GlibIvy.main ()
-  with
-    End_of_file -> Ivy.stop ()
diff --git a/sw/lib/ocaml/ivy/examples/ivyivyprobe.ml b/sw/lib/ocaml/ivy/examples/ivyivyprobe.ml
deleted file mode 100644
index b51a70875d..0000000000
--- a/sw/lib/ocaml/ivy/examples/ivyivyprobe.ml
+++ /dev/null
@@ -1,8 +0,0 @@
-let _ =
-  Ivyprobe.init ();
-  try
-    ignore (IvyLoop.set_up_channel Unix.stdin Ivy.stop (fun _ -> Ivyprobe.read stdin));
-    IvyLoop.main ()
-  with
-    End_of_file -> Ivy.stop ()
-
diff --git a/sw/lib/ocaml/ivy/examples/ivyprobe.ml b/sw/lib/ocaml/ivy/examples/ivyprobe.ml
deleted file mode 100644
index d8fe4cea4f..0000000000
--- a/sw/lib/ocaml/ivy/examples/ivyprobe.ml
+++ /dev/null
@@ -1,40 +0,0 @@
-(* $Id: ivyprobe.ml,v 1.1 2004/10/18 10:55:37 brisset Exp $ *)
-
-let print_message app message =
-  Printf.printf "%s sent" (Ivy.name_of_client app);
-  Array.iter (fun s -> Printf.printf " '%s'" s) message;
-  print_newline ()
-
-let read = fun channel ->
-  let l = input_line channel in
-  Ivy.send l
-
-let watch_clients c e =
-  let dis = match e with Ivy.Connected -> "" | Ivy.Disconnected -> "dis" in
-  Printf.printf "%s %sconnected from %s\n"
-    (Ivy.name_of_client c)
-    dis
-    (Ivy.host_of_client c);
-  flush stdout
-
-let init = fun () ->
-  let regexp = ref ""
-  and name = ref "MLIVYPROBE"
-  and port = ref 2010
-  and domain = ref "127.255.255.255" in
-  Arg.parse
-    [ "-b", Arg.Int (fun x -> port := x), "<Port number>\tDefault is 2010, unused if IVYBUS is set";
-      "-domain", Arg.String (fun x -> domain := x), "<Network address>\tDefault is 127.255.255.255, unused if IVYBUS is set";
-      "-n", Arg.String (fun s -> name := s), "<Name of the prober>\tDefault is MLIVYPROBE"]
-    (fun s -> regexp := s)
-    "Usage: ";
-
-  let bus =
-    try Sys.getenv "IVYBUS" with
-      Not_found -> Printf.sprintf "%s:%d" !domain !port in
-  Ivy.init !name "READY" watch_clients;
-  Ivy.start bus;
-
-  Printf.printf "\nEnd of file to stop\n\n"; flush stdout;
-
-  ignore (Ivy.bind print_message !regexp)
diff --git a/sw/lib/ocaml/ivy/examples/tkivyprobe.ml b/sw/lib/ocaml/ivy/examples/tkivyprobe.ml
deleted file mode 100644
index 67ee3aa275..0000000000
--- a/sw/lib/ocaml/ivy/examples/tkivyprobe.ml
+++ /dev/null
@@ -1,10 +0,0 @@
-let _ =
-  Ivyprobe.init ();
-  let top = Tk.openTk () in
-  try
-    ignore (TkIvy.set_up_channel Unix.stdin Ivy.stop (fun _ -> Ivyprobe.read stdin));
-    TkIvy.main ()
-  with
-    End_of_file -> Ivy.stop ()
-
-
diff --git a/sw/lib/ocaml/ivy/glibIvy.ml b/sw/lib/ocaml/ivy/glibIvy.ml
deleted file mode 100644
index 42b0991af8..0000000000
--- a/sw/lib/ocaml/ivy/glibIvy.ml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-type channel
-external main : unit -> unit = "ivy_GtkmainLoop"
-external ext_channelSetUp : Unix.file_descr -> string -> channel = "ivy_GtkchannelSetUp"
-let set_up_channel fd delete read =
-  ext_channelSetUp fd (Ivy.cb_register read)
-external close_channel : channel -> unit = "ivy_GtkchannelClose"
-
diff --git a/sw/lib/ocaml/ivy/glibIvy.mli b/sw/lib/ocaml/ivy/glibIvy.mli
deleted file mode 100644
index aec0a80445..0000000000
--- a/sw/lib/ocaml/ivy/glibIvy.mli
+++ /dev/null
@@ -1,13 +0,0 @@
-val main : unit -> unit
-(** Glib main loop *)
-
-type channel
-(** Channel handled by the main loop *)
-
-val set_up_channel : Unix.file_descr -> (unit -> unit) -> (channel -> unit) -> channel
-(** [set_up_channel fd delete read] gives the opportunity to the main loop
-to call [read] when data is available on [fd] and [delete] when [fd] is
-closed *)
-
-val close_channel : channel -> unit
-(** Stops the handling of a channel by the main loop *)
diff --git a/sw/lib/ocaml/ivy/ivy.ml b/sw/lib/ocaml/ivy/ivy.ml
deleted file mode 100644
index 0d5b33cb0c..0000000000
--- a/sw/lib/ocaml/ivy/ivy.ml
+++ /dev/null
@@ -1,81 +0,0 @@
-(* $Id: ivy.ml,v 1.7 2004/09/11 17:06:59 poine Exp $ *)
-
-
-type binding
-type client
-type client_event = Connected | Disconnected
-type cb = client -> string array -> unit
-type client_cb = client -> client_event -> unit
-
-external send : string -> unit = "ivy_sendMsg"
-external stop : unit -> unit = "ivy_stop"
-external ext_init : string -> string -> string -> unit = "ivy_init"
-
-
-let gensym = let n = ref 0 in fun p -> incr n; p ^ string_of_int !n
-let cb_register = fun closure ->
-  let s = gensym "callback_" in
-  Callback.register s closure;
-  s
-
-let init = fun name ready ccb ->
-  ext_init name ready (cb_register ccb)
-
-
-external start : string -> unit = "ivy_start"
-external ext_bind : string -> string -> binding = "ivy_bindMsg"
-
-let bind = fun (cb:cb) regexp ->
-  ext_bind (cb_register cb) regexp
-
-external unbind : binding -> unit = "ivy_unbindMsg"
-
-
-external name_of_client : client -> string = "ivy_name_of_client"
-external host_of_client : client -> string = "ivy_host_of_client"
-
-
-
-let marshal_tag = "MARSHAL"
-
-let hexa_char = fun c ->
-  assert(0 <= c && c < 16);
-  if c < 10 then
-    Char.chr (c + Char.code '0')
-  else
-    Char.chr (c + Char.code 'A' - 10)
-
-let hexa_code = fun c ->
-  if '0' <= c && c <= '9' then
-    Char.code c - Char.code '0'
-  else if 'A' <= c && c <= 'F' then
-    Char.code c - Char.code 'A' + 10
-  else failwith (Printf.sprintf "hexa_code: %c" c)
-
-
-let hexa_of_string = fun s ->
-  let n = String.length s in
-  let h = String.create (n*2) in
-  for i = 0 to n - 1 do
-    let c = Char.code s.[i] in
-    h.[2*i] <- hexa_char (c lsr 4);
-    h.[2*i+1] <- hexa_char (c land 0xf)
-  done;
-  h
-
-let string_of_hexa = fun h ->
-  let n = String.length h / 2 in
-  let s = String.create n in
-  for i = 0 to n - 1 do
-    s.[i] <- Char.chr (hexa_code h.[2*i] lsl 4 + hexa_code h.[2*i+1])
-  done;
-  s
-
-
-let send_data = fun tag value ->
-  let s = hexa_of_string (Marshal.to_string value []) in
-  send (Printf.sprintf "%s %s %s" marshal_tag tag s)
-
-let data_bind = fun cb tag ->
-  let r = Printf.sprintf "%s %s (.*)" marshal_tag tag in
-  bind (fun c a -> cb c (Marshal.from_string (string_of_hexa a.(0)) 0)) r
diff --git a/sw/lib/ocaml/ivy/ivy.mli b/sw/lib/ocaml/ivy/ivy.mli
deleted file mode 100644
index 1d6c6dd578..0000000000
--- a/sw/lib/ocaml/ivy/ivy.mli
+++ /dev/null
@@ -1,57 +0,0 @@
-(** $Id: ivy.mli,v 1.8 2004/09/11 22:23:32 brisset Exp $ *)
-
-(** Interface for ivy-c (http://www.tls.cena.fr/products/ivy/) *)
-
-type binding
-(** Identification of bindings (callback/message) *)
-
-type client
-(** Identification of client applications *)
-
-val name_of_client : client -> string
-val host_of_client : client -> string
-(** Access to client identification *)
-
-type client_event = Connected | Disconnected
-(** Status of (de)connecting applications *)
-
-type cb = client -> string array -> unit
-(** Profile of message binding callback *)
-
-type client_cb = client -> client_event -> unit
-(** Profile of callback for (de)connecting applications *)
-
-val init : string -> string -> client_cb -> unit
-(** [init name ready cb] initializes the application as an IVY client,
-identifying itself with [name], first sending the [ready] message. [cb]
-will be called each time a new application (de)connects to this IVY bus. *)
-
-val start : string -> unit
-(** [start bus] starts the connection to machine/network/port specified in
-[bus]. Syntax for [bus] is ["IPaddress:port"] *)
-
-val bind : cb -> string -> binding
-(** [bind cb regexp] binds callback [cb] to messages matching the [regexp]
-regular expression. [cb] will be called with the array of matching groups
-defined in [regexp]. *)
-
-val send : string -> unit
-(** [send message] sends a message to the IVY initialized bus *)
-
-val stop : unit -> unit
-(** Exits the main loop *)
-
-val unbind : binding -> unit
-(** Removes a message binding *)
-
-val send_data : string -> 'a -> unit
-(** [send_data tag value] marshals [value] into a string and sends it with
-[tag] over the IVY bus *)
-
-val data_bind : (client -> 'a -> unit) -> string -> binding
-(** [data_bind cb tag] binds [cb] to IVY messages sent with [send_data] and
-tagged with [tag]. This operation IS NOT type safe.*)
-
-(***)
-
-val cb_register : ('a -> 'b) -> string
diff --git a/sw/lib/ocaml/ivy/ivyLoop.ml b/sw/lib/ocaml/ivy/ivyLoop.ml
deleted file mode 100644
index 5808de40a2..0000000000
--- a/sw/lib/ocaml/ivy/ivyLoop.ml
+++ /dev/null
@@ -1,22 +0,0 @@
-type channel
-type delete_channel_cb = unit -> unit
-type timer
-type timer_cb = timer -> unit
-
-external ext_timer : int -> int -> string -> timer = "ivy_timerRepeatafter"
-
-let timer = fun n t cb ->
-  let closure_name = Ivy.cb_register cb in
-  ext_timer n t closure_name
-
-external remove_timer : timer -> unit = "ivy_timerRemove"
-
-external main : unit -> unit = "ivy_mainLoop"
-
-external ext_channelSetUp : Unix.file_descr -> string -> channel = "ivy_channelSetUp"
-external close_channel : channel -> unit = "ivy_channelClose"
-
-
-type read_channel_cb = channel -> unit
-let set_up_channel fd delete read =
-  ext_channelSetUp fd (Ivy.cb_register read)
diff --git a/sw/lib/ocaml/ivy/ivyLoop.mli b/sw/lib/ocaml/ivy/ivyLoop.mli
deleted file mode 100644
index 1313dc9202..0000000000
--- a/sw/lib/ocaml/ivy/ivyLoop.mli
+++ /dev/null
@@ -1,25 +0,0 @@
-val main : unit -> unit
-(** Starts the loop which handles asynchronous communications. The standard
-version does not return until IVY is explictly stopped *)
-
-type channel
-(** Channel handled by the main loop *)
-
-val set_up_channel : Unix.file_descr -> (unit -> unit) -> (channel -> unit) -> channel
-(** [set_up_channel fd delete read] gives the opportunity to the main loop
-to call [read] when data is available on [fd] and [delete] when [fd] is
-closed *)
-
-val close_channel : channel -> unit
-(** Stops the handling of a channel by the main loop *)
-
-type timer
-(** Timer identifier *)
-
-val timer : int -> int -> (timer -> unit) -> timer
-(** [timer n ms cb] sets a timer which will call [n] times the callback [cb]
-with a period of [ms] milliseconds *)
-
-val remove_timer : timer -> unit
-(** [remove_timer t] stops the timer [t] *)
-
diff --git a/sw/lib/ocaml/ivy/tkIvy.ml b/sw/lib/ocaml/ivy/tkIvy.ml
deleted file mode 100644
index 357576b272..0000000000
--- a/sw/lib/ocaml/ivy/tkIvy.ml
+++ /dev/null
@@ -1,8 +0,0 @@
-
-type channel
-external main : unit -> unit = "ivy_TclmainLoop"
-external ext_channelSetUp : Unix.file_descr -> string -> channel = "ivy_TclchannelSetUp"
-let set_up_channel fd delete read =
-  ext_channelSetUp fd (Ivy.cb_register read)
-external close_channel : channel -> unit = "ivy_TclchannelClose"
-
diff --git a/sw/lib/ocaml/ivy/tkIvy.mli b/sw/lib/ocaml/ivy/tkIvy.mli
deleted file mode 100644
index e477d8ecdd..0000000000
--- a/sw/lib/ocaml/ivy/tkIvy.mli
+++ /dev/null
@@ -1,13 +0,0 @@
-val main : unit -> unit
-(** Tk main loop *)
-
-type channel
-(** Channel handled by the main loop *)
-
-val set_up_channel : Unix.file_descr -> (unit -> unit) -> (channel -> unit) -> channel
-(** [set_up_channel fd delete read] gives the opportunity to the main loop
-to call [read] when data is available on [fd] and [delete] when [fd] is
-closed *)
-
-val close_channel : channel -> unit
-(** Stops the handling of a channel by the main loop *)
diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml
index 0caeb8d63e..28b1c33039 100644
--- a/sw/lib/ocaml/pprz.ml
+++ b/sw/lib/ocaml/pprz.ml
@@ -73,6 +73,7 @@ let (//) = Filename.concat
 let messages_file = Env.paparazzi_src // "conf" // "messages.xml"
 let lazy_messages_xml = lazy (Xml.parse_file messages_file)
 let messages_xml = fun () -> Lazy.force lazy_messages_xml
+let units_file = Env.paparazzi_src // "conf" // "units.xml"
 
 external float_of_bytes : string -> int -> float = "c_float_of_indexed_bytes"
 external double_of_bytes : string -> int -> float = "c_double_of_indexed_bytes"
@@ -157,21 +158,42 @@ let payload_size_of_message = fun message ->
     message.fields
     2 (** + message id + aircraft id *)
 
+exception Unit_conversion_error of string
+exception Unknown_conversion of string * string
+
+let scale_of_units = fun from_unit to_unit ->
+  if (from_unit = to_unit) then
+    1.0
+  else
+    try
+      let units_xml = Xml.parse_file units_file in
+      (* find the first occurence of matching units or raise Not_found *)
+      let _unit = List.find (fun u ->
+          (* will raise Xml.No_attribute if not a valid attribute *)
+          let f = Xml.attrib u "from"
+          and t = Xml.attrib u "to" in
+          if from_unit = f && to_unit = t then true else false
+        ) (Xml.children units_xml) in
+      (* return coef, raise Failure if coef is not a numerical value *)
+      float_of_string (Xml.attrib _unit "coef")
+    with Xml.File_not_found _ -> raise (Unit_conversion_error ("Parse error of conf/units.xml"))
+      | Xml.No_attribute _ | Xml.Not_element _ -> raise (Unit_conversion_error ("File conf/units.xml has errors"))
+      | Failure "float_of_string" -> raise (Unit_conversion_error ("Unit coef is not numerical value"))
+      | Not_found -> raise (Unknown_conversion (from_unit, to_unit))
+      | _ -> raise (Unknown_conversion (from_unit, to_unit))
+
 
 let alt_unit_coef_of_xml = function xml ->
   try Xml.attrib xml "alt_unit_coef"
   with _ ->
     let u = try Xml.attrib xml "unit" with _ -> "" in
     let au = try Xml.attrib xml "alt_unit" with _ -> "" in
-    match (u, au) with
-      ("deg", "rad") | ("deg/s", "rad/s") -> string_of_float (pi /. 180.)
-    | ("rad", "deg") | ("rad/s", "deg/s") -> string_of_float (180. /. pi)
-    | ("m", "cm") | ("m/s", "cm/s") -> "100."
-    | ("cm", "m") | ("cm/s", "m/s") -> "0.01"
-    | ("m", "mm") | ("m/s", "mm/s") -> "1000."
-    | ("mm", "m") | ("mm/s", "m/s") -> "0.001"
-    | ("decideg", "deg") -> "0.1"
-    | (_, _) -> "1."
+    let coef = try string_of_float (scale_of_units u au) with
+      Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; exit 1
+    | Unknown_conversion _ -> "1." (* Use coef 1. *)
+    | _ -> "1."
+    in
+    coef
 
 let pipe_regexp = Str.regexp "|"
 let field_of_xml = fun xml ->
diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli
index 3ab239c9ca..7dad2be584 100644
--- a/sw/lib/ocaml/pprz.mli
+++ b/sw/lib/ocaml/pprz.mli
@@ -86,21 +86,22 @@ val int32_assoc : string -> values -> Int32.t
 val hex_of_int_array : value -> string
 (** Returns the hexadecimal string of an array of integers *)
 
+exception Unit_conversion_error of string
+(** Unit_conversion_error raised when parsing error occurs *)
+exception Unknown_conversion of string * string
+(** Unknown_conversion raised when conversion fails *)
+
+val scale_of_units : string -> string -> float
+(** scale_of_units from to
+ *  Returns conversion factor between two units
+ *  The possible conversions are described in conf/units.xml
+ *  May raise Invalid_argument if one of the unit is not valid
+ *  or if units.xml is not valid
+ *)
+
 val alt_unit_coef_of_xml : Xml.xml -> string
 (** Return coef for alternate unit
-    Default possible corrections:
-    deg -> rad
-    rad -> deg
-    m -> cm
-    cm -> m
-    m -> mm
-    mm -> m
-    m/s -> cm/s
-    cm/s -> m/s
-    m/s -> mm/s
-    mm/s -> m/s
-    decideg -> deg
-    *)
+ *)
 
 exception Unknown_msg_name of string * string
 (** [Unknown_msg_name (name, class_name)] Raised if message [name] is not
diff --git a/sw/logalizer/matlab_log/messages.xml b/sw/logalizer/matlab_log/messages.xml
index eb5c7f8386..c749615108 100644
--- a/sw/logalizer/matlab_log/messages.xml
+++ b/sw/logalizer/matlab_log/messages.xml
@@ -559,7 +559,7 @@
       </message>
       <message name="TELEMETRY_STATUS" ID="32">
         <field name="ac_id" type="string"/>
-        <field name="time_since_last_bat_msg" type="float" unit="s"/>
+        <field name="time_since_last_msg" type="float" unit="s"/>
       </message>
       <message name="TELEMETRY_ERROR" ID="33">
         <field name="ac_id" type="string"/>
diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml
index ef516682d4..72de7e5f70 100644
--- a/sw/simulator/flightModel.ml
+++ b/sw/simulator/flightModel.ml
@@ -87,18 +87,50 @@ module Make(A:Data.MISSION) = struct
       Not_found ->
 	failwith (Printf.sprintf "Child 'section' with 'name=%s' expected in '%s'\n" name (Xml.to_string A.ac.airframe))
 
+  let tag_define = fun sect name ->
+    try
+      (ExtXml.child sect ~select:(fun x -> ExtXml.attrib x "name" = name) "define")
+    with
+        Not_found ->
+	      failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string sect))
+
   let defined_value = fun sect name ->
     try
-      (Xml.attrib (ExtXml.child sect ~select:(fun x -> ExtXml.attrib x "name" = name) "define") "value")
+      (Xml.attrib (tag_define sect name) "value")
     with
-      Not_found ->
-	failwith (Printf.sprintf "Child 'define' with 'name=%s' expected in '%s'\n" name (Xml.to_string sect))
+        Not_found ->
+	      failwith (Printf.sprintf "Child 'define' with 'name=%s' in '%s' has no value\n" name (Xml.to_string sect))
 
   let float_value = fun section s ->
     let x = (defined_value section s) in
     try float_of_string x with Failure "float_of_string" ->
       failwith (sprintf "float_of_string: %s" x)
 
+  (* FIXME: refactor code_unit_scale of tag to pprz.ml *)
+  let code_unit_scale_of_tag = function t ->
+    (* if unit attribute is not specified don't even attempt to convert the units *)
+    let u = try ExtXml.attrib t "unit" with _ -> failwith "Unit conversion error" in
+    let cu = try ExtXml.attrib t "code_unit" with _ -> "" in
+    (* default value for code_unit is rad[/s] when unit is deg[/s] *)
+    try match (u, cu) with
+        ("deg", "") -> Pprz.scale_of_units u "rad" (* implicit conversion to rad *)
+      | ("deg/s", "") -> Pprz.scale_of_units u "rad/s" (* implicit conversion to rad/s *)
+      | (_, "") -> failwith "Unit conversion error" (* code unit is not defined and no implicit conversion *)
+      | (_,_) -> Pprz.scale_of_units u cu (* try to convert *)
+    with
+        Pprz.Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; exit 1
+      | Pprz.Unknown_conversion (su, scu) -> prerr_endline (sprintf "Warning: unknown unit conversion: from %s to %s" su scu); flush stderr; failwith "Unknown unit conversion"
+      | _ -> failwith "Unit conversion error"
+
+  let code_value = fun section s ->
+    let t = (tag_define section s) in
+    try
+      let coef = try (code_unit_scale_of_tag t) with _ -> 1. in
+      (ExtXml.float_attrib t "value") *. coef
+    with
+        _ ->
+          failwith (Printf.sprintf "Can't convert 'define' with 'name=%s' in '%s' to floating point value\n" s (Xml.to_string section))
+
   let simu_section =
     try section "SIMU" with _ -> Xml.Element("", [], [])
 
@@ -139,12 +171,12 @@ module Make(A:Data.MISSION) = struct
 
   let infrared_section = try section "INFRARED" with _ -> Xml.Element("",[],[])
 
-  let nominal_airspeed = float_of_string (defined_value misc_section "NOMINAL_AIRSPEED")
-  let maximum_airspeed = try float_value misc_section "MAXIMUM_AIRSPEED" with _ -> nominal_airspeed *. 1.5
-  let max_power = try float_value misc_section "MAXIMUM_POWER" with _ -> 5. *. maximum_airspeed *. weight
+  let nominal_airspeed = code_value misc_section "NOMINAL_AIRSPEED"
+  let maximum_airspeed = try code_value misc_section "MAXIMUM_AIRSPEED" with _ -> nominal_airspeed *. 1.5
+  let max_power = try code_value misc_section "MAXIMUM_POWER" with _ -> 5. *. maximum_airspeed *. weight
 
-  let roll_neutral_default = try rad_of_deg (float_value infrared_section "ROLL_NEUTRAL_DEFAULT") with _ -> 0.
-  let pitch_neutral_default = try rad_of_deg (float_value infrared_section "PITCH_NEUTRAL_DEFAULT") with _ -> 0.
+  let roll_neutral_default = try code_value infrared_section "ROLL_NEUTRAL_DEFAULT" with _ -> 0.
+  let pitch_neutral_default = try code_value infrared_section "PITCH_NEUTRAL_DEFAULT" with _ -> 0.
 
 
   let vert_ctrl_section = try section "VERTICAL CONTROL" with _ -> Xml.Element("",[],[])
diff --git a/sw/simulator/flight_gear.h b/sw/simulator/flight_gear.h
index b5515c89cb..eaf35b0ea9 100644
--- a/sw/simulator/flight_gear.h
+++ b/sw/simulator/flight_gear.h
@@ -211,9 +211,15 @@ struct FGNetMiniFDM {
 #define FG_NET_GUI_VERSION 7
 #define FG_NET_GUI_MAX_TANKS 4
 
+// FIXME: Flightgear on OSX is still 32 bit get rid
+// off these pragmas when it goes to 64 bit.
 #ifdef __x86_64__
 #pragma pack(push)
+#ifdef __APPLE__
 #pragma pack(4)
+#else
+#pragma pack(8)
+#endif
 #endif
 struct FGNetGUI {
   uint32_t version;           // increment when data values change
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index 287e69fb3e..a3af693611 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -53,28 +53,28 @@ let cb_register = fun closure ->
 
 
 module type AIRCRAFT =
-  sig
-    val init : int -> GPack.box -> unit
+sig
+  val init : int -> GPack.box -> unit
     (** [init ac_id box] *)
-    val boot : Stdlib.value -> unit
+  val boot : Stdlib.value -> unit
     (** [boot time_acceleration] *)
 
-    val commands : pprz_t array -> unit
-	(** Called once at init *)
+  val commands : pprz_t array -> unit
+    (** Called once at init *)
 
-    val infrared_and_airspeed : float -> float -> float -> float -> unit
-	(** [infrared ir_left ir_front ir_top air_speed] Called on timer *)
+  val infrared_and_airspeed : float -> float -> float -> float -> unit
+    (** [infrared ir_left ir_front ir_top air_speed] Called on timer *)
 
-    val attitude_and_rates : float -> float -> float -> float -> float -> float ->unit
+  val attitude_and_rates : float -> float -> float -> float -> float -> float ->unit
     (** [ahrs phi theta psi p q r] Called on timer *)
 
-    val gps : Gps.state -> unit
-	(** [gps state] Called on timer *)
-  end
+  val gps : Gps.state -> unit
+  (** [gps state] Called on timer *)
+end
 
 
 module type AIRCRAFT_ITL =
-    functor (A : Data.MISSION) -> functor (FM: FlightModel.SIG) -> AIRCRAFT
+  functor (A : Data.MISSION) -> functor (FM: FlightModel.SIG) -> AIRCRAFT
 
 external fg_sizeof : unit -> int = "fg_sizeof"
 external fg_msg : string -> float -> float -> float -> float -> float -> float -> unit = "fg_msg_bytecode" "fg_msg_native"
@@ -118,9 +118,9 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     let ground_alt =
       Srtm.add_path (Env.paparazzi_home ^ "/data/srtm");
       try
-	float (Srtm.of_wgs84 !pos0)
+        float (Srtm.of_wgs84 !pos0)
       with Srtm.Tile_not_found x ->
-	float_attrib flight_plan "ground_alt" in
+        float_attrib flight_plan "ground_alt" in
     ref ground_alt
 
   let main () =
@@ -171,18 +171,18 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
     let ask_for_world_env = fun () ->
       try
-	let (x, y, z) = FlightModel.get_xyz !state in
+        let (x, y, z) = FlightModel.get_xyz !state in
 
-	let gps_sol = compute_gps_state (x,y,z) (FlightModel.get_time !state) in
+        let gps_sol = compute_gps_state (x,y,z) (FlightModel.get_time !state) in
 
-	let float = fun f -> Pprz.Float f in
-	let values = ["east", float x; "north", float y; "up", float z;
-		      "lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat);
-		      "long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long);
-		      "alt", float gps_sol.Gps.alt ] in
-	Ground_Pprz.message_req "sim" "WORLD_ENV" values world_update
+        let float = fun f -> Pprz.Float f in
+        let values = ["east", float x; "north", float y; "up", float z;
+                      "lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat);
+                      "long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long);
+                      "alt", float gps_sol.Gps.alt ] in
+        Ground_Pprz.message_req "sim" "WORLD_ENV" values world_update
       with
-	exc -> fprintf stderr "Error in sim: %s\n%!" (Printexc.to_string exc)
+          exc -> fprintf stderr "Error in sim: %s\n%!" (Printexc.to_string exc)
     in
 
     ignore (GMain.Timeout.add 1000 (fun () -> ask_for_world_env (); true));
@@ -191,15 +191,15 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     let fm_task = fun () ->
       FM.do_commands !state commands;
       let agl =
-	if !noground then max_float
-	else
-	  match !last_gps_state with
-	    Some s ->
-	      begin
-		try s.Gps.alt -. float (Srtm.of_wgs84 s.Gps.wgs84) with
-		  _ -> s.Gps.alt
-	      end
-	  | None -> 0. in
+        if !noground then max_float
+        else
+          match !last_gps_state with
+              Some s ->
+                begin
+                  try s.Gps.alt -. float (Srtm.of_wgs84 s.Gps.wgs84) with
+                      _ -> s.Gps.alt
+                end
+            | None -> 0. in
       FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y, !wind_z) agl fm_period
 
     and ir_task = fun () ->
@@ -231,33 +231,33 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     (** Sending to Flight Gear *)
     let fg_task = fun socket buffer () ->
       match !last_gps_state with
-	None -> ()
-      | Some s ->
-	  let lat = s.Gps.wgs84.Latlong.posn_lat
-	  and lon = s.Gps.wgs84.Latlong.posn_long
-	  and alt = s.Gps.alt
-(*	  and theta_ = s.Gps.course *)
-	  and (phi, theta, psi) = FlightModel.get_attitude !state in
-	  fg_msg buffer lat lon alt phi theta psi;
-(**       for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; **)
-      try
-	ignore (Unix.send socket buffer 0 (String.length buffer) [])
-      with
-	Unix.Unix_error (e,f,a) -> Printf.fprintf stderr "Error fg: %s (%s(%s))\n" (Unix.error_message e) f a
+          None -> ()
+        | Some s ->
+          let lat = s.Gps.wgs84.Latlong.posn_lat
+          and lon = s.Gps.wgs84.Latlong.posn_long
+          and alt = s.Gps.alt
+      (*    and theta_ = s.Gps.course *)
+          and (phi, theta, psi) = FlightModel.get_attitude !state in
+          fg_msg buffer lat lon alt phi theta psi;
+      (**       for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; **)
+          try
+            ignore (Unix.send socket buffer 0 (String.length buffer) [])
+          with
+              Unix.Unix_error (e,f,a) -> Printf.fprintf stderr "Error fg: %s (%s(%s))\n" (Unix.error_message e) f a
     in
 
     let set_pos = fun _ ->
       let current_pos = Latlong.string_of !pos0 in
       begin
-	match GToolbox.input_string ~title:"Setting geographic position" ~text:current_pos "Geographic position"  with
-	  Some s -> pos0 := Latlong.of_string s
-	| _ -> ()
+        match GToolbox.input_string ~title:"Setting geographic position" ~text:current_pos "Geographic position"  with
+            Some s -> pos0 := Latlong.of_string s
+          | _ -> ()
       end;
       begin
-	let text = string_of_float !alt0 in
-	match GToolbox.input_string ~title:"Setting initial altitude" ~text "Geographic altitude"  with
-	  Some s -> alt0 := float_of_string s
-	| _ -> ()
+        let text = string_of_float !alt0 in
+        match GToolbox.input_string ~title:"Setting initial altitude" ~text "Geographic altitude"  with
+            Some s -> alt0 := float_of_string s
+          | _ -> ()
       end
     in
 
@@ -270,14 +270,14 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
       (** Connection to Flight Gear client *)
       if !fg_client <> "" then
-	try
-	  let inet_addr = Unix.inet_addr_of_string !fg_client in
-	  let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
-	  Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501));
-	  let buffer = String.create (fg_sizeof ()) in
-	  Stdlib.timer ~scale:time_scale fg_period (fg_task socket buffer)
-	with
-	  e -> fprintf stderr "Error while connecting to fg: %s" (Printexc.to_string e)
+        try
+          let inet_addr = Unix.inet_addr_of_string !fg_client in
+          let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
+          Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501));
+          let buffer = String.create (fg_sizeof ()) in
+          Stdlib.timer ~scale:time_scale fg_period (fg_task socket buffer)
+        with
+            e -> fprintf stderr "Error while connecting to fg: %s" (Printexc.to_string e)
     in
 
     let take_off = fun () -> FlightModel.set_air_speed !state FM.nominal_airspeed in
@@ -295,8 +295,8 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     if not !autoboot then begin
       let s = GButton.button ~label:"Boot" ~packing:(hbox#pack) () in
       let callback = fun () ->
-	set_pos_and_boot ();
-	s#misc#set_sensitive false in
+        set_pos_and_boot ();
+        s#misc#set_sensitive false in
       ignore (s#connect#clicked ~callback)
     end else
       set_pos_and_boot ();
@@ -304,17 +304,17 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     if not !autolaunch then begin
       let t = GButton.button ~label:"Launch" ~packing:hbox#pack () in
       let callback = fun () ->
-	take_off ();
-	t#misc#set_sensitive false in
+        take_off ();
+        t#misc#set_sensitive false in
       ignore (t#connect#clicked ~callback);
 
       (* Monitor an AUTO2 launch to disable the button *)
       let monitor = fun () ->
-	if FlightModel.get_air_speed !state > 0. then begin
-	  t#misc#set_sensitive false;
-	  false
-	end else
-	  true in
+        if FlightModel.get_air_speed !state > 0. then begin
+          t#misc#set_sensitive false;
+          false
+        end else
+          true in
       ignore (GMain.Timeout.add 1000 monitor)
     end else
       take_off ();
diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml
index 0f2d0ddad9..a1d2e8a7fb 100644
--- a/sw/tools/gen_airframe.ml
+++ b/sw/tools/gen_airframe.ml
@@ -80,19 +80,43 @@ let define_integer name v n =
   in
   continious_frac (truncate v) v (1, (truncate v)) (0, 1)
 
+let code_unit_scale_of_tag = function t ->
+  (* if unit attribute is not specified don't even attempt to convert the units *)
+  let u = try ExtXml.attrib t "unit" with _ -> failwith "Unit conversion error" in
+  let cu = try ExtXml.attrib t "code_unit" with _ -> "" in
+  (* default value for code_unit is rad[/s] when unit is deg[/s] *)
+  try match (u, cu) with
+      ("deg", "") -> Pprz.scale_of_units u "rad" (* implicit conversion to rad *)
+    | ("deg/s", "") -> Pprz.scale_of_units u "rad/s" (* implicit conversion to rad/s *)
+    | (_, "") -> failwith "Unit conversion error" (* code unit is not defined and no implicit conversion *)
+    | (_,_) -> Pprz.scale_of_units u cu (* try to convert *)
+  with
+      Pprz.Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; exit 1
+    | Pprz.Unknown_conversion (su, scu) -> prerr_endline (sprintf "Warning: unknown unit conversion: from %s to %s" su scu); flush stderr; failwith "Unknown unit conversion"
+    | _ -> failwith "Unit conversion error"
+
+
 let parse_element = fun prefix s ->
   match Xml.tag s with
-    "define" -> begin
+      "define" -> begin
+        try
+          begin
+            (* fail if units conversion is not found and just copy value instead,
+               this is important for integer values, you can't just multiply them with 1.0 *)
             try
-        define (prefix^ExtXml.attrib s "name") (ExtXml.display_entities (ExtXml.attrib s "value"));
-        define_integer (prefix^(ExtXml.attrib s "name")) (ExtXml.float_attrib s "value") (ExtXml.int_attrib s "integer");
-            with _ -> ();
-        end
-  | "linear" ->
+              let value = (ExtXml.float_attrib s "value") *. (code_unit_scale_of_tag s) in
+              define (prefix^ExtXml.attrib s "name") (string_of_float value);
+            with
+                _ -> define (prefix^ExtXml.attrib s "name") (ExtXml.display_entities (ExtXml.attrib s "value"));
+          end;
+          define_integer (prefix^(ExtXml.attrib s "name")) (ExtXml.float_attrib s "value") (ExtXml.int_attrib s "integer");
+        with _ -> ();
+      end
+    | "linear" ->
       let name = ExtXml.attrib s "name"
       and n = int_of_string (ExtXml.attrib s "arity") in
       define_macro (prefix^name) n s
-  | _ -> xml_error "define|linear"
+    | _ -> xml_error "define|linear"
 
 
 
diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml
index 86dde60a4f..3284b9bdb4 100644
--- a/sw/tools/gen_messages.ml
+++ b/sw/tools/gen_messages.ml
@@ -115,10 +115,10 @@ module Gen_onboard = struct
   let print_field = fun h (t, name, (_f: format option)) ->
     match t with
       Basic _ ->
-	fprintf h "\t  DownlinkPut%sByAddr(_chan, (%s)); \\\n" (Syntax.nameof t) name
+	fprintf h "\t  DownlinkPut%sByAddr(_trans, _dev, (%s)); \\\n" (Syntax.nameof t) name
     | Array (t, varname) ->
 	let _s = Syntax.sizeof (Basic t) in
-	fprintf h "\t  DownlinkPut%sArray(_chan, %s, %s); \\\n" (Syntax.nameof (Basic t)) (Syntax.length_name varname) name
+	fprintf h "\t  DownlinkPut%sArray(_trans, _dev, %s, %s); \\\n" (Syntax.nameof (Basic t)) (Syntax.length_name varname) name
 
   let print_parameter h = function
       (Array _, s, _) -> fprintf h "%s, %s" (Syntax.length_name s) s
@@ -148,26 +148,26 @@ module Gen_onboard = struct
 
   let print_downlink_macro = fun h {name=s; fields = fields} ->
     if List.length fields > 0 then begin
-      fprintf h "#define DOWNLINK_SEND_%s(_chan, " s;
+      fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev, " s;
     end else
-       fprintf h "#define DOWNLINK_SEND_%s(_chan " s;
+       fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev " s;
     print_macro_parameters h fields;
     fprintf h "){ \\\n";
     let size = (size_fields fields "0") in
-    fprintf h "\tif (DownlinkCheckFreeSpace(_chan, DownlinkSizeOf(_chan, %s))) {\\\n" size;
-    fprintf h "\t  DownlinkCountBytes(_chan, DownlinkSizeOf(_chan, %s)); \\\n" size;
-    fprintf h "\t  DownlinkStartMessage(_chan, \"%s\", DL_%s, %s) \\\n" s s size;
+    fprintf h "\tif (DownlinkCheckFreeSpace(_trans, _dev, DownlinkSizeOf(_trans, _dev, %s))) {\\\n" size;
+    fprintf h "\t  DownlinkCountBytes(_trans, _dev, DownlinkSizeOf(_trans, _dev, %s)); \\\n" size;
+    fprintf h "\t  DownlinkStartMessage(_trans, _dev, \"%s\", DL_%s, %s) \\\n" s s size;
     List.iter (print_field h) fields;
-    fprintf h "\t  DownlinkEndMessage(_chan ) \\\n";
+    fprintf h "\t  DownlinkEndMessage(_trans, _dev ) \\\n";
     fprintf h "\t} else \\\n";
-    fprintf h "\t  DownlinkOverrun(_chan ); \\\n";
+    fprintf h "\t  DownlinkOverrun(_trans, _dev ); \\\n";
     fprintf h "}\n\n"
 
   let print_null_downlink_macro = fun h {name=s; fields = fields} ->
     if List.length fields > 0 then begin
-      fprintf h "#define DOWNLINK_SEND_%s(_chan, " s;
+      fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev, " s;
     end else
-      fprintf h "#define DOWNLINK_SEND_%s(_chan " s;
+      fprintf h "#define DOWNLINK_SEND_%s(_trans, _dev " s;
     print_macro_parameters h fields;
     fprintf h ") {}\n"
 
diff --git a/sw/tools/gen_periodic.ml b/sw/tools/gen_periodic.ml
index b0fe980800..28d28ba223 100644
--- a/sw/tools/gen_periodic.ml
+++ b/sw/tools/gen_periodic.ml
@@ -39,7 +39,7 @@ let lprintf = fun c f ->
   fprintf c "%s" (String.make !margin ' ');
   fprintf c f
 
-let output_modes = fun avr_h process_name channel_name modes freq modules ->
+let output_modes = fun avr_h process_name channel_name device_name modes freq modules ->
   let min_period = 1./.float freq in
   let max_period = 65536. /. float freq in
   (** For each mode in this process *)
@@ -84,7 +84,7 @@ let output_modes = fun avr_h process_name channel_name modes freq modules ->
           l := (p, !phase) :: !l;
           i := !i + freq/10;
           right ();
-          lprintf avr_h "PERIODIC_SEND_%s(%s);\\\n" message_name channel_name;
+          lprintf avr_h "PERIODIC_SEND_%s(%s,%s);\\\n" message_name channel_name device_name;
           left ();
           lprintf avr_h "} \\\n"
         )
@@ -118,10 +118,11 @@ let _ =
   (** For each process *)
   List.iter
     (fun process ->
-      let process_name = ExtXml.attrib process "name" and
-      channel_name =  ExtXml.attrib process "channel" in
+      let process_name = ExtXml.attrib process "name"
+      and channel_name =  ExtXml.attrib_or_default process "channel" "DefaultChannel"
+      and device_name = ExtXml.attrib_or_default process "device" "DefaultDevice" in
 
-      fprintf avr_h "\n/* Macros for %s process channel %s */\n" process_name channel_name;
+      fprintf avr_h "\n/* Macros for %s process channel %s with device %s */\n" process_name channel_name device_name;
 
       let modes = Xml.children process in
 
@@ -140,9 +141,9 @@ let _ =
         incr i)
         modes;
 
-      lprintf avr_h "#define PeriodicSend%s(%s) {  /* %dHz */ \\\n" process_name channel_name freq;
+      lprintf avr_h "#define PeriodicSend%s(%s,%s) {  /* %dHz */ \\\n" process_name channel_name device_name freq;
       right ();
-      output_modes avr_h process_name channel_name modes freq modules_name;
+      output_modes avr_h process_name channel_name device_name modes freq modules_name;
       left ();
       lprintf avr_h "}\n"
     )
diff --git a/sw/tools/gen_settings.ml b/sw/tools/gen_settings.ml
index 8d2042dc9e..d145e5ecf2 100644
--- a/sw/tools/gen_settings.ml
+++ b/sw/tools/gen_settings.ml
@@ -59,7 +59,7 @@ let print_dl_settings = fun settings ->
   List.iter
     (fun s ->
       try
-    modules := StringSet.add (ExtXml.attrib s "module") !modules
+        modules := StringSet.add (ExtXml.attrib s "module") !modules
       with ExtXml.Error e -> ()
     )
     settings;
@@ -78,15 +78,15 @@ let print_dl_settings = fun settings ->
   List.iter
     (fun s ->
       let v = ExtXml.attrib s "var" in
-    begin
-    try
-      let h = ExtXml.attrib s "handler" and
-          m =  ExtXml.attrib s "module" in
-      lprintf "case %d: %s_%s( _value ); _value = %s; break;\\\n" !idx (Filename.basename m) h v
-    with
-      ExtXml.Error e -> lprintf "case %d: %s = _value; break;\\\n" !idx v
-    end;
-    incr idx
+      begin
+        try
+          let h = ExtXml.attrib s "handler" and
+              m =  ExtXml.attrib s "module" in
+          lprintf "case %d: %s_%s( _value ); _value = %s; break;\\\n" !idx (Filename.basename m) h v
+        with
+            ExtXml.Error e -> lprintf "case %d: %s = _value; break;\\\n" !idx v
+      end;
+      incr idx
     )
     settings;
   lprintf "default: break;\\\n";
@@ -97,7 +97,7 @@ let print_dl_settings = fun settings ->
   let nb_values = !idx in
 
   (** Macro to call to downlink current values *)
-  lprintf "#define PeriodicSendDlValue(_chan) { \\\n";
+  lprintf "#define PeriodicSendDlValue(_trans, _dev) { \\\n";
   if nb_values > 0 then begin
     right ();
     lprintf "static uint8_t i;\\\n";
@@ -108,13 +108,13 @@ let print_dl_settings = fun settings ->
     right ();
     List.iter
       (fun s ->
-    let v = ExtXml.attrib s "var" in
-    lprintf "case %d: var = %s; break;\\\n" !idx v; incr idx)
+        let v = ExtXml.attrib s "var" in
+        lprintf "case %d: var = %s; break;\\\n" !idx v; incr idx)
       settings;
     lprintf "default: var = 0.; break;\\\n";
     left ();
     lprintf "}\\\n";
-    lprintf "DOWNLINK_SEND_DL_VALUE(_chan, &i, &var);\\\n";
+    lprintf "DOWNLINK_SEND_DL_VALUE(_trans, _dev, &i, &var);\\\n";
     lprintf "i++;\\\n";
     left ()
   end;
@@ -177,13 +177,13 @@ let print_persistent_settings = fun settings ->
     (fun s ->
       let v = ExtXml.attrib s "var" in
       begin
-	 try
-	   let h = ExtXml.attrib s "handler" and
-               m =  ExtXml.attrib s "module" in
-	   lprintf "%s_%s( pers_settings.s_%d );\n"  (Filename.basename m) h !idx ;
-(*	   lprintf "%s = pers_settings.s_%d;\n" v !idx *) (* do we want to set the value too or just call the handler ? *)
-	 with
-	   ExtXml.Error e ->  lprintf "%s = pers_settings.s_%d;\n" v !idx
+        try
+          let h = ExtXml.attrib s "handler" and
+              m =  ExtXml.attrib s "module" in
+          lprintf "%s_%s( pers_settings.s_%d );\n"  (Filename.basename m) h !idx ;
+        (*	   lprintf "%s = pers_settings.s_%d;\n" v !idx *) (* do we want to set the value too or just call the handler ? *)
+        with
+            ExtXml.Error e ->  lprintf "%s = pers_settings.s_%d;\n" v !idx
       end;
       incr idx)
     pers_settings;
@@ -252,7 +252,7 @@ let join_xml_files = fun xml_files ->
     let xml = Xml.parse_file xml_file in
     let these_rc_settings =
       try Xml.children (ExtXml.child xml "rc_settings") with
-    Not_found -> [] in
+          Not_found -> [] in
     let these_dl_settings =
       try Xml.children (ExtXml.child xml "dl_settings") with
     Not_found -> [] in