commit 58fe0e0b7c95ecebf083a444b247510a634715b7
Author: Thomas Schöpping <tschoepp@cit-ec.uni-bielefeld.de>
Date:   Wed Jun 8 13:46:15 2016 +0200

    Initial commit of AMiRo-OS version 1.0. This is the first version released open source.

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..320c10b
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,23 @@
+doc
+build
+.dep
+*.tmp
+*.backup
+
+# Eclipse files
+*eclipse*
+
+# QtCreator files
+*.includes
+*.files
+*.config
+*.creator
+*.creator.user
+
+# Shadow files
+*kate-swp
+*~
+
+# Backup files
+*backup*
+*Backup*
diff --git a/README.txt b/README.txt
new file mode 100644
index 0000000..54de67d
--- /dev/null
+++ b/README.txt
@@ -0,0 +1,197 @@
+AMiRo-OS is the operating system for the base version of the
+Autonomous Mini Robot (AMiRo) [1,2]. It utilizes ChibiOS (a real-time
+operating system for embedded devices developed by Giovanni di Sirio;
+see <http://chibios.org>) as system kernel and extends it with
+platform specific functionalities.
+
+Copyright (C) 2016  Thomas Schöpping et al.
+(a complete list of all authors is given below)
+
+This program 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 3 of the License, or (at
+your option) any later version.
+
+This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+The development of this software was supported by the Excellence
+Cluster EXC 227 Cognitive Interaction Technology. The Excellence
+Cluster EXC 227 is a grant of the Deutsche Forschungsgemeinschaft
+(DFG) in the context of the German Excellence Initiative.
+
+Authors:
+ - Thomas Schöpping        <tschoepp[at]cit-ec.uni-bielefeld.de>
+ - Timo Korthals           <tkorthals[at]cit-ec.uni-bielefeld.de>
+ - Stefan Herbechtsmeier   <sherbrec[at]cit-ec.uni-bielefeld.de>
+ - Teerapat Chinapirom     <tchinapirom[at]cit-ec.uni-bielefeld.de>
+ - Robert Abel
+ - Marvin Barther
+ - Claas Braun
+ - Tristan Kenneweg
+
+References:
+ [1] Herbrechtsmeier S., Rückert U., & Sitte J. (2012). "AMiRo -
+     Autonomous Mini Robot for Research and Education". In Advances in
+     Autonomous Mini Robots (pp. 101-112). Springer Berlin
+     Heidelberg.
+ [2] Schöpping T., Korthals T., Herbrechtsmeier S., & Rückert U.
+     (2015). "AMiRo: A Mini Robot for Scientific Applications" In
+     Advances in Computational Intelligence (pp. 199-205). Springer
+     International Publishing.
+
+
+
+#####################################################################
+#                                                                   #
+#     RRRRRRRR  EEEEEEEE    AAA    DDDDDDDD  MM     MM EEEEEEEE     #
+#     RR     RR EE         AA AA   DD     DD MMM   MMM EE           #
+#     RR     RR EE        AA   AA  DD     DD MMMM MMMM EE           #
+#     RRRRRRRR  EEEEEE   AA     AA DD     DD MM MMM MM EEEEEE       #
+#     RR   RR   EE       AAAAAAAAA DD     DD MM     MM EE           #
+#     RR    RR  EE       AA     AA DD     DD MM     MM EE           #
+#     RR     RR EEEEEEEE AA     AA DDDDDDDD  MM     MM EEEEEEEE     #
+#                                                                   #
+#####################################################################
+
+This file will help you to setup all required software on your system,
+compile the source code, and flash it to the AMiRo modules.
+
+=====================================================================
+
+CONTENTS:
+ 1  Required software
+   1.1  gcc-arm-none-eabi
+   1.2  ChibiOS
+   1.3  AMiRo-BLT
+ 2  Recommended software
+   2.1  gtkterm and hterm
+   2.2  QtCreator
+ 3  Building and flashing
+
+=====================================================================
+
+1 - REQUIRED SOFTWARE
+---------------------
+
+In order to compile the source code, you need to install the GCC for
+ARM embedded devices. Since AMiRo-OS requires ChibiOS as system
+kernel, you need a copy of that project as well. Furthermore, AMiRo-OS
+requires a compatible bootloader, such as provided by the AMiRo-BLT
+project.
+
+1.1 gcc-arm-none-eabi
+
+Various versions of the GCC for ARM embedded devices can be found at
+<https://launchpad.net/gcc-arm-embedded>. It is highly recommended to
+use the version 4.8 with update 2014-q1 since some others will cause
+issues. For installation of the compiler toolchain, please follow the
+instructions that can be found on the web page.
+
+1.2 ChibiOS
+
+Since AMiRo-OS uses ChibiOS as underlying system kernel, you need to
+acquire a copy of it as well. First, go to the directory which
+contains the AMiRo-OS folder (but do not go into the AMiRo-OS
+directory itself!). Now clone the GIT repository of ChibiOS and
+checkout version 2.6.x:
+  >$ git clone https://github.com/ChibiOS/ChibiOS.git ChibiOS
+  >$ cd ChibiOS
+  >$ git checkout 2e6dfc7364e7551483922ea6afbaea8f3501ab0e
+It is highly recommended to use exactly this commit. Although newer
+commits in the 2.6.x branch might work fine, AMiRo-OS is not
+compatible with ChibiOS version 3 or newer.
+
+AMiRo-OS comes with some patches to ChibiOS, which must be applied as
+well before compiling the project. Therefore you need to copy all
+files from the ./patches directory of AMiRo-OS to the root directory
+of ChibiOS. You can then apply the patches via the following command:
+  >$ for i in `ls | grep patch`; do git am --ignore-space-change --ignore-whitespace < ${i}; done
+If the files could not be patched successfully, you are probably using
+an incompatible version of ChibiOS (try to checkout the correct commit
+as denoted above).
+
+1.3 AMiRo-BLT
+
+AMiRo-BLT is an additional software project, which is developed in
+parallel with AMiRo-OS. If you did not receive a copy of AMiRo-BLT
+with AMiRo-OS, you can find all code and documentation at
+<https://opensource.cit-ec.de/projects/amiro-os>. Instructions for
+installation and how to use the software provided by AMiRo-BLT can be
+found on the web page or in the project's readme file.
+
+2 - RECOMMENDED SOFTWARE
+------------------------
+
+In order to fully use all features of AMiRo-OS it is recommended to
+install the 'hterm' or 'gtkterm' application for accessing the robot.
+To ease further development, this project offers support for the 
+QtCreator IDE.
+
+2.1 - gtkterm and hterm
+
+Depending on your operating system, it is recommended to install
+'gtkterm' for Linux (available in the Ubuntu repositories), or 'hterm'
+for Windows.
+For gtkterm you need to modify the configuration file ~/.gtktermrc (it
+is generated automatically when you start the application for the
+first time) as follows:
+
+  port	= /dev/ttyUSB0
+  speed	= 115200
+  bits	= 8
+  stopbits	= 1
+  parity	= none
+  flow	= none
+  wait_delay	= 0
+  wait_char	= -1
+  rs485_rts_time_before_tx	= 30
+  rs485_rts_time_after_tx	= 30
+  echo	= False
+  crlfauto	= True
+
+For hterm you must need to configure the tool analogously.
+
+2.2 - QtCreator
+
+In order to setup QtCreator projects for the three AMiRo base modules,
+a script is provided in the directory ./ide/qtcreator/. It is
+accompanied by an additional README.txt file, which contains further
+information.
+
+3 - BUILDING AND FLASHING
+-------------------------
+
+Each time you modify any part of AMiRo-OS, you need to recompile the
+whole project for the according AMiRo module. Therefore you have to
+use the makefiles provided in ./devices/<DeviceToRecompile>/ by simply
+executing 'make' in the according directory. If you want to compile
+all modules at once, you can also use the makefile in the ./devices/
+folder.
+
+After compilation, you always have to flash the generated program to
+the robot. Therefore you need to install the SerialBoot tool provided
+by the AMiRo-BLT project. Furthermore the tool must be accessible
+globally under the environment variable 'SERIALBOOT'. You can do this
+by appending the following line to your ~/.bashrc file:
+
+  export SERIALBOOT=</absolute/path/to/the/SerialBoot/binary>
+
+You can test the tool by calling it via the variable:
+  >$ ${SERIALBOOT}
+This should print some information about the tool.
+Similar to the compilation procedure as described above, you can flash
+either each module separately, or all modules at once by executing
+'make flash' from the according directory.
+Note that you must connect the programming cable either to the
+DiWheelDrive or the PowerManagement module for flashing the operating
+system. All other modules are powered off after reset so that only
+these two offer a bootloader that is required for flashing.
+
+=====================================================================
+
diff --git a/boards/DiWheelDrive/board.c b/boards/DiWheelDrive/board.c
new file mode 100644
index 0000000..7101d85
--- /dev/null
+++ b/boards/DiWheelDrive/board.c
@@ -0,0 +1,117 @@
+#include "ch.h"
+#include "hal.h"
+
+/**
+ * @brief   PAL setup.
+ * @details Digital I/O ports static configuration as defined in @p board.h.
+ *          This variable is used by the HAL when initializing the PAL driver.
+ */
+#if HAL_USE_PAL || defined(__DOXYGEN__)
+const PALConfig pal_default_config =
+{
+  {VAL_GPIOAODR, VAL_GPIOACRL, VAL_GPIOACRH},
+  {VAL_GPIOBODR, VAL_GPIOBCRL, VAL_GPIOBCRH},
+  {VAL_GPIOCODR, VAL_GPIOCCRL, VAL_GPIOCCRH},
+  {VAL_GPIODODR, VAL_GPIODCRL, VAL_GPIODCRH},
+  {VAL_GPIOEODR, VAL_GPIOECRL, VAL_GPIOECRH},
+  {VAL_GPIOFODR, VAL_GPIOFCRL, VAL_GPIOFCRH},
+  {VAL_GPIOGODR, VAL_GPIOGCRL, VAL_GPIOGCRH},
+};
+
+#endif
+
+/*
+ * Early initialization code.
+ * This initialization must be performed just after stack setup and before
+ * any other initialization.
+ */
+void __early_init(void) {
+
+  stm32_clock_init();
+}
+
+/*
+ * Board-specific initialization code.
+ */
+void boardInit(void) {
+  /*
+   * Several I/O pins are re-mapped:
+   *   JTAG disabled and SWJ enabled
+   *   TIM2 to the PA15/PB3/PA2/PA3 pins.
+   *   TIM3 to PC6/PC7 pins.
+   *   USART3 to the PC10/PC11 pins.
+   *   I2C1 to the PB8/PB9 pins.
+   */
+  AFIO->MAPR = AFIO_MAPR_SWJ_CFG_DISABLE |
+               AFIO_MAPR_TIM2_REMAP_PARTIALREMAP1 |
+               AFIO_MAPR_TIM3_REMAP_FULLREMAP |
+               AFIO_MAPR_USART3_REMAP_PARTIALREMAP |
+               AFIO_MAPR_I2C1_REMAP;
+}
+
+inline void boardWriteIoPower(const uint8_t value)
+{
+    if (value) {
+        // drive pins
+        palSetPadMode(GPIOA, GPIOA_CAN_TX, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
+    } else {
+        // float pins
+        palSetPadMode(GPIOA, GPIOA_CAN_TX, PAL_MODE_INPUT);
+    }
+}
+
+inline void boardWriteLed(int value)
+{
+    palWritePad(GPIOA, GPIOA_LED, !value);
+}
+
+inline void boardRequestShutdown(void)
+{
+  palClearPad(GPIOC, GPIOC_SYS_PD_N);
+}
+
+inline void boardStandby(void)
+{
+
+  palSetPad(GPIOC, GPIOC_SYS_PD_N);
+  chSysLock();
+  // Standby
+  // set deepsleep bit
+  SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
+  // enable wakeup pin
+  //PWR->CSR |= PWR_CSR_EWUP;
+  // set PDDS, clear WUF, clear SBF
+  PWR->CR |= (PWR_CR_CWUF | PWR_CR_PDDS | PWR_CR_CSBF);
+  // clear RTC wakeup source flags
+  RTC->CRL &= ~(RTC_CRL_ALRF);
+  // Wait for Interrupt
+  __WFI();
+
+}
+
+inline void boardWakeup(void) {
+
+  palClearPad(GPIOC, GPIOC_SYS_PD_N);
+  chThdSleepMicroseconds(10);
+  palSetPad(GPIOC, GPIOC_SYS_PD_N);
+}
+
+inline void boardClearI2CBus(const uint8_t scl_pad) {
+
+  uint8_t i;
+
+  // configure I²C SCL open drain
+  palSetPadMode(GPIOB, scl_pad, PAL_MODE_OUTPUT_OPENDRAIN);
+
+  // perform bus clear as per I²C Specification v5 3.1.16
+  for (i = 0x00u; i < 0x09u; i++) {
+    palClearPad(GPIOB, scl_pad);
+    chThdSleepMicroseconds(5);
+    palSetPad(GPIOB, scl_pad);
+    chThdSleepMicroseconds(5);
+  }
+
+  // reconfigure I²C SCL
+  palSetPadMode(GPIOB, scl_pad, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
+
+}
diff --git a/boards/DiWheelDrive/board.h b/boards/DiWheelDrive/board.h
new file mode 100644
index 0000000..d06a026
--- /dev/null
+++ b/boards/DiWheelDrive/board.h
@@ -0,0 +1,207 @@
+#ifndef _BOARD_H_
+#define _BOARD_H_
+
+/*
+ * Setup for AMiRo DiWheelDrive board.
+ */
+
+/*
+ * Board identifier.
+ */
+#define BOARD_DI_WHEEL_DRIVE
+#define BOARD_NAME              "AMiRo DiWheelDrive"
+#define BOARD_VERSION           "1.1"
+
+/*
+ * Board frequencies.
+ */
+#define STM32_LSECLK            0
+#define STM32_HSECLK            8000000
+
+/*
+ * MCU type as defined in the ST header file stm32f1xx.h.
+ */
+#define STM32F10X_HD
+
+/*
+ * IO pins assignments.
+ */
+#define GPIOA_WKUP              0
+#define GPIOA_LED               1
+#define GPIOA_DRIVE_PWM1A       2
+#define GPIOA_DRIVE_PWM1B       3
+#define GPIOA_MOTION_SCLK       5
+#define GPIOA_MOTION_MISO       6
+#define GPIOA_MOTION_MOSI       7
+#define GPIOA_PROG_RX           9
+#define GPIOA_PROG_TX           10
+#define GPIOA_CAN_RX            11
+#define GPIOA_CAN_TX            12
+#define GPIOA_SWDIO             13
+#define GPIOA_SWCLK             14
+#define GPIOA_DRIVE_PWM2B       15
+
+
+#define GPIOB_DRIVE_SENSE2      1
+#define GPIOB_POWER_EN          2
+#define GPIOB_DRIVE_PWM2A       3
+#define GPIOB_COMPASS_DRDY      5
+#define GPIOB_DRIVE_ENC1A       6
+#define GPIOB_DRIVE_ENC1B       7
+#define GPIOB_COMPASS_SCL       8
+#define GPIOB_COMPASS_SDA       9
+#define GPIOB_IR_SCL            10
+#define GPIOB_IR_SDA            11
+#define GPIOB_IR_INT            12
+#define GPIOB_GYRO_DRDY         13
+#define GPIOB_SYS_UART_UP       14
+#define GPIOB_ACCEL_INT_N       15
+
+#define GPIOC_DRIVE_SENSE1      0
+#define GPIOC_SYS_INT_N         1
+#define GPIOC_PATH_DCSTAT       3
+#define GPIOC_PATH_DCEN         5
+#define GPIOC_DRIVE_ENC2B       6
+#define GPIOC_DRIVE_ENC2A       7
+#define GPIOC_SYS_PD_N          8
+#define GPIOC_SYS_REG_EN        9
+#define GPIOC_SYS_UART_RX       10
+#define GPIOC_SYS_UART_TX       11
+#define GPIOC_ACCEL_SS_N        13
+#define GPIOC_GYRO_SS_N         14
+
+#define GPIOD_OSC_IN            0
+#define GPIOD_OSC_OUT           1
+#define GPIOD_SYS_WARMRST_N     2
+
+/*
+ * I/O ports initial setup, this configuration is established soon after reset
+ * in the initialization code.
+ */
+#define PIN_MODE_INPUT(n)               (0x4U << (((n) % 8) * 4))
+#define PIN_MODE_INPUT_PULLX(n)         (0x8U << (((n) % 8) * 4))
+#define PIN_MODE_INPUT_ANALOG(n)        (0x0U << (((n) % 8) * 4))
+/* Push Pull output 50MHz */
+#define PIN_MODE_OUTPUT_PUSHPULL(n)     (0x3U << (((n) % 8) * 4))
+/* Open Drain output 50MHz */
+#define PIN_MODE_OUTPUT_OPENDRAIN(n)    (0x7U << (((n) % 8) * 4))
+/* Alternate Push Pull output 50MHz */
+#define PIN_MODE_ALTERNATE_PUSHPULL(n)  (0xbU << (((n) % 8) * 4))
+/* Alternate Open Drain output 50MHz */
+#define PIN_MODE_ALTERNATE_OPENDRAIN(n) (0xfU << (((n) % 8) * 4))
+
+/*
+ * Port A setup.
+ */
+#define VAL_GPIOACRL            (PIN_MODE_INPUT(GPIOA_WKUP) | \
+                                 PIN_MODE_OUTPUT_OPENDRAIN(GPIOA_LED) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_DRIVE_PWM1A) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_DRIVE_PWM1B) | \
+                                 PIN_MODE_INPUT_PULLX(4) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_MOTION_SCLK) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOA_MOTION_MISO) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_MOTION_MOSI))
+#define VAL_GPIOACRH            (PIN_MODE_INPUT_PULLX(8) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_PROG_RX) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOA_PROG_TX) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOA_CAN_RX) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_CAN_TX) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOA_SWDIO) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOA_SWCLK) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_DRIVE_PWM2B))
+#define VAL_GPIOAODR            0xF7FF /* prevent power over CAN bug */
+
+/*
+ * Port B setup.
+ */
+#define VAL_GPIOBCRL            (PIN_MODE_INPUT_PULLX(0) | \
+                                 PIN_MODE_INPUT_ANALOG(GPIOB_DRIVE_SENSE2) | \
+                                 PIN_MODE_OUTPUT_PUSHPULL(GPIOB_POWER_EN) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOB_DRIVE_PWM2A) | \
+                                 PIN_MODE_INPUT_PULLX(4) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOB_COMPASS_DRDY) | \
+                                 PIN_MODE_INPUT(GPIOB_DRIVE_ENC1A) | \
+                                 PIN_MODE_INPUT(GPIOB_DRIVE_ENC1B))
+#define VAL_GPIOBCRH            (PIN_MODE_ALTERNATE_OPENDRAIN(GPIOB_COMPASS_SCL) | \
+                                 PIN_MODE_ALTERNATE_OPENDRAIN(GPIOB_COMPASS_SDA) | \
+                                 PIN_MODE_ALTERNATE_OPENDRAIN(GPIOB_IR_SCL) | \
+                                 PIN_MODE_ALTERNATE_OPENDRAIN(GPIOB_IR_SDA) | \
+                                 PIN_MODE_INPUT(GPIOB_IR_INT) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOB_GYRO_DRDY) | \
+                                 PIN_MODE_OUTPUT_OPENDRAIN(GPIOB_SYS_UART_UP) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOB_ACCEL_INT_N))
+#define VAL_GPIOBODR            0xFFFB /* initially the motors are not powered */
+
+/*
+ * Port C setup.
+ */
+#define VAL_GPIOCCRL            (PIN_MODE_INPUT_ANALOG(GPIOC_DRIVE_SENSE1) | \
+                                 PIN_MODE_OUTPUT_OPENDRAIN(GPIOC_SYS_INT_N) | \
+                                 PIN_MODE_INPUT_PULLX(2) | \
+                                 PIN_MODE_INPUT(GPIOC_PATH_DCSTAT) | \
+                                 PIN_MODE_INPUT_PULLX(4) | \
+                                 PIN_MODE_OUTPUT_PUSHPULL(GPIOC_PATH_DCEN) | \
+                                 PIN_MODE_INPUT(GPIOC_DRIVE_ENC2B) | \
+                                 PIN_MODE_INPUT(GPIOC_DRIVE_ENC2A))
+#define VAL_GPIOCCRH            (PIN_MODE_OUTPUT_OPENDRAIN(GPIOC_SYS_PD_N) | \
+                                 PIN_MODE_INPUT(GPIOC_SYS_REG_EN) | \
+                                 PIN_MODE_INPUT(GPIOC_SYS_UART_RX) | \
+                                 PIN_MODE_INPUT(GPIOC_SYS_UART_TX) | \
+                                 PIN_MODE_INPUT_PULLX(12) | \
+                                 PIN_MODE_OUTPUT_PUSHPULL(GPIOC_ACCEL_SS_N) | \
+                                 PIN_MODE_OUTPUT_PUSHPULL(GPIOC_GYRO_SS_N) | \
+                                 PIN_MODE_INPUT_PULLX(15))
+#define VAL_GPIOCODR            0xFFDD /* initially charging via the pins is disabled and SYSNIN_N indicates that the OS is busy */
+
+/*
+ * Port D setup.
+ */
+#define VAL_GPIODCRL            (PIN_MODE_INPUT(GPIOD_OSC_IN) | \
+                                 PIN_MODE_INPUT(GPIOD_OSC_OUT) | \
+                                 PIN_MODE_OUTPUT_OPENDRAIN(GPIOD_SYS_WARMRST_N) | \
+                                 PIN_MODE_INPUT_PULLX(3) | \
+                                 PIN_MODE_INPUT_PULLX(4) | \
+                                 PIN_MODE_INPUT_PULLX(5) | \
+                                 PIN_MODE_INPUT_PULLX(6) | \
+                                 PIN_MODE_INPUT_PULLX(7))
+#define VAL_GPIODCRH            0x88888888
+#define VAL_GPIODODR            0xFFFF
+
+/*
+ * Port E setup.
+ */
+#define VAL_GPIOECRL            0x88888888 /*  PE7...PE0 */
+#define VAL_GPIOECRH            0x88888888 /* PE15...PE8 */
+#define VAL_GPIOEODR            0xFFFF
+
+/*
+ * Port F setup.
+ */
+#define VAL_GPIOFCRL            0x88888888 /*  PF7...PF0 */
+#define VAL_GPIOFCRH            0x88888888 /* PF15...PF8 */
+#define VAL_GPIOFODR            0xFFFF
+
+/*
+ * Port G setup.
+ */
+#define VAL_GPIOGCRL            0x88888888 /*  PG7...PG0 */
+#define VAL_GPIOGCRH            0x88888888 /* PG15...PG8 */
+#define VAL_GPIOGODR            0xFFFF
+
+#if !defined(_FROM_ASM_)
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void boardInit(void);
+  void boardWriteIoPower(const uint8_t value);
+  void boardWriteLed(int value);
+  void boardRequestShutdown(void);
+  void boardStandby(void);
+  void boardWakeup(void);
+  void boardClearI2CBus(const uint8_t scl_pad);
+#ifdef __cplusplus
+}
+#endif
+#endif /* _FROM_ASM_ */
+
+#endif /* _BOARD_H_ */
diff --git a/boards/DiWheelDrive/board.mk b/boards/DiWheelDrive/board.mk
new file mode 100644
index 0000000..ad22449
--- /dev/null
+++ b/boards/DiWheelDrive/board.mk
@@ -0,0 +1,9 @@
+# List of all the board related files.
+BOARDSRC = ${AMIRO}/boards/DiWheelDrive/board.c
+
+# Required include directories
+BOARDINC = ${AMIRO}/boards/DiWheelDrive
+
+# Required linker directory
+BOARDLD = ${AMIRO}/ports
+
diff --git a/boards/LightRing/board.c b/boards/LightRing/board.c
new file mode 100644
index 0000000..a06ba24
--- /dev/null
+++ b/boards/LightRing/board.c
@@ -0,0 +1,85 @@
+#include "ch.h"
+#include "hal.h"
+
+/**
+ * @brief   PAL setup.
+ * @details Digital I/O ports static configuration as defined in @p board.h.
+ *          This variable is used by the HAL when initializing the PAL driver.
+ */
+#if HAL_USE_PAL || defined(__DOXYGEN__)
+const PALConfig pal_default_config =
+{
+  {VAL_GPIOAODR, VAL_GPIOACRL, VAL_GPIOACRH},
+  {VAL_GPIOBODR, VAL_GPIOBCRL, VAL_GPIOBCRH},
+  {VAL_GPIOCODR, VAL_GPIOCCRL, VAL_GPIOCCRH},
+  {VAL_GPIODODR, VAL_GPIODCRL, VAL_GPIODCRH},
+  {VAL_GPIOEODR, VAL_GPIOECRL, VAL_GPIOECRH},
+  {VAL_GPIOFODR, VAL_GPIOFCRL, VAL_GPIOFCRH},
+  {VAL_GPIOGODR, VAL_GPIOGCRL, VAL_GPIOGCRH},
+};
+
+#endif
+
+/*
+ * Early initialization code.
+ * This initialization must be performed just after stack setup and before
+ * any other initialization.
+ */
+void __early_init(void) {
+
+  stm32_clock_init();
+}
+
+/*
+ * Board-specific initialization code.
+ */
+void boardInit(void) {
+  /*
+   * Several I/O pins are re-mapped:
+   *   JTAG disabled and SWD enabled
+   */
+  AFIO->MAPR = AFIO_MAPR_SWJ_CFG_JTAGDISABLE |
+               AFIO_MAPR_USART3_REMAP_PARTIALREMAP;
+}
+
+inline void boardRequestShutdown(void)
+{
+  palClearPad(GPIOC, GPIOC_SYS_PD_N);
+}
+
+inline void boardStandby(void)
+{
+
+  palSetPad(GPIOC, GPIOC_SYS_PD_N);
+  chSysLock();
+  // Standby
+  // set deepsleep bit
+  SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
+  // set PDDS, clear WUF, clear SBF
+  PWR->CR |= (PWR_CR_CWUF | PWR_CR_PDDS | PWR_CR_CSBF);
+  // clear RTC wakeup source flags
+  RTC->CRL &= ~(RTC_CRL_ALRF);
+  // Wait for Interrupt
+  __WFI();
+
+}
+
+inline void boardClearI2CBus(const uint8_t scl_pad) {
+
+  uint8_t i;
+
+  // configure I²C SCL open drain
+  palSetPadMode(GPIOB, scl_pad, PAL_MODE_OUTPUT_OPENDRAIN);
+
+  // perform bus clear as per I²C Specification v5 3.1.16
+  for (i = 0x00u; i < 0x09u; i++) {
+    palClearPad(GPIOB, scl_pad);
+    chThdSleepMicroseconds(5);
+    palSetPad(GPIOB, scl_pad);
+    chThdSleepMicroseconds(5);
+  }
+
+  // reconfigure I²C SCL
+  palSetPadMode(GPIOB, scl_pad, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
+
+}
diff --git a/boards/LightRing/board.h b/boards/LightRing/board.h
new file mode 100644
index 0000000..dbbb8ed
--- /dev/null
+++ b/boards/LightRing/board.h
@@ -0,0 +1,187 @@
+#ifndef _BOARD_H_
+#define _BOARD_H_
+
+/*
+ * Setup for AMiRo LightRing board.
+ */
+
+/*
+ * Board identifier.
+ */
+#define BOARD_LIGHT_RING
+#define BOARD_NAME              "AMiRo LightRing"
+#define BOARD_VERSION           "1.0"
+
+/*
+ * Board frequencies.
+ */
+#define STM32_LSECLK            0
+#define STM32_HSECLK            8000000
+
+/*
+ * MCU type as defined in the ST header file stm32f1xx.h.
+ */
+#define STM32F10X_HD
+
+/*
+ * IO pins assignments.
+ */
+#define GPIOA_LASER_RX          2
+#define GPIOA_LASER_TX          3
+#define GPIOA_LIGHT_BLANK       4
+#define GPIOA_LIGHT_SCLK        5
+#define GPIOA_LIGHT_MOSI        7
+#define GPIOA_PROG_RX           9
+#define GPIOA_PROG_TX           10
+#define GPIOA_CAN_RX            11
+#define GPIOA_CAN_TX            12
+
+#define GPIOB_LASER_EN          2
+#define GPIOB_LASER_OC_N        5
+#define GPIOB_SYS_UART_DN       6
+#define GPIOB_WL_GDO2           8
+#define GPIOB_WL_GDO0           9
+#define GPIOB_MEM_SCL           10
+#define GPIOB_MEM_SDA           11
+#define GPIOB_WL_SS_N           12
+#define GPIOB_WL_SCLK           13
+#define GPIOB_WL_MISO           14
+#define GPIOB_WL_MOSI           15
+
+#define GPIOC_LIGHT_XLAT        4
+#define GPIOC_SYS_UART_RX       10
+#define GPIOC_SYS_UART_TX       11
+#define GPIOC_SYS_PD_N          14
+
+#define GPIOD_OSC_IN            0
+#define GPIOD_OSC_OUT           1
+#define GPIOD_SYS_INT_N         2
+
+/*
+ * I/O ports initial setup, this configuration is established soon after reset
+ * in the initialization code.
+ */
+#define PIN_MODE_INPUT(n)               (0x4U << (((n) % 8) * 4))
+#define PIN_MODE_INPUT_PULLX(n)         (0x8U << (((n) % 8) * 4))
+#define PIN_MODE_INPUT_ANALOG(n)        (0x0U << (((n) % 8) * 4))
+/* Push Pull output 50MHz */
+#define PIN_MODE_OUTPUT_PUSHPULL(n)     (0x3U << (((n) % 8) * 4))
+/* Open Drain output 50MHz */
+#define PIN_MODE_OUTPUT_OPENDRAIN(n)    (0x7U << (((n) % 8) * 4))
+/* Alternate Push Pull output 50MHz */
+#define PIN_MODE_ALTERNATE_PUSHPULL(n)  (0xbU << (((n) % 8) * 4))
+/* Alternate Open Drain output 50MHz */
+#define PIN_MODE_ALTERNATE_OPENDRAIN(n) (0xfU << (((n) % 8) * 4))
+
+/*
+ * Port A setup.
+ */
+#define VAL_GPIOACRL            (PIN_MODE_INPUT_PULLX(0) | \
+                                 PIN_MODE_INPUT_PULLX(1) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_LASER_RX) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOA_LASER_TX) | \
+                                 PIN_MODE_OUTPUT_PUSHPULL(GPIOA_LIGHT_BLANK) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_LIGHT_SCLK) | \
+                                 PIN_MODE_INPUT_PULLX(6) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_LIGHT_MOSI))
+#define VAL_GPIOACRH            (PIN_MODE_INPUT_PULLX(8) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_PROG_RX) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOA_PROG_TX) | \
+                                 PIN_MODE_INPUT(GPIOA_CAN_RX) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOA_CAN_TX) | \
+                                 PIN_MODE_INPUT_PULLX(13) | \
+                                 PIN_MODE_INPUT_PULLX(14) | \
+                                 PIN_MODE_INPUT_PULLX(15))
+#define VAL_GPIOAODR            0xFFFF
+
+/*
+ * Port B setup.
+ */
+#define VAL_GPIOBCRL            (PIN_MODE_INPUT_PULLX(0) | \
+                                 PIN_MODE_INPUT_PULLX(1) | \
+                                 PIN_MODE_OUTPUT_PUSHPULL(GPIOB_LASER_EN) | \
+                                 PIN_MODE_INPUT_PULLX(3) | \
+                                 PIN_MODE_INPUT_PULLX(4) | \
+                                 PIN_MODE_INPUT(GPIOB_LASER_OC_N) | \
+                                 PIN_MODE_OUTPUT_OPENDRAIN(GPIOB_SYS_UART_DN) | \
+                                 PIN_MODE_INPUT_PULLX(7))
+#define VAL_GPIOBCRH            (PIN_MODE_INPUT_PULLX(GPIOB_WL_GDO2) | \
+                                 PIN_MODE_INPUT_PULLX(GPIOB_WL_GDO0) | \
+                                 PIN_MODE_ALTERNATE_OPENDRAIN(GPIOB_MEM_SCL) | \
+                                 PIN_MODE_ALTERNATE_OPENDRAIN(GPIOB_MEM_SDA) | \
+                                 PIN_MODE_OUTPUT_PUSHPULL(GPIOB_WL_SS_N) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOB_WL_SCLK) | \
+                                 PIN_MODE_INPUT(GPIOB_WL_MISO) | \
+                                 PIN_MODE_ALTERNATE_PUSHPULL(GPIOB_WL_MOSI))
+#define VAL_GPIOBODR            0xFFFB /* initially LASER_EN is deactivated */
+
+/*
+ * Port C setup.
+ */
+#define VAL_GPIOCCRL            (PIN_MODE_INPUT_PULLX(0) | \
+                                 PIN_MODE_INPUT_PULLX(1) | \
+                                 PIN_MODE_INPUT_PULLX(2) | \
+                                 PIN_MODE_INPUT_PULLX(3) | \
+                                 PIN_MODE_OUTPUT_PUSHPULL(GPIOC_LIGHT_XLAT) | \
+                                 PIN_MODE_INPUT_PULLX(5) | \
+                                 PIN_MODE_INPUT_PULLX(6) | \
+                                 PIN_MODE_INPUT_PULLX(7))
+#define VAL_GPIOCCRH            (PIN_MODE_INPUT_PULLX(8) | \
+                                 PIN_MODE_INPUT_PULLX(9) | \
+                                 PIN_MODE_INPUT(GPIOC_SYS_UART_RX) | \
+                                 PIN_MODE_INPUT(GPIOC_SYS_UART_TX) | \
+                                 PIN_MODE_INPUT_PULLX(12) | \
+                                 PIN_MODE_INPUT_PULLX(13) | \
+                                 PIN_MODE_OUTPUT_OPENDRAIN(GPIOC_SYS_PD_N) | \
+                                 PIN_MODE_INPUT_PULLX(15))
+#define VAL_GPIOCODR            0xFFFF
+
+/*
+ * Port D setup.
+ */
+#define VAL_GPIODCRL            (PIN_MODE_INPUT(GPIOD_OSC_IN) | \
+                                 PIN_MODE_INPUT(GPIOD_OSC_OUT) | \
+                                 PIN_MODE_OUTPUT_OPENDRAIN(GPIOD_SYS_INT_N) | \
+                                 PIN_MODE_INPUT_PULLX(3) | \
+                                 PIN_MODE_INPUT_PULLX(4) | \
+                                 PIN_MODE_INPUT_PULLX(5) | \
+                                 PIN_MODE_INPUT_PULLX(6) | \
+                                 PIN_MODE_INPUT_PULLX(7))
+#define VAL_GPIODCRH            0x88888888
+#define VAL_GPIODODR            0xFFFB /* initially SYS_INT_N indicates that the OS is busy */
+
+/*
+ * Port E setup.
+ */
+#define VAL_GPIOECRL            0x88888888 /*  PE7...PE0 */
+#define VAL_GPIOECRH            0x88888888 /* PE15...PE8 */
+#define VAL_GPIOEODR            0xFFFF
+
+/*
+ * Port F setup.
+ */
+#define VAL_GPIOFCRL            0x88888888 /*  PF7...PF0 */
+#define VAL_GPIOFCRH            0x88888888 /* PF15...PF8 */
+#define VAL_GPIOFODR            0xFFFF
+
+/*
+ * Port G setup.
+ */
+#define VAL_GPIOGCRL            0x88888888 /*  PG7...PG0 */
+#define VAL_GPIOGCRH            0x88888888 /* PG15...PG8 */
+#define VAL_GPIOGODR            0xFFFF
+
+#if !defined(_FROM_ASM_)
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void boardInit(void);
+  void boardRequestShutdown(void);
+  void boardStandby(void);
+  void boardClearI2CBus(const uint8_t scl_pad);
+#ifdef __cplusplus
+}
+#endif
+#endif /* _FROM_ASM_ */
+
+#endif /* _BOARD_H_ */
diff --git a/boards/LightRing/board.mk b/boards/LightRing/board.mk
new file mode 100644
index 0000000..f8f1d7a
--- /dev/null
+++ b/boards/LightRing/board.mk
@@ -0,0 +1,9 @@
+# List of all the board related files.
+BOARDSRC = ${AMIRO}/boards/LightRing/board.c
+
+# Required include directories
+BOARDINC = ${AMIRO}/boards/LightRing
+
+# Required linker directory
+BOARDLD = ${AMIRO}/ports
+
diff --git a/boards/PowerManagement/board.c b/boards/PowerManagement/board.c
new file mode 100644
index 0000000..e596128
--- /dev/null
+++ b/boards/PowerManagement/board.c
@@ -0,0 +1,167 @@
+#include "ch.h"
+#include "hal.h"
+#include "board.h"
+
+/**
+ * @brief   PAL setup.
+ * @details Digital I/O ports static configuration as defined in @p board.h.
+ *          This variable is used by the HAL when initializing the PAL driver.
+ */
+#if HAL_USE_PAL || defined(__DOXYGEN__)
+const PALConfig pal_default_config =
+{
+  {VAL_GPIOA_MODER, VAL_GPIOA_OTYPER, VAL_GPIOA_OSPEEDR, VAL_GPIOA_PUPDR, VAL_GPIOA_ODR, VAL_GPIOA_AFRL, VAL_GPIOA_AFRH},
+  {VAL_GPIOB_MODER, VAL_GPIOB_OTYPER, VAL_GPIOB_OSPEEDR, VAL_GPIOB_PUPDR, VAL_GPIOB_ODR, VAL_GPIOB_AFRL, VAL_GPIOB_AFRH},
+  {VAL_GPIOC_MODER, VAL_GPIOC_OTYPER, VAL_GPIOC_OSPEEDR, VAL_GPIOC_PUPDR, VAL_GPIOC_ODR, VAL_GPIOC_AFRL, VAL_GPIOC_AFRH},
+  {VAL_GPIOD_MODER, VAL_GPIOD_OTYPER, VAL_GPIOD_OSPEEDR, VAL_GPIOD_PUPDR, VAL_GPIOD_ODR, VAL_GPIOD_AFRL, VAL_GPIOD_AFRH},
+  {VAL_GPIOE_MODER, VAL_GPIOE_OTYPER, VAL_GPIOE_OSPEEDR, VAL_GPIOE_PUPDR, VAL_GPIOE_ODR, VAL_GPIOE_AFRL, VAL_GPIOE_AFRH},
+  {VAL_GPIOF_MODER, VAL_GPIOF_OTYPER, VAL_GPIOF_OSPEEDR, VAL_GPIOF_PUPDR, VAL_GPIOF_ODR, VAL_GPIOF_AFRL, VAL_GPIOF_AFRH},
+  {VAL_GPIOG_MODER, VAL_GPIOG_OTYPER, VAL_GPIOG_OSPEEDR, VAL_GPIOG_PUPDR, VAL_GPIOG_ODR, VAL_GPIOG_AFRL, VAL_GPIOG_AFRH},
+  {VAL_GPIOH_MODER, VAL_GPIOH_OTYPER, VAL_GPIOH_OSPEEDR, VAL_GPIOH_PUPDR, VAL_GPIOH_ODR, VAL_GPIOH_AFRL, VAL_GPIOH_AFRH},
+  {VAL_GPIOI_MODER, VAL_GPIOI_OTYPER, VAL_GPIOI_OSPEEDR, VAL_GPIOI_PUPDR, VAL_GPIOI_ODR, VAL_GPIOI_AFRL, VAL_GPIOI_AFRH}
+};
+#endif
+
+/*
+ * Early initialization code.
+ * This initialization must be performed just after stack setup and before
+ * any other initialization.
+ */
+void __early_init(void) {
+
+  stm32_clock_init();
+}
+
+/*
+ * Board-specific initialization code.
+ */
+void boardInit(void) {
+
+}
+
+inline void boardWriteIoPower(int value)
+{
+    palWritePad(GPIOA, GPIOA_SYS_REG_EN, value);
+    if (value) {
+        // drive pins
+        palSetPadMode(GPIOA, GPIOA_CAN_TX, PAL_MODE_ALTERNATE(9));
+        palSetPadMode(GPIOA, GPIOA_SYS_UART_TX, PAL_MODE_ALTERNATE(7));
+        palSetPadMode(GPIOB, GPIOB_BT_CTS, PAL_MODE_ALTERNATE(7));
+    } else {
+        // float pins
+        palSetPadMode(GPIOA, GPIOA_CAN_TX, PAL_MODE_INPUT);
+        palSetPadMode(GPIOA, GPIOA_SYS_UART_TX, PAL_MODE_INPUT);
+        palSetPadMode(GPIOB, GPIOB_BT_CTS, PAL_MODE_INPUT);
+    }
+    chThdSleepMilliseconds(50);
+}
+
+inline void boardWriteLed(int value)
+{
+    palWritePad(GPIOB, GPIOB_LED, !value);
+}
+
+inline void boardWriteSystemPower(int value)
+{
+    palWritePad(GPIOB, GPIOB_POWER_EN, value);
+    chThdSleepMilliseconds(50);
+}
+
+inline void boardWriteWarmRestart(const uint8_t value)
+{
+    palWritePad(GPIOC, GPIOC_SYS_WARMRST_N, ~value);
+    chThdSleepMilliseconds(50);
+}
+
+inline void boardChargerSetState(uint8_t chrg_mask, uint8_t state)
+{
+  if (chrg_mask & (1u << 0))
+    palWritePad(GPIOC, GPIOC_CHARGE_EN1_N, ~state);
+  if (chrg_mask & (1u << 1))
+    palWritePad(GPIOD, GPIOD_CHARGE_EN2_N, ~state);
+}
+
+inline void boardBluetoothSetState(uint8_t state)
+{
+    palWritePad(GPIOC, GPIOC_BT_RST, ~state);
+}
+
+inline void boardRequestShutdown(void)
+{
+  palClearPad(GPIOC, GPIOC_SYS_PD_N);
+}
+
+#define RTC_ISR_TAMP2F ((uint32_t)0x00004000)
+
+inline void boardStandby(void)
+{
+
+  chSysLock();
+  // set deepsleep bit
+  SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
+  // enable wakeup pin
+  PWR->CSR |= PWR_CSR_EWUP;
+  // set PDDS, clear WUF, clear SBF
+  PWR->CR |= (PWR_CR_CWUF | PWR_CR_PDDS | PWR_CR_CSBF);
+  // clear RTC wakeup source flags
+  RTC->ISR &= ~(RTC_ISR_ALRBF | RTC_ISR_ALRAF | RTC_ISR_WUTF | RTC_ISR_TAMP1F | RTC_ISR_TAMP2F |
+                RTC_ISR_TSOVF | RTC_ISR_TSF);
+  // Wait for Interrupt
+  __WFI();
+
+}
+
+inline void boardStop(const uint8_t lpds, const uint8_t fpds)
+{
+
+  chSysLock();
+  // set deepsleep bit
+  SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
+  // enable wakeup pin
+  //PWR->CSR |= PWR_CSR_EWUP;
+  // clear PDDS, clear LPDS, clear FPDS
+  PWR->CR &= ~(PWR_CR_PDDS | PWR_CR_LPDS | PWR_CR_FPDS);
+  // clear WUF, clear SBF
+  PWR->CR |= (PWR_CR_CWUF | PWR_CR_CSBF);
+  if (lpds)
+    PWR->CR |= (PWR_CR_LPDS);
+  if (fpds)
+    PWR->CR |= (PWR_CR_FPDS);
+  // clear RTC wakeup source flags
+  RTC->ISR &= ~(RTC_ISR_ALRBF | RTC_ISR_ALRAF | RTC_ISR_WUTF | RTC_ISR_TAMP1F | RTC_ISR_TAMP2F |
+                RTC_ISR_TSOVF | RTC_ISR_TSF);
+  // clear pending interrupts
+  EXTI->PR = ~0;
+  // Wait for Interrupt
+  __WFI();
+
+}
+
+#undef RTC_ISR_TAMP2F
+
+inline void boardWakeup(void) {
+
+  palClearPad(GPIOC, GPIOC_SYS_PD_N);
+  chThdSleepMicroseconds(10);
+  palSetPad(GPIOC, GPIOC_SYS_PD_N);
+}
+
+inline void boardClearI2CBus(const uint8_t scl_pad) {
+
+  uint8_t i;
+
+  // configure I²C SCL open drain
+  palSetPadMode(GPIOB, scl_pad, PAL_MODE_OUTPUT_OPENDRAIN);
+
+  // perform bus clear as per I²C Specification v5 3.1.16
+  for (i = 0x00u; i < 0x09u; i++) {
+    palClearPad(GPIOB, scl_pad);
+    chThdSleepMicroseconds(5);
+    palSetPad(GPIOB, scl_pad);
+    chThdSleepMicroseconds(5);
+  }
+
+  // reconfigure I²C SCL
+  palSetPadMode(GPIOB, scl_pad, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);
+
+}
diff --git a/boards/PowerManagement/board.h b/boards/PowerManagement/board.h
new file mode 100644
index 0000000..54a72cd
--- /dev/null
+++ b/boards/PowerManagement/board.h
@@ -0,0 +1,384 @@
+#ifndef _BOARD_H_
+#define _BOARD_H_
+
+/*
+ * Setup for AMiRo PowerManagement board.
+ */
+
+/*
+ * Board identifier.
+ */
+#define BOARD_POWER_MANAGEMENT
+#define BOARD_NAME              "AMiRo PowerManagement"
+#define BOARD_VERSION           "1.1"
+
+/*
+ * Board frequencies.
+ */
+#define STM32_LSECLK            0
+#define STM32_HSECLK            8000000
+
+/*
+ * Board voltages.
+ * Required for performance limits calculation.
+ */
+#define STM32_VDD               330
+
+/*
+ * MCU type as defined in the ST header file stm32f4xx.h.
+ */
+#define STM32F40_41xxx
+
+/*
+ * IO pins assignments.
+ */
+#define GPIOA_WKUP              0
+#define GPIOA_SYS_UART_TX       2
+#define GPIOA_SYS_UART_RX       3
+#define GPIOA_SYS_SPI_SS0_N     4
+#define GPIOA_SYS_SPI_SCLK      5
+#define GPIOA_SYS_SPI_MISO      6
+#define GPIOA_SYS_SPI_MOSI      7
+#define GPIOA_SYS_REG_EN        8
+#define GPIOA_PROG_RX           9
+#define GPIOA_PROG_TX           10
+#define GPIOA_CAN_RX            11
+#define GPIOA_CAN_TX            12
+#define GPIOA_SYS_SPI_SS1_N     15
+
+#define GPIOB_IR_INT1_N         0
+#define GPIOB_VSYS_SENSE        1
+#define GPIOB_POWER_EN          2
+#define GPIOB_SYS_UART_DN       3
+#define GPIOB_CHARGE_STAT2A     4
+#define GPIOB_BUZZER            5
+#define GPIOB_GAUGE_BATLOW2     6
+#define GPIOB_GAUGE_BATGD2_N    7
+#define GPIOB_GAUGE_SCL2        8
+#define GPIOB_GAUGE_SDA2        9
+#define GPIOB_GAUGE_SCL1        10
+#define GPIOB_GAUGE_SDA1        11
+#define GPIOB_LED               12
+#define GPIOB_BT_RTS            13
+#define GPIOB_BT_CTS            14
+#define GPIOB_SYS_UART_UP       15
+
+#define GPIOC_CHARGE_STAT1A     0
+#define GPIOC_GAUGE_BATLOW1     1
+#define GPIOC_GAUGE_BATGD1_N    2
+#define GPIOC_CHARGE_EN1_N      3
+#define GPIOC_IR_INT2_N         4
+#define GPIOC_TOUCH_INT_N       5
+#define GPIOC_SYS_DONE          6
+#define GPIOC_SYS_PROG_N        7
+#define GPIOC_PATH_DC           8
+#define GPIOC_SYS_SPI_DIR       9
+#define GPIOC_BT_RX             10
+#define GPIOC_BT_TX             11
+#define GPIOC_SYS_INT_N         12
+#define GPIOC_SYS_PD_N          13
+#define GPIOC_SYS_WARMRST_N     14
+#define GPIOC_BT_RST            15
+
+#define GPIOD_CHARGE_EN2_N      2
+
+#define GPIOH_OSC_IN            0
+#define GPIOH_OSC_OUT           1
+
+/*
+ * I/O ports initial setup, this configuration is established soon after reset
+ * in the initialization code.
+ */
+#define PIN_MODE_INPUT(n)       (0U << ((n) * 2))
+#define PIN_MODE_OUTPUT(n)      (1U << ((n) * 2))
+#define PIN_MODE_ALTERNATE(n)   (2U << ((n) * 2))
+#define PIN_MODE_ANALOG(n)      (3U << ((n) * 2))
+#define PIN_OTYPE_PUSHPULL(n)   (0U << (n))
+#define PIN_OTYPE_OPENDRAIN(n)  (1U << (n))
+#define PIN_OSPEED_2M(n)        (0U << ((n) * 2))
+#define PIN_OSPEED_25M(n)       (1U << ((n) * 2))
+#define PIN_OSPEED_50M(n)       (2U << ((n) * 2))
+#define PIN_OSPEED_100M(n)      (3U << ((n) * 2))
+#define PIN_PUDR_FLOATING(n)    (0U << ((n) * 2))
+#define PIN_PUDR_PULLUP(n)      (1U << ((n) * 2))
+#define PIN_PUDR_PULLDOWN(n)    (2U << ((n) * 2))
+#define PIN_AFIO_AF(n, v)       ((v##U) << ((n % 8) * 4))
+
+/*
+ * Port A setup.
+ */
+#define VAL_GPIOA_MODER         (PIN_MODE_INPUT(GPIOA_WKUP) | \
+                                 PIN_MODE_INPUT(1) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_SYS_UART_TX) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_SYS_UART_RX) | \
+                                 PIN_MODE_INPUT(GPIOA_SYS_SPI_SS0_N) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_SYS_SPI_SCLK) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_SYS_SPI_MOSI) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_SYS_SPI_MISO) | \
+                                 PIN_MODE_INPUT(GPIOA_SYS_SPI_SS1_N) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_PROG_RX) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_PROG_TX) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_CAN_RX) | \
+                                 PIN_MODE_ALTERNATE(GPIOA_CAN_TX) | \
+                                 PIN_MODE_INPUT(13) | \
+                                 PIN_MODE_INPUT(14) | \
+                                 PIN_MODE_OUTPUT(GPIOA_SYS_REG_EN))
+#define VAL_GPIOA_OTYPER        (PIN_OTYPE_PUSHPULL(GPIOA_SYS_UART_TX) | \
+                                 PIN_OTYPE_PUSHPULL(GPIOA_SYS_SPI_SS0_N) | \
+                                 PIN_OTYPE_PUSHPULL(GPIOA_PROG_RX) | \
+                                 PIN_OTYPE_PUSHPULL(GPIOA_CAN_TX) | \
+                                 PIN_OTYPE_PUSHPULL(GPIOA_SYS_REG_EN))
+#define VAL_GPIOA_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOA_PUPDR         (PIN_PUDR_FLOATING(GPIOA_WKUP) | \
+                                 PIN_PUDR_PULLUP(1) | \
+                                 PIN_PUDR_FLOATING(GPIOA_SYS_UART_TX) | \
+                                 PIN_PUDR_FLOATING(GPIOA_SYS_UART_RX) | \
+                                 PIN_PUDR_FLOATING(GPIOA_SYS_SPI_SS0_N) | \
+                                 PIN_PUDR_FLOATING(GPIOA_SYS_SPI_SCLK) | \
+                                 PIN_PUDR_FLOATING(GPIOA_SYS_SPI_MOSI) | \
+                                 PIN_PUDR_FLOATING(GPIOA_SYS_SPI_MISO) | \
+                                 PIN_PUDR_FLOATING(GPIOA_SYS_SPI_SS1_N) | \
+                                 PIN_PUDR_FLOATING(GPIOA_PROG_RX) | \
+                                 PIN_PUDR_PULLUP(GPIOA_PROG_TX) | \
+                                 PIN_PUDR_FLOATING(GPIOA_CAN_RX) | \
+                                 PIN_PUDR_FLOATING(GPIOA_CAN_TX) | \
+                                 PIN_PUDR_PULLUP(13) | \
+                                 PIN_PUDR_PULLUP(14) | \
+                                 PIN_PUDR_FLOATING(GPIOA_SYS_REG_EN))
+
+#define VAL_GPIOA_ODR           0xFFFF
+#define VAL_GPIOA_AFRL          (PIN_AFIO_AF(GPIOA_SYS_UART_TX, 7) | \
+                                 PIN_AFIO_AF(GPIOA_SYS_UART_RX, 7) | \
+                                 PIN_AFIO_AF(GPIOA_SYS_SPI_SCLK, 5) | \
+                                 PIN_AFIO_AF(GPIOA_SYS_SPI_MISO, 5) | \
+                                 PIN_AFIO_AF(GPIOA_SYS_SPI_MOSI, 5))
+#define VAL_GPIOA_AFRH          (PIN_AFIO_AF(GPIOA_PROG_RX, 7) | \
+                                 PIN_AFIO_AF(GPIOA_PROG_TX, 7) | \
+                                 PIN_AFIO_AF(GPIOA_CAN_RX, 9) | \
+                                 PIN_AFIO_AF(GPIOA_CAN_TX, 9))
+
+/*
+ * Port B setup.
+ */
+#define VAL_GPIOB_MODER         (PIN_MODE_INPUT(GPIOB_IR_INT1_N) | \
+                                 PIN_MODE_ANALOG(GPIOB_VSYS_SENSE) | \
+                                 PIN_MODE_OUTPUT(GPIOB_POWER_EN) | \
+                                 PIN_MODE_OUTPUT(GPIOB_SYS_UART_DN) | \
+                                 PIN_MODE_INPUT(GPIOB_CHARGE_STAT2A) | \
+                                 PIN_MODE_ALTERNATE(GPIOB_BUZZER) | \
+                                 PIN_MODE_INPUT(GPIOB_GAUGE_BATLOW2) | \
+                                 PIN_MODE_INPUT(GPIOB_GAUGE_BATGD2_N) | \
+                                 PIN_MODE_ALTERNATE(GPIOB_GAUGE_SCL2) | \
+                                 PIN_MODE_ALTERNATE(GPIOB_GAUGE_SDA2) | \
+                                 PIN_MODE_ALTERNATE(GPIOB_GAUGE_SCL1) | \
+                                 PIN_MODE_ALTERNATE(GPIOB_GAUGE_SDA1) | \
+                                 PIN_MODE_OUTPUT(GPIOB_LED) | \
+                                 PIN_MODE_ALTERNATE(GPIOB_BT_RTS) | \
+                                 PIN_MODE_ALTERNATE(GPIOB_BT_CTS) | \
+                                 PIN_MODE_OUTPUT(GPIOB_SYS_UART_UP))
+#define VAL_GPIOB_OTYPER        (PIN_OTYPE_PUSHPULL(GPIOB_POWER_EN) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOB_SYS_UART_DN) | \
+                                 PIN_OTYPE_PUSHPULL(GPIOB_BUZZER) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOB_GAUGE_SCL2) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOB_GAUGE_SDA2) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOB_GAUGE_SCL1) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOB_GAUGE_SDA1) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOB_LED) | \
+                                 PIN_OTYPE_PUSHPULL(GPIOB_BT_CTS) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOB_SYS_UART_UP))
+#define VAL_GPIOB_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOB_PUPDR         0x00000000
+#define VAL_GPIOB_ODR           0xFFFF
+#define VAL_GPIOB_AFRL          (PIN_AFIO_AF(GPIOB_BUZZER, 2))
+#define VAL_GPIOB_AFRH          (PIN_AFIO_AF(GPIOB_GAUGE_SCL2, 4) | \
+                                 PIN_AFIO_AF(GPIOB_GAUGE_SDA2, 4) | \
+                                 PIN_AFIO_AF(GPIOB_GAUGE_SCL1, 4) | \
+                                 PIN_AFIO_AF(GPIOB_GAUGE_SDA1, 4) | \
+                                 PIN_AFIO_AF(GPIOB_BT_RTS, 7) | \
+                                 PIN_AFIO_AF(GPIOB_BT_CTS, 7))
+
+/*
+ * Port C setup.
+ */
+#define VAL_GPIOC_MODER         (PIN_MODE_INPUT(GPIOC_CHARGE_STAT1A) | \
+                                 PIN_MODE_INPUT(GPIOC_GAUGE_BATLOW1) | \
+                                 PIN_MODE_INPUT(GPIOC_GAUGE_BATGD1_N) | \
+                                 PIN_MODE_OUTPUT(GPIOC_CHARGE_EN1_N) | \
+                                 PIN_MODE_INPUT(GPIOC_IR_INT2_N) | \
+                                 PIN_MODE_INPUT(GPIOC_TOUCH_INT_N) | \
+                                 PIN_MODE_INPUT(GPIOC_SYS_DONE) | \
+                                 PIN_MODE_OUTPUT(GPIOC_SYS_PROG_N) | \
+                                 PIN_MODE_INPUT(GPIOC_PATH_DC) | \
+                                 PIN_MODE_OUTPUT(GPIOC_SYS_SPI_DIR) | \
+                                 PIN_MODE_ALTERNATE(GPIOC_BT_RX) | \
+                                 PIN_MODE_ALTERNATE(GPIOC_BT_TX) | \
+                                 PIN_MODE_OUTPUT(GPIOC_SYS_INT_N) | \
+                                 PIN_MODE_OUTPUT(GPIOC_SYS_PD_N) | \
+                                 PIN_MODE_OUTPUT(GPIOC_SYS_WARMRST_N) | \
+                                 PIN_MODE_OUTPUT(GPIOC_BT_RST))
+#define VAL_GPIOC_OTYPER        (PIN_OTYPE_OPENDRAIN(GPIOC_CHARGE_EN1_N) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOC_SYS_PROG_N) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOC_SYS_SPI_DIR) | \
+                                 PIN_OTYPE_PUSHPULL(GPIOC_BT_RX) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOC_SYS_INT_N) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOC_SYS_PD_N) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOC_SYS_WARMRST_N) | \
+                                 PIN_OTYPE_OPENDRAIN(GPIOC_BT_RST))
+#define VAL_GPIOC_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOC_PUPDR         0x00000000
+#define VAL_GPIOC_ODR           0xEEFF /* emulate open drain for PATH_DC. This is required to prevent accidental shortcuts. Furthermore, pull down SYS_INT_N to indicate the OS is starting */
+#define VAL_GPIOC_AFRL          0x00000000
+#define VAL_GPIOC_AFRH          (PIN_AFIO_AF(GPIOC_BT_RX, 7) | \
+                                 PIN_AFIO_AF(GPIOC_BT_TX, 7))
+
+/*
+ * Port D setup.
+ */
+#define VAL_GPIOD_MODER         (PIN_MODE_INPUT(0) | \
+                                 PIN_MODE_INPUT(1) | \
+                                 PIN_MODE_OUTPUT(GPIOD_CHARGE_EN2_N) | \
+                                 PIN_MODE_INPUT(3) | \
+                                 PIN_MODE_INPUT(4) | \
+                                 PIN_MODE_INPUT(5) | \
+                                 PIN_MODE_INPUT(6) | \
+                                 PIN_MODE_INPUT(7) | \
+                                 PIN_MODE_INPUT(8) | \
+                                 PIN_MODE_INPUT(9) | \
+                                 PIN_MODE_INPUT(10) | \
+                                 PIN_MODE_INPUT(11) | \
+                                 PIN_MODE_INPUT(12) | \
+                                 PIN_MODE_INPUT(13) | \
+                                 PIN_MODE_INPUT(14) | \
+                                 PIN_MODE_INPUT(15))
+#define VAL_GPIOD_OTYPER        (PIN_OTYPE_OPENDRAIN(GPIOD_CHARGE_EN2_N))
+#define VAL_GPIOD_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOD_PUPDR         (PIN_PUDR_PULLUP(0) | \
+                                 PIN_PUDR_PULLUP(1) | \
+                                 PIN_PUDR_FLOATING(GPIOD_CHARGE_EN2_N) | \
+                                 PIN_PUDR_PULLUP(3) | \
+                                 PIN_PUDR_PULLUP(4) | \
+                                 PIN_PUDR_PULLUP(5) | \
+                                 PIN_PUDR_PULLUP(6) | \
+                                 PIN_PUDR_PULLUP(7) | \
+                                 PIN_PUDR_PULLUP(8) | \
+                                 PIN_PUDR_PULLUP(9) | \
+                                 PIN_PUDR_PULLUP(10) | \
+                                 PIN_PUDR_PULLUP(11) | \
+                                 PIN_PUDR_PULLUP(12) | \
+                                 PIN_PUDR_PULLUP(13) | \
+                                 PIN_PUDR_PULLUP(14) | \
+                                 PIN_PUDR_PULLUP(15))
+#define VAL_GPIOD_ODR           0x0FFF
+#define VAL_GPIOD_AFRL          0x00000000
+#define VAL_GPIOD_AFRH          0x00000000
+
+/*
+ * Port E setup.
+ */
+#define VAL_GPIOE_MODER         0x00000000
+#define VAL_GPIOE_OTYPER        0x00000000
+#define VAL_GPIOE_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOE_PUPDR         0xFFFFFFFF
+#define VAL_GPIOE_ODR           0xFFFF
+#define VAL_GPIOE_AFRL          0x00000000
+#define VAL_GPIOE_AFRH          0x00000000
+
+/*
+ * Port F setup.
+ * All input with pull-up.
+ */
+#define VAL_GPIOF_MODER         0x00000000
+#define VAL_GPIOF_OTYPER        0x00000000
+#define VAL_GPIOF_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOF_PUPDR         0xFFFFFFFF
+#define VAL_GPIOF_ODR           0xFFFF
+#define VAL_GPIOF_AFRL          0x00000000
+#define VAL_GPIOF_AFRH          0x00000000
+
+/*
+ * Port G setup.
+ * All input with pull-up.
+ */
+#define VAL_GPIOG_MODER         0x00000000
+#define VAL_GPIOG_OTYPER        0x00000000
+#define VAL_GPIOG_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOG_PUPDR         0xFFFFFFFF
+#define VAL_GPIOG_ODR           0xFFFF
+#define VAL_GPIOG_AFRL          0x00000000
+#define VAL_GPIOG_AFRH          0x00000000
+
+/*
+ * Port H setup.
+ */
+#define VAL_GPIOH_MODER         (PIN_MODE_INPUT(GPIOH_OSC_IN) | \
+                                 PIN_MODE_INPUT(GPIOH_OSC_OUT) | \
+                                 PIN_MODE_INPUT(2) | \
+                                 PIN_MODE_INPUT(3) | \
+                                 PIN_MODE_INPUT(4) | \
+                                 PIN_MODE_INPUT(5) | \
+                                 PIN_MODE_INPUT(6) | \
+                                 PIN_MODE_INPUT(7) | \
+                                 PIN_MODE_INPUT(8) | \
+                                 PIN_MODE_INPUT(9) | \
+                                 PIN_MODE_INPUT(10) | \
+                                 PIN_MODE_INPUT(11) | \
+                                 PIN_MODE_INPUT(12) | \
+                                 PIN_MODE_INPUT(13) | \
+                                 PIN_MODE_INPUT(14) | \
+                                 PIN_MODE_INPUT(15))
+#define VAL_GPIOH_OTYPER        0x00000000
+#define VAL_GPIOH_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOH_PUPDR         (PIN_PUDR_FLOATING(GPIOH_OSC_IN) | \
+                                 PIN_PUDR_FLOATING(GPIOH_OSC_OUT) | \
+                                 PIN_PUDR_PULLUP(2) | \
+                                 PIN_PUDR_PULLUP(3) | \
+                                 PIN_PUDR_PULLUP(4) | \
+                                 PIN_PUDR_PULLUP(5) | \
+                                 PIN_PUDR_PULLUP(6) | \
+                                 PIN_PUDR_PULLUP(7) | \
+                                 PIN_PUDR_PULLUP(8) | \
+                                 PIN_PUDR_PULLUP(9) | \
+                                 PIN_PUDR_PULLUP(10) | \
+                                 PIN_PUDR_PULLUP(11) | \
+                                 PIN_PUDR_PULLUP(12) | \
+                                 PIN_PUDR_PULLUP(13) | \
+                                 PIN_PUDR_PULLUP(14) | \
+                                 PIN_PUDR_PULLUP(15))
+#define VAL_GPIOH_ODR           0xFFFF
+#define VAL_GPIOH_AFRL          0x00000000
+#define VAL_GPIOH_AFRH          0x00000000
+
+/*
+ * Port I setup.
+ * All input with pull-up.
+ */
+#define VAL_GPIOI_MODER         0x00000000
+#define VAL_GPIOI_OTYPER        0x00000000
+#define VAL_GPIOI_OSPEEDR       0xFFFFFFFF
+#define VAL_GPIOI_PUPDR         0xFFFFFFFF
+#define VAL_GPIOI_ODR           0xFFFF
+#define VAL_GPIOI_AFRL          0x00000000
+#define VAL_GPIOI_AFRH          0x00000000
+
+#if !defined(_FROM_ASM_)
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void boardInit(void);
+  void boardWriteIoPower(int value);
+  void boardWriteLed(int value);
+  void boardWriteSystemPower(int value);
+  void boardWriteWarmRestart(const uint8_t value);
+  void boardChargerSetState(uint8_t chrg_mask, uint8_t state);
+  void boardBluetoothSetState(uint8_t state);
+  void boardRequestShutdown(void);
+  void boardStandby(void);
+  void boardStop(const uint8_t lpds, const uint8_t fpds);
+  void boardWakeup(void);
+  void boardClearI2CBus(const uint8_t scl_pad);
+#ifdef __cplusplus
+}
+#endif
+#endif /* _FROM_ASM_ */
+
+#endif /* _BOARD_H_ */
diff --git a/boards/PowerManagement/board.mk b/boards/PowerManagement/board.mk
new file mode 100644
index 0000000..6988c4f
--- /dev/null
+++ b/boards/PowerManagement/board.mk
@@ -0,0 +1,10 @@
+# List of all the board related files.
+BOARDSRC = ${AMIRO}/boards/PowerManagement/board.c \
+           ${AMIRO}/boards/PowerManagement/wakeup.c
+
+# Required include directories
+BOARDINC = ${AMIRO}/boards/PowerManagement
+
+# Required linker directory
+BOARDLD = ${AMIRO}/ports
+
diff --git a/boards/PowerManagement/wakeup.c b/boards/PowerManagement/wakeup.c
new file mode 100644
index 0000000..35d537a
--- /dev/null
+++ b/boards/PowerManagement/wakeup.c
@@ -0,0 +1,51 @@
+#include <ch.h>
+#include <hal.h>
+
+#include <board.h>
+#include "wakeup.h"
+
+enum wakeup_mode   wakeup_mode;
+enum wakeup_source wakeup_source;
+
+void standby_wakeup_source(void) {
+
+  uint32_t csr = PWR->CSR;
+  uint32_t gpioc_val = palReadPort(GPIOC);
+
+  // default to POR
+  wakeup_mode = WAKE_MODE_POR;
+  wakeup_source = WAKE_SRC_UNKN;
+
+  // make sure reset while powered on looks like POR
+  PWR->CR |= (PWR_CR_CSBF);
+
+  if ((csr & (PWR_CSR_SBF | PWR_CSR_WUF)) == (PWR_CSR_SBF | PWR_CSR_WUF)) {
+
+    // wakeup or RTC event from standby mode
+    // default to WKUP
+    wakeup_mode = WAKE_MODE_STDBY;
+    wakeup_source = WAKE_SRC_WKUP;
+
+    if (gpioc_val & (1u << GPIOC_PATH_DC))
+      wakeup_source |= WAKE_SRC_PATHDC_CONNECT;
+
+    if (!(gpioc_val & (1u << GPIOC_TOUCH_INT_N)))
+      wakeup_source |= WAKE_SRC_TOUCH_INT;
+
+    /*
+     * default to SYS_PD_N source when
+     * no other reason for wakeup was found.
+     */
+    if (wakeup_source == WAKE_SRC_WKUP)
+      wakeup_source |= WAKE_SRC_SYS_PD_N;
+
+  }
+
+}
+
+void stop_wakeup_source(void) {
+
+  wakeup_mode = WAKE_MODE_STOP;
+  wakeup_source = WAKE_SRC_UNKN;
+
+}
diff --git a/boards/PowerManagement/wakeup.h b/boards/PowerManagement/wakeup.h
new file mode 100644
index 0000000..d9778ae
--- /dev/null
+++ b/boards/PowerManagement/wakeup.h
@@ -0,0 +1,73 @@
+#ifndef WAKEUP_H_
+#define WAKEUP_H_
+
+enum wakeup_mode {
+
+  /**
+   * Wakeup POR.
+   */
+  WAKE_MODE_POR,
+  /**
+   * Wakeup from stop mode.
+   */
+  WAKE_MODE_STOP,
+  /**
+   * Wakeup from standby mode.
+   */
+  WAKE_MODE_STDBY,
+
+};
+
+enum wakeup_source {
+
+  /**
+   * Wakeup source unknown.
+   */
+  WAKE_SRC_UNKN                 = 0x00000000u,
+  /**
+   * Wakeup source wakeup pin.
+   */
+  WAKE_SRC_WKUP                 = 0x00000001u,
+  /**
+   * Wakeup source DC jack connected.
+   */
+  WAKE_SRC_PATHDC_CONNECT       = 0x00000002u,
+  /**
+   * Wakeup source DC jack disconnected.
+   */
+  WAKE_SRC_PATHDC_DISCONNECT    = 0x00000004u,
+  /**
+   * Wakeup source touch button interrupt.
+   */
+  WAKE_SRC_TOUCH_INT            = 0x00000008u,
+  /**
+   * Wakeup source other board.
+   */
+  WAKE_SRC_SYS_PD_N             = 0x00000010u,
+  /**
+   * Wakeup source any RTC event.
+   */
+  WAKE_SRC_RTC                  = 0x00000060u,
+  /**
+   * Wakeup source RTC Tamper/Timestamp event.
+   */
+  WAKE_SRC_RTC_TAMPER_TIMESTAMP = 0x00000020u,
+  /**
+   * Wakeup source RTC Wakeup.
+   */
+  WAKE_SRC_RTC_WAKEUP           = 0x00000040u,
+
+};
+
+extern enum wakeup_mode   wakeup_mode;
+extern enum wakeup_source wakeup_source;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void standby_wakeup_source(void);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* WAKEUP_H_ */
diff --git a/components/Color.cpp b/components/Color.cpp
new file mode 100644
index 0000000..4723da7
--- /dev/null
+++ b/components/Color.cpp
@@ -0,0 +1,249 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <amiro/Color.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+Color::Color()
+    : red(0),
+      green(0),
+      blue(0) {
+}
+
+Color::Color(const Color& color)
+    : red(color.red),
+      green(color.green),
+      blue(color.blue) {
+}
+
+Color::Color(int r, int g, int b)
+    : red(r),
+      green(g),
+      blue(b) {
+}
+
+Color::Color(GlobalColor color) {
+  static const struct {
+    unsigned char red;
+    unsigned char green;
+    unsigned char blue;
+  } global_colors[] = {
+      { 240, 248, 255 },
+      { 250, 235, 215 },
+      { 0, 255, 255 },
+      { 127, 255, 212 },
+      { 240, 255, 255 },
+      { 245, 245, 220 },
+      { 255, 228, 196 },
+      { 0, 0, 0 },
+      { 255, 235, 205 },
+      { 0, 0, 255 },
+      { 138, 43, 226 },
+      { 165, 42, 42 },
+      { 222, 184, 135 },
+      { 95, 158, 160 },
+      { 127, 255, 0 },
+      { 210, 105, 30 },
+      { 255, 127, 80 },
+      { 100, 149, 237 },
+      { 255, 248, 220 },
+      { 220, 20, 60 },
+      { 0, 255, 255 },
+      { 0, 0, 139 },
+      { 0, 139, 139 },
+      { 184, 134, 11 },
+      { 169, 169, 169 },
+      { 0, 100, 0 },
+      { 169, 169, 169 },
+      { 189, 183, 107 },
+      { 139, 0, 139 },
+      { 85, 107, 47 },
+      { 255, 140, 0 },
+      { 153, 50, 204 },
+      { 139, 0, 0 },
+      { 233, 150, 122 },
+      { 143, 188, 143 },
+      { 72, 61, 139 },
+      { 47, 79, 79 },
+      { 47, 79, 79 },
+      { 0, 206, 209 },
+      { 148, 0, 211 },
+      { 255, 20, 147 },
+      { 0, 191, 255 },
+      { 105, 105, 105 },
+      { 105, 105, 105 },
+      { 30, 144, 255 },
+      { 178, 34, 34 },
+      { 255, 250, 240 },
+      { 34, 139, 34 },
+      { 255, 0, 255 },
+      { 220, 220, 220 },
+      { 248, 248, 255 },
+      { 255, 215, 0 },
+      { 218, 165, 32 },
+      { 128, 128, 128 },
+      { 128, 128, 128 },
+      { 0, 128, 0 },
+      { 173, 255, 47 },
+      { 240, 255, 240 },
+      { 255, 105, 180 },
+      { 205, 92, 92 },
+      { 75, 0, 130 },
+      { 255, 255, 240 },
+      { 240, 230, 140 },
+      { 230, 230, 250 },
+      { 255, 240, 245 },
+      { 124, 252, 0 },
+      { 255, 250, 205 },
+      { 173, 216, 230 },
+      { 240, 128, 128 },
+      { 224, 255, 255 },
+      { 250, 250, 210 },
+      { 211, 211, 211 },
+      { 144, 238, 144 },
+      { 211, 211, 211 },
+      { 255, 182, 193 },
+      { 255, 160, 122 },
+      { 32, 178, 170 },
+      { 135, 206, 250 },
+      { 119, 136, 153 },
+      { 119, 136, 153 },
+      { 176, 196, 222 },
+      { 255, 255, 224 },
+      { 0, 255, 0 },
+      { 50, 205, 50 },
+      { 250, 240, 230 },
+      { 255, 0, 255 },
+      { 128, 0, 0 },
+      { 102, 205, 170 },
+      { 0, 0, 205 },
+      { 186, 85, 211 },
+      { 147, 112, 219 },
+      { 60, 179, 113 },
+      { 123, 104, 238 },
+      { 0, 250, 154 },
+      { 72, 209, 204 },
+      { 199, 21, 133 },
+      { 25, 25, 112 },
+      { 245, 255, 250 },
+      { 255, 228, 225 },
+      { 255, 228, 181 },
+      { 255, 222, 173 },
+      { 0, 0, 128 },
+      { 253, 245, 230 },
+      { 128, 128, 0 },
+      { 107, 142, 35 },
+      { 255, 165, 0 },
+      { 255, 69, 0 },
+      { 218, 112, 214 },
+      { 238, 232, 170 },
+      { 152, 251, 152 },
+      { 175, 238, 238 },
+      { 219, 112, 147 },
+      { 255, 239, 213 },
+      { 255, 218, 185 },
+      { 205, 133, 63 },
+      { 255, 192, 203 },
+      { 221, 160, 221 },
+      { 176, 224, 230 },
+      { 128, 0, 128 },
+      { 255, 0, 0 },
+      { 188, 143, 143 },
+      { 65, 105, 225 },
+      { 139, 69, 19 },
+      { 250, 128, 114 },
+      { 244, 164, 96 },
+      { 46, 139, 87 },
+      { 255, 245, 238 },
+      { 160, 82, 45 },
+      { 192, 192, 192 },
+      { 135, 206, 235 },
+      { 106, 90, 205 },
+      { 112, 128, 144 },
+      { 112, 128, 144 },
+      { 255, 250, 250 },
+      { 0, 255, 127 },
+      { 70, 130, 180 },
+      { 210, 180, 140 },
+      { 0, 128, 128 },
+      { 216, 191, 216 },
+      { 255, 99, 71 },
+      { 64, 224, 208 },
+      { 238, 130, 238 },
+      { 245, 222, 179 },
+      { 255, 255, 255 },
+      { 245, 245, 245 },
+      { 255, 255, 0 },
+      { 154, 205, 50 }, };
+  this->setRedGreenBlue(global_colors[color].red, global_colors[color].green,
+                        global_colors[color].blue);
+}
+
+Color Color::darker(int factor) {
+  if (factor <= 0)
+    return *this;
+  else if (factor < 100)
+    return this->lighter(10000 / factor);
+
+  int red = 100 * this->red / factor;
+  int green = 100 * this->green / factor;
+  int blue = 100 * this->blue / factor;
+
+  return Color(red, green, blue);
+}
+
+void Color::getRedGreenBlue(int* r, int* g, int* b) {
+  *r = this->red;
+  *g = this->green;
+  *b = this->blue;
+}
+
+int Color::getRed() {
+  return this->red;
+}
+
+int Color::getGreen() {
+  return this->green;
+}
+
+int Color::getBlue() {
+  return this->blue;
+}
+
+Color Color::lighter(int factor) {
+  if (factor <= 0)
+    return *this;
+  else if (factor < 100)
+    return this->darker(10000 / factor);
+
+  int red = factor * this->red / 100;
+  int green = factor * this->green / 100;
+  int blue = factor * this->blue / 100;
+
+  red = (red < 0) ? 0 : (red > 255) ? 255 : red;
+  green = (green < 0) ? 0 : (green > 255) ? 255 : green;
+  blue = (blue < 0) ? 0 : (blue > 255) ? 255 : blue;
+
+  return Color(red, green, blue);
+}
+
+void Color::setRedGreenBlue(int red, int green, int blue) {
+  this->red = red;
+  this->green = green;
+  this->blue = blue;
+}
+
+Color& Color::operator=(Color const& color) {
+  if (this != &color) {
+    this->red = color.red;
+    this->green = color.green;
+    this->blue = color.blue;
+  }
+  return *this;
+}
+
+Color& Color::operator=(GlobalColor color) {
+  return operator=(Color(color));
+}
diff --git a/components/ControllerAreaNetworkRx.cpp b/components/ControllerAreaNetworkRx.cpp
new file mode 100644
index 0000000..64d7615
--- /dev/null
+++ b/components/ControllerAreaNetworkRx.cpp
@@ -0,0 +1,193 @@
+#include <ch.hpp>
+#include <hal.h>
+#include <string.h>  // memcpy
+
+#include <amiro/Constants.h>
+#include <amiro/ControllerAreaNetworkRx.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+using namespace types;
+using namespace amiro::constants;
+
+ControllerAreaNetworkRx::ControllerAreaNetworkRx(CANDriver *can, const uint8_t boardId)
+    : BaseStaticThread<128>(),
+      boardId(boardId),
+      canDriver(can) {
+#ifdef STM32F4XX
+  this->canConfig.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP;
+  this->canConfig.btr = CAN_BTR_SJW(1) | CAN_BTR_TS2(3) | CAN_BTR_TS1(15)
+                        | CAN_BTR_BRP(1);
+#else
+  this->canConfig.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP;
+  this->canConfig.btr = CAN_BTR_SJW(1) | CAN_BTR_TS2(2) | CAN_BTR_TS1(13)
+                        | CAN_BTR_BRP(1);
+#endif
+}
+
+ControllerAreaNetworkRx::
+~ControllerAreaNetworkRx() {
+
+}
+
+//----------------------------------------------------------------
+
+msg_t ControllerAreaNetworkRx::receiveSensorVal(CANRxFrame *frame) {
+  int deviceId = this->decodeDeviceId(frame);
+
+  // chprintf((BaseSequentialStream*) &SD1, "DeviceId: %d\r\n",deviceId);
+
+  switch (deviceId) {
+    case CAN::PROXIMITY_RING_ID(0):
+    case CAN::PROXIMITY_RING_ID(1):
+    case CAN::PROXIMITY_RING_ID(2):
+    case CAN::PROXIMITY_RING_ID(3):
+    case CAN::PROXIMITY_RING_ID(4):
+    case CAN::PROXIMITY_RING_ID(5):
+    case CAN::PROXIMITY_RING_ID(6):
+    case CAN::PROXIMITY_RING_ID(7):
+      if (frame->DLC == 2) {
+        int index = deviceId & 0x7;
+        proximityRingValue[index] = frame->data16[0];
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::ACTUAL_SPEED_ID:
+      if (frame->DLC == 8) {
+        actualSpeed[0] = frame->data32[0];
+        actualSpeed[1] = frame->data32[1];
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::ODOMETRY_ID:
+      if (frame->DLC == 8) {
+        this->robotPosition.x = (frame->data8[0] << 8 | frame->data8[1] << 16 | frame->data8[2] << 24);
+        this->robotPosition.y = (frame->data8[3] << 8 | frame->data8[4] << 16 | frame->data8[5] << 24);
+        this->robotPosition.f_z = (frame->data8[6] << 8 | frame->data8[7] << 16);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::PROXIMITY_FLOOR_ID:
+      // chprintf((BaseSequentialStream*) &SD1, "CAN::PROXIMITY_FLOOR_ID");
+      if (frame->DLC == 8) {
+        proximityFloorValue[0] = frame->data16[0];
+        proximityFloorValue[1] = frame->data16[1];
+        proximityFloorValue[2] = frame->data16[2];
+        proximityFloorValue[3] = frame->data16[3];
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::POWER_STATUS_ID:
+      if (frame->DLC == 6) {
+        powerStatus.charging_flags.value = frame->data8[0];
+        powerStatus.state_of_charge = frame->data8[1];
+        powerStatus.minutes_remaining = frame->data16[1];
+        powerStatus.power_consumption = frame->data16[2];
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::ROBOT_ID:
+      if (frame->DLC == 1) {
+        robotId = frame->data8[0];
+        return RDY_OK;
+      }
+      break;
+
+    default:
+      break;
+  }
+
+  return RDY_RESET;
+}
+
+uint16_t ControllerAreaNetworkRx::getProximityRingValue(int index) {
+  return this->proximityRingValue[index];
+}
+
+void ControllerAreaNetworkRx::getActualSpeed(types::kinematic &targetSpeed) {
+  targetSpeed.x = this->actualSpeed[0];
+  targetSpeed.w_z = this->actualSpeed[1];
+}
+
+types::position ControllerAreaNetworkRx::getOdometry() {
+  return this->robotPosition;
+}
+
+power_status &ControllerAreaNetworkRx::getPowerStatus() {
+  return this->powerStatus;
+}
+
+uint8_t ControllerAreaNetworkRx::getRobotID() {
+  return this->robotId;
+}
+
+
+uint16_t ControllerAreaNetworkRx::getProximityFloorValue(int index) {
+  return this->proximityFloorValue[index];
+}
+
+//----------------------------------------------------------------
+
+msg_t ControllerAreaNetworkRx::main(void) {
+
+  this->rxFullCanEvtSource = reinterpret_cast<EvtSource *>(&this->canDriver->rxfull_event);
+
+  this->rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
+
+  canStart(this->canDriver, &this->canConfig);
+
+  this->setName("ControllerAreaNetworkRx");
+
+  while (!this->shouldTerminate()) {
+    eventmask_t eventMask = this->waitOneEventTimeout(ALL_EVENTS, MS2ST(10000));
+    switch (eventMask) {
+
+      case EVENT_MASK(CAN::RECEIVED_ID):
+        CANRxFrame rxframe;
+        msg_t message = canReceive(this->canDriver, CAN_ANY_MAILBOX, &rxframe, TIME_IMMEDIATE);
+        if (message == RDY_OK) {
+          // chprintf((BaseSequentialStream*) &SD1, "Rx Message");
+          message = this->receiveMessage(&rxframe);
+          if (message != RDY_OK)
+            this->receiveSensorVal(&rxframe);
+        }
+        break;
+    }
+  }
+
+  canStop(this->canDriver);
+  this->rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
+
+  return RDY_OK;
+}
+
+msg_t ControllerAreaNetworkRx::receiveMessage(CANRxFrame *frame) {
+  (void)frame;
+  return RDY_OK;
+}
+
+//----------------------------------------------------------------
+
+int ControllerAreaNetworkRx::decodeBoardId(CANRxFrame *frame) {
+  return (frame->SID >> CAN::BOARD_ID_SHIFT) & CAN::BOARD_ID_MASK;
+}
+
+int ControllerAreaNetworkRx::decodeDeviceId(CANRxFrame *frame) {
+  return (frame->SID >> CAN::DEVICE_ID_SHIFT) & CAN::DEVICE_ID_MASK;
+}
+
+int ControllerAreaNetworkRx::decodeIndexId(CANRxFrame *frame) {
+  return (frame->SID >> CAN::INDEX_ID_SHIFT) & CAN::INDEX_ID_MASK;
+}
+
+//----------------------------------------------------------------
+
+int ControllerAreaNetworkRx::rxCmdShell(CANRxFrame *frame) {
+  return (frame->SID >> CAN::BOARD_ID_SHIFT) & CAN::BOARD_ID_MASK;
+}
diff --git a/components/ControllerAreaNetworkTx.cpp b/components/ControllerAreaNetworkTx.cpp
new file mode 100644
index 0000000..844dff7
--- /dev/null
+++ b/components/ControllerAreaNetworkTx.cpp
@@ -0,0 +1,229 @@
+#include <ch.hpp>
+#include <hal.h>
+#include <string.h>  // memcpy
+
+#include <amiro/Constants.h>
+#include <amiro/ControllerAreaNetworkTx.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+using namespace types;
+using namespace amiro::constants;
+
+ControllerAreaNetworkTx::ControllerAreaNetworkTx(CANDriver *can, const uint8_t boardId)
+    : BaseStaticThread<128>(),
+      boardId(boardId),
+      canDriver(can)
+{
+#ifdef STM32F4XX
+  this->canConfig.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP;
+  this->canConfig.btr = CAN_BTR_SJW(1) | CAN_BTR_TS2(3) | CAN_BTR_TS1(15)
+                        | CAN_BTR_BRP(1);
+#else
+  this->canConfig.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP;
+  this->canConfig.btr = CAN_BTR_SJW(1) | CAN_BTR_TS2(2) | CAN_BTR_TS1(13)
+                        | CAN_BTR_BRP(1);
+#endif
+}
+
+ControllerAreaNetworkTx::
+~ControllerAreaNetworkTx() {
+
+}
+
+//----------------------------------------------------------------
+
+void ControllerAreaNetworkTx::setLightBrightness(int brightness) {
+  CANTxFrame frame;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::BRIGHTNESS_ID);
+  frame.data8[0] = brightness;
+  frame.DLC = 1;
+  this->transmitMessage(&frame);
+}
+
+void ControllerAreaNetworkTx::setLightColor(int index, Color color) {
+  CANTxFrame frame;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::COLOR_ID(index));
+  frame.data8[0] = color.getRed();
+  frame.data8[1] = color.getGreen();
+  frame.data8[2] = color.getBlue();
+  frame.DLC = 3;
+  this->transmitMessage(&frame);
+}
+
+void ControllerAreaNetworkTx::setOdometry(types::position robotPosition) {
+  CANTxFrame frame;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::SET_ODOMETRY_ID);
+  // Cut of the first byte, which precission is not needed
+  int32_t x_mm = (robotPosition.x >> 8);
+  int32_t y_mm = (robotPosition.y >> 8);
+  int16_t f_z_mrad = int16_t(robotPosition.f_z >> 8 );
+  // Copy the data structure
+  memcpy((uint8_t *)&(frame.data8[0]), (uint8_t *)&x_mm, 3);
+  memcpy((uint8_t *)&(frame.data8[3]), (uint8_t *)&y_mm, 3);
+  memcpy((uint8_t *)&(frame.data8[6]), (uint8_t *)&f_z_mrad, 2);
+  frame.DLC = 8;
+  this->transmitMessage(&frame);
+}
+
+//----------------------------------------------------------------
+
+void ControllerAreaNetworkTx::setTargetSpeed(int32_t leftURpm, int32_t rightURpm) {
+  CANTxFrame frame;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::TARGET_RPM_ID);
+  frame.data32[0] = leftURpm;
+  frame.data32[1] = rightURpm;
+  frame.DLC = 8;
+  this->transmitMessage(&frame);
+}
+
+void ControllerAreaNetworkTx::setTargetSpeed(kinematic &targetSpeed) {
+  CANTxFrame frame;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::TARGET_SPEED_ID);
+  frame.data32[0] = targetSpeed.x;
+  frame.data32[1] = targetSpeed.w_z;
+  frame.DLC = 8;
+  this->transmitMessage(&frame);
+}
+
+void ControllerAreaNetworkTx::setTargetPosition(types::position &targetPosition, uint32_t targetTime) {
+  CANTxFrame frame;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::TARGET_POSITION_ID);
+  frame.data32[0] = targetPosition.x;
+  frame.data32[1] = targetPosition.f_z;
+  frame.data32[1] = targetTime;
+  frame.DLC = 12;
+  this->transmitMessage(&frame);
+}
+
+void ControllerAreaNetworkTx::setKinematicConstants(float Ed, float Eb) {
+  CANTxFrame frame;
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::SET_KINEMATIC_CONST_ID);
+  frame.data32[0] = Ed;
+  frame.data32[1] = Eb;
+  frame.DLC = 8;
+  this->transmitMessage(&frame);
+}
+
+void ControllerAreaNetworkTx::broadcastShutdown() {
+    CANTxFrame frame;
+    frame.SID = 0x00u;
+    this->encodeDeviceId(&frame, CAN::BROADCAST_SHUTDOWN);
+    frame.data16[0] = CAN::SHUTDOWN_MAGIC;
+    frame.DLC = 2;
+    this->transmitMessage(&frame);
+
+}
+
+//----------------------------------------------------------------
+
+void ControllerAreaNetworkTx::txQueryShell(uint8_t toBoardId, char *textdata, uint16_t size) {
+    uint16_t cnt = size;
+    CANTxFrame frame;
+    frame.SID = 0;
+    frame.DLC = 8;
+    this->encodeDeviceId(&frame, CAN::SHELL_QUERY_ID(toBoardId));
+
+    while (cnt > 8) {
+        memcpy(frame.data8,&textdata[size-cnt],8) ;
+        this->transmitMessage(&frame);
+        cnt -= 8;
+    }
+    memcpy(frame.data8,&textdata[size-cnt],cnt) ;
+    frame.DLC = cnt;
+    this->transmitMessage(&frame);
+}
+
+
+void ControllerAreaNetworkTx::txReplyShell(uint8_t toBoardId, char *textdata, uint16_t size) {
+    uint16_t cnt = size;
+    CANTxFrame frame;
+    frame.SID = 0;
+    frame.DLC = 8;
+    this->encodeDeviceId(&frame, CAN::SHELL_REPLY_ID(toBoardId));
+
+    while (cnt > 8) {
+        memcpy(frame.data8,&textdata[size-cnt],8) ;
+        this->transmitMessage(&frame);
+        cnt -= 8;
+    }
+    memcpy(frame.data8,&textdata[size-cnt],cnt) ;
+    frame.DLC = cnt;
+    this->transmitMessage(&frame);
+}
+
+//----------------------------------------------------------------
+
+msg_t ControllerAreaNetworkTx::main(void) {
+  evtInit(&this->evtimer, CAN::UPDATE_PERIOD_MSEC);
+
+  this->eventTimerEvtSource = reinterpret_cast<EvtSource *>(&this->evtimer.et_es);
+
+  this->eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
+
+  evtStart(&this->evtimer);
+
+  this->setName("ControllerAreaNetworkTx");
+
+  while (!this->shouldTerminate()) {
+    eventmask_t eventMask = this->waitOneEvent(ALL_EVENTS);
+    switch (eventMask) {
+      case EVENT_MASK(CAN::PERIODIC_TIMER_ID):
+        updateSensorVal();
+        periodicBroadcast();
+        break;
+    }
+  }
+
+  evtStop(&this->evtimer);
+  this->eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
+
+  return RDY_OK;
+}
+
+msg_t ControllerAreaNetworkTx::updateSensorVal() {
+  return RDY_OK;
+}
+
+//----------------------------------------------------------------
+
+void ControllerAreaNetworkTx::encodeBoardId(CANTxFrame *frame, int board) {
+  frame->SID |= (board & CAN::BOARD_ID_MASK) << CAN::BOARD_ID_SHIFT;
+}
+
+void ControllerAreaNetworkTx::encodeDeviceId(CANTxFrame *frame, int device) {
+  frame->SID |= (device & CAN::DEVICE_ID_MASK) << CAN::DEVICE_ID_SHIFT;
+}
+
+void ControllerAreaNetworkTx::encodeIndexId(CANTxFrame *frame, int index) {
+  frame->SID |= (index & CAN::INDEX_ID_MASK) << CAN::INDEX_ID_SHIFT;
+}
+
+//----------------------------------------------------------------
+
+void ControllerAreaNetworkTx::transmitMessage(CANTxFrame *frame) {
+  this->encodeBoardId(frame, boardId);
+  frame->IDE = CAN_IDE_STD;
+  frame->RTR = CAN_RTR_DATA;
+  /**
+   * We cannot use TIME_INFINITE here b/c
+   * lower boards might crash/power down etc.
+   * and block CAN bus (though they should not...).
+   * Then, we get stuck here and cannot terminate CAN thread
+   * Therefore:
+   *
+   * 1 us       * (  1 + 11 +   1 +   1 +   1 +   4 +   64 +  15 +     1 +   1 +     1 +   7) * 5        = 545 us
+   * 1/ (1 MHz) * (SOF + ID + RTR + IDE + RES + DLC + DATA + CRC + DELIM + ACK + DELIM + EOF) * #RETRIES
+   */
+
+  BaseThread::sleep(MS2ST(2));
+  canTransmit(this->canDriver, CAN_TX_MAILBOXES, frame, US2ST(545));
+}
diff --git a/components/Debug.c b/components/Debug.c
new file mode 100644
index 0000000..b1bd957
--- /dev/null
+++ b/components/Debug.c
@@ -0,0 +1,58 @@
+#include <ch.h>
+#include <hal.h>
+
+/*
+ * System_halt
+ * error code
+ *
+ *            - SV#1, misplaced @p chSysDisable().
+ *            - SV#2, misplaced @p chSysSuspend()
+ *            - SV#3, misplaced @p chSysEnable().
+ *            - SV#4, misplaced @p chSysLock().
+ *            - SV#5, misplaced @p chSysUnlock().
+ *            - SV#6, misplaced @p chSysLockFromIsr().
+ *            - SV#7, misplaced @p chSysUnlockFromIsr().
+ *            - SV#8, misplaced @p CH_IRQ_PROLOGUE().
+ *            - SV#9, misplaced @p CH_IRQ_EPILOGUE().
+ *            - SV#10, misplaced I-class function.
+ *            - SV#11, misplaced S-class function.
+ */
+
+static inline void serialWaitForEmpty(void) {
+  while (!((SD1.usart->SR) & USART_SR_TC)) {
+    ;
+  }
+}
+
+void haltErrorCode(void) {
+#if CH_DBG_SYSTEM_STATE_CHECK
+  char errorCode[26]="\nSystem halt! Error Code:\0";
+  int i=0;
+  rccEnableUSART1(FALSE);
+  nvicEnableVector(USART1_IRQn,CORTEX_PRIORITY_MASK(STM32_SERIAL_USART1_PRIORITY));
+  SD1.state = SD_READY;
+
+  SD1.usart->CR1 = 0;
+  SD1.usart->CR2 = 0;
+  SD1.usart->CR3 = 0;
+
+  SD1.usart->CR1 |= USART_CR1_UE | USART_CR1_TE;
+  SD1.usart->CR2 |= USART_CR2_STOP;
+  SD1.usart->BRR = STM32_PCLK2 / 115200;
+
+  while(errorCode[i]!='\0') {
+    SD1.usart->DR = errorCode[i];
+    serialWaitForEmpty();
+    i++;
+  }
+  i=0;
+  while(dbg_panic_msg[i]!='\0') {
+    SD1.usart->DR = dbg_panic_msg[i];
+    serialWaitForEmpty();
+    i++;
+  }
+
+  SD1.usart->DR = '\n';
+  serialWaitForEmpty();
+#endif
+}
diff --git a/components/DistControl.cpp b/components/DistControl.cpp
new file mode 100644
index 0000000..21c6036
--- /dev/null
+++ b/components/DistControl.cpp
@@ -0,0 +1,220 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <qei.h>
+
+#include <amiro/DistControl.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+using namespace types;
+using namespace constants;
+using namespace constants::DiWheelDrive;
+
+DistControl::DistControl(MotorControl* mc, MotorIncrements* mi)
+    : BaseStaticThread<256>(),
+      motorControl(mc),
+      motorIncrements(mi),
+      period(10)
+       {
+
+  // initialize velocities
+  this->maxVelocity.y = 0;
+  this->maxVelocity.z = 0;
+  this->maxVelocity.w_x = 0;
+  this->maxVelocity.w_y = 0;
+  this->targetVelocity.x = 0;
+  this->targetVelocity.y = 0;
+  this->targetVelocity.z = 0;
+  this->targetVelocity.w_x = 0;
+  this->targetVelocity.w_y = 0;
+  this->targetVelocity.w_z = 0;
+  this->minVelocity.y = 0;
+  this->minVelocity.z = 0;
+  this->minVelocity.w_x = 0;
+  this->minVelocity.w_y = 0;
+  this->minVelocity.w_z = 0;
+
+  // set max and min velocities
+  this->maxVelocity.x = 0.15 * 1e6; // 15 cm/s
+  this->minVelocity.x = 0.02 * 1e6; // 2 cm/s
+  this->maxVelocity.w_z = 2*maxVelocity.x / MotorControl::actualWheelBaseDistanceSI;
+  this->minVelocity.w_z = 2*minVelocity.x / MotorControl::actualWheelBaseDistanceSI;
+}
+
+int DistControl::getCurrentTargetDist() {
+  return (int)(targetDistance*1e6);
+}
+
+int DistControl::getCurrentTargetAngle() {
+  return (int)(targetAngle*1e6);
+}
+
+void DistControl::setTargetPosition(int32_t distance, int32_t angle, uint16_t time) {
+  chSysLock();
+  targetDistance = distance; // um
+  drivingForward = distance > 0;
+  if (!drivingForward) {
+    targetDistance *= -1;
+  }
+  targetAngle = angle; // urad
+  turningLeft = angle > 0;
+  if (!turningLeft) {
+    targetAngle *= -1;
+  }
+  restTime = time * 1e3; // us
+  controllerActive = true;
+  chSysUnlock();
+  fullDistance[LEFT_WHEEL] = 0;
+  fullDistance[RIGHT_WHEEL] = 0;
+  motorControl->updateIncrements(motorIncrements, increment, incrementDifference);
+}
+
+bool DistControl::isActive(void) {
+  return controllerActive;
+}
+
+void DistControl::deactivateController(void) {
+  chSysLock();
+  controllerActive = false;
+  targetDistance = 0;
+  targetAngle = 0;
+  restTime = 0;
+  for (int idx=0; idx < 2; idx++) {
+    increment[idx] = 0;
+    incrementDifference[idx] = 0;
+    actualDistance[idx] = 0;
+    fullDistance[idx] = 0;
+  }
+  chSysUnlock();
+}
+
+msg_t DistControl::main(void) {
+  systime_t time = System::getTime();
+  systime_t printTime = time;
+  this->setName("DistControl");
+
+  DistControl::deactivateController();
+
+  while (!this->shouldTerminate()) {
+    time += MS2ST(this->period);
+
+    if (controllerActive) {
+      // get increment differences for each wheel
+      motorControl->updateIncrements(motorIncrements, increment, incrementDifference); // ticks
+
+      // calculate driven distance difference for each wheel
+      motorControl->updateDistance(incrementDifference, actualDistance); // m
+
+      // calculate full driven distance for each wheel
+      for (int idxWheel = 0; idxWheel < 2; idxWheel++) {
+        fullDistance[idxWheel] += (int32_t)(actualDistance[idxWheel]*1e6);
+      }
+
+      // calculate whole driven distance and angle
+      realDistance = (fullDistance[LEFT_WHEEL] + fullDistance[RIGHT_WHEEL]) / 2.0; // um
+      if (!drivingForward) {
+        realDistance *= -1;
+      }
+      realAngle = (fullDistance[RIGHT_WHEEL] - fullDistance[LEFT_WHEEL]) / MotorControl::actualWheelBaseDistanceSI; // urad
+      if (!turningLeft) {
+        realAngle *= -1;
+      }
+
+      // calculate distance and angle to drive
+      errorDistance = targetDistance - realDistance; // um
+      if (errorDistance < 0) {
+        errorDistance = 0;
+      }
+      errorAngle = targetAngle - realAngle; // urad
+      if (errorAngle < 0) {
+        errorAngle = 0;
+      }
+
+      // calculate velocities for motor control
+      DistControl::calcVelocities();
+
+      if (controllerActive && newVelocities) {
+        // set target velocities
+        this->motorControl->setTargetSpeed(targetVelocity);
+        newVelocities = false;
+      }
+
+/*
+      if (time-printTime > MS2ST(100)) {
+        chprintf((BaseSequentialStream*) &SD1, "dist = %i um, angle = %i urad, ed = %i um, ea = %i, v = %i um/s, w = %i urad/s\n", realDistance, realAngle, errorDistance, errorAngle, targetVelocity.x, targetVelocity.w_z);
+        printTime = time;
+      }
+*/
+
+      // reduce rest time (us)
+      restTime -= period*1e3;
+      if (restTime < 1) {
+        restTime = 1;
+      }
+
+      // deactivate controller if necessary
+      if (errorDistance == 0 && errorAngle == 0) {
+        deactivateController();
+      }
+
+    }
+
+    chThdSleepUntil(time);
+  }
+
+  return true;
+}
+
+void DistControl::calcVelocities(void) {
+  // TODO calculate target velocities better
+
+  // set intuitive velocities
+  int32_t forwardSpeed = (int32_t) (errorDistance * 1e6 / (1.0f*restTime)); // um/s
+  int32_t angleSpeed = (int32_t) (errorAngle * 1e6 / (1.0f*restTime)); // urad/s
+
+  int32_t maxForward = maxVelocity.x;
+  if (maxForward > errorDistance) {
+    maxForward = errorDistance;
+  }
+
+  int32_t maxTurn = maxVelocity.w_z;
+  if (maxTurn > errorAngle) {
+    maxTurn = errorAngle;
+  }
+
+  // check max forward speed
+  if (forwardSpeed > maxForward) {
+    forwardSpeed = maxForward;
+    angleSpeed = (int32_t) (forwardSpeed * ((1.0f*errorAngle) / (1.0f*errorDistance)));
+  }
+
+  // check max angle speed
+  if (angleSpeed > maxTurn) {
+    angleSpeed = maxTurn;
+    forwardSpeed = (int32_t) (angleSpeed * ((1.0f*errorDistance) / (1.0f*errorAngle)));
+  }
+
+  // check for too small speeds
+  if (errorDistance > 0 && forwardSpeed < minVelocity.x) {
+    forwardSpeed = minVelocity.x;
+  }
+  if (errorAngle > 0 && errorDistance == 0 && angleSpeed < minVelocity.w_z) {
+    angleSpeed = minVelocity.w_z;
+  }
+
+  // set velocity directions
+  if (!drivingForward) {
+    forwardSpeed *= -1;
+  }
+  if (!turningLeft) {
+    angleSpeed *= -1;
+  }
+
+  // if nessecary set new target velocities
+  if (forwardSpeed != targetVelocity.x || angleSpeed != targetVelocity.w_z) {
+    newVelocities = true;
+    targetVelocity.x = forwardSpeed;
+    targetVelocity.w_z = angleSpeed;
+  }
+}
diff --git a/components/FileSystemInputOutput/FSIODiWheelDrive.cpp b/components/FileSystemInputOutput/FSIODiWheelDrive.cpp
new file mode 100644
index 0000000..f7ce6e3
--- /dev/null
+++ b/components/FileSystemInputOutput/FSIODiWheelDrive.cpp
@@ -0,0 +1,406 @@
+#include <amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp>
+
+using namespace amiro;
+using namespace amiro::fileSystemIo;
+
+msg_t
+FSIODiWheelDrive::
+getVcnl4020Offset (uint16_t *buffer, uint8_t idx) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(uint16_t);
+
+  // Check if the vcnl index is in a valid range
+  if (idx >= sizeof(DiWheelDrive_1_1::vcnl4020offset)) {
+    return WRONG_INDEX;
+  }
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_1, vcnl4020offset[idx]));
+      if (this->bsmv >= 0x01u) {
+        bytesWritten = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+msg_t
+FSIODiWheelDrive::
+setVcnl4020Offset (uint16_t buffer, uint8_t idx) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(uint16_t);
+
+  // Check if the vcnl index is in a valid range
+  if (idx >= sizeof(DiWheelDrive_1_1::vcnl4020offset)) {
+    return WRONG_INDEX;
+  }
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_1, vcnl4020offset[idx]));
+      if (this->bsmv >= 0x01u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+msg_t
+FSIODiWheelDrive::
+setwheelfactor (float buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_3, wheelfactor));
+      if (this->bsmv >= 0x03u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+msg_t
+FSIODiWheelDrive::
+getWheelFactor (float *buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_3, wheelfactor));
+      if (this->bsmv >= 0x03u) {
+        bytesWritten = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+msg_t
+FSIODiWheelDrive::
+setiGain (float buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_3, igain));
+      if (this->bsmv >= 0x03u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+msg_t
+FSIODiWheelDrive::
+getiGain (float *buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_3, igain));
+      if (this->bsmv >= 0x03u) {
+        bytesWritten = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+
+
+msg_t
+FSIODiWheelDrive::
+setdGain (float buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_3, dgain));
+      if (this->bsmv >= 0x03u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+msg_t
+FSIODiWheelDrive::
+getdGain (float *buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_3, dgain));
+      if (this->bsmv >= 0x03u) {
+        bytesWritten = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+
+
+msg_t
+FSIODiWheelDrive::
+setpGain (int buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_3, pgain));
+      if (this->bsmv >= 0x03u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+msg_t
+FSIODiWheelDrive::
+getpGain (int *buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_3, pgain));
+      if (this->bsmv >= 0x03u) {
+        bytesWritten = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+
+
+msg_t
+FSIODiWheelDrive::
+getEd (float *buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_2, Ed));
+      if (this->bsmv >= 0x02u) {
+        bytesWritten = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+msg_t
+FSIODiWheelDrive::
+setEd (float buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_2, Ed));
+      if (this->bsmv >= 0x02u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+msg_t
+FSIODiWheelDrive::
+getEb (float *buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_2, Eb));
+      if (this->bsmv >= 0x02u) {
+        bytesWritten = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
+
+msg_t
+FSIODiWheelDrive::
+setEb (float buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(float);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIODiWheelDrive::DiWheelDrive_1_2, Eb));
+      if (this->bsmv >= 0x02u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+  }
+
+  if (bytesWritten != bufferSize) {
+    return IO_ERROR;
+  } else {
+    return OK;
+  }
+}
diff --git a/components/FileSystemInputOutput/FSIOLightRing.cpp b/components/FileSystemInputOutput/FSIOLightRing.cpp
new file mode 100644
index 0000000..a9cf982
--- /dev/null
+++ b/components/FileSystemInputOutput/FSIOLightRing.cpp
@@ -0,0 +1,4 @@
+#include <amiro/FileSystemInputOutput/FSIOLightRing.hpp>
+
+using namespace amiro;
+using namespace amiro::fileSystemIo;
\ No newline at end of file
diff --git a/components/FileSystemInputOutput/FSIOPowerManagement.cpp b/components/FileSystemInputOutput/FSIOPowerManagement.cpp
new file mode 100644
index 0000000..31c9058
--- /dev/null
+++ b/components/FileSystemInputOutput/FSIOPowerManagement.cpp
@@ -0,0 +1,68 @@
+#include <amiro/FileSystemInputOutput/FSIOPowerManagement.hpp>
+
+using namespace amiro;
+using namespace amiro::fileSystemIo;
+
+msg_t
+FSIOPowerManagement::
+getVcnl4020Offset (uint16_t *buffer, uint8_t idx) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(uint16_t);
+
+  // Check if the vcnl index is in a valid range
+  if (idx >= sizeof(PowerManagement_1_1::vcnl4020offset)) {
+    return WRONG_INDEX;
+  }
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIOPowerManagement::PowerManagement_1_1, vcnl4020offset[idx]));
+      if (this->bsmv >= 0x01u) {
+        bytesWritten = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+    }
+
+    if (bytesWritten != bufferSize) {
+      return IO_ERROR;
+    } else {
+      return OK;
+    }
+}
+
+msg_t
+FSIOPowerManagement::
+setVcnl4020Offset (uint16_t buffer, uint8_t idx) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(uint16_t);
+
+  // Check if the vcnl index is in a valid range
+  if (idx >= sizeof(PowerManagement_1_1::vcnl4020offset)) {
+    return WRONG_INDEX;
+  }
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FSIOPowerManagement::PowerManagement_1_1, vcnl4020offset[idx]));
+      if (this->bsmv >= 0x01u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+    }
+
+    if (bytesWritten != bufferSize) {
+      return IO_ERROR;
+    } else {
+      return OK;
+    }
+}
diff --git a/components/FileSystemInputOutput/FileSystemInputOutputBase.cpp b/components/FileSystemInputOutput/FileSystemInputOutputBase.cpp
new file mode 100644
index 0000000..7fe529e
--- /dev/null
+++ b/components/FileSystemInputOutput/FileSystemInputOutputBase.cpp
@@ -0,0 +1,220 @@
+#include <amiro/FileSystemInputOutput/FileSystemInputOutputBase.hpp>
+
+using namespace amiro;
+using namespace amiro::fileSystemIo;
+
+msg_t
+FileSystemIoBase::init()
+{
+  msg_t returnValue = OK;
+
+  // Get the preamble
+  const uint8_t bufferSize = sizeof(FileSystemIoBase::preamble_1_1);
+  uint8_t buffer[bufferSize];
+  chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FileSystemIoBase::preamble_1_1, magicByte));
+  const size_t bytesRead = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+
+  // reading failed -> error
+  if (bytesRead != bufferSize) {
+    returnValue |= IO_ERROR;
+    return returnValue;
+  }
+
+  // magic byte is incorrect -> error
+  if (buffer[offsetof(FileSystemIoBase::preamble_1_1, magicByte)] != magicByteValue()) {
+    returnValue |= WRONG_MAGICBYTE_VALUE;
+    return returnValue;
+  }
+
+  // BSMV is incorrect -> error
+  if (buffer[offsetof(FileSystemIoBase::preamble_1_1, byteStructureMajorVersion)] != BSMV) {
+    returnValue |= WRONG_BSMV;
+    return returnValue;
+  }
+
+  // bsmv in smaller -> initialize
+  if (buffer[offsetof(FileSystemIoBase::preamble_1_1, byteStructureMinorVersion)] < bsmv) {
+    returnValue |= WRONG_bsmv;
+    // initialize all missing data
+    for (uint8_t v = bsmv; v > buffer[offsetof(FileSystemIoBase::preamble_1_1, byteStructureMinorVersion)]; --v)
+    {
+      returnValue |= initPreamble(BSMV, v, false);
+    }
+  }
+
+  // HMV is incorrect -> error
+  if (buffer[offsetof(FileSystemIoBase::preamble_1_1, hardwareMajorVersion)] != HMV) {
+    returnValue |= WRONG_HMV;
+    return returnValue;
+  }
+
+  // hmv is incorrect -> error
+  if (buffer[offsetof(FileSystemIoBase::preamble_1_1, hardwareMinorVersion)] >= hmv) {
+    returnValue |= WRONG_hmv;
+  }
+
+  return returnValue;
+}
+
+msg_t
+FileSystemIoBase::
+getCheck() {
+
+  // Get the preamble
+  const uint8_t bufferSize = sizeof(FileSystemIoBase::preamble_1_1);
+  uint8_t buffer[bufferSize];
+  chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FileSystemIoBase::preamble_1_1, magicByte));
+  const size_t bytesRead = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) buffer, bufferSize);
+
+  if (bytesRead != bufferSize) {
+    return IO_ERROR;
+  } else if (buffer[offsetof(FileSystemIoBase::preamble_1_1, magicByte)] != magicByteValue()) {
+    return WRONG_MAGICBYTE_VALUE;
+  } else if (buffer[offsetof(FileSystemIoBase::preamble_1_1, byteStructureMajorVersion)] != BSMV) {
+    return WRONG_BSMV;
+  } else if (buffer[offsetof(FileSystemIoBase::preamble_1_1, byteStructureMinorVersion)] < bsmv) {
+    return WRONG_bsmv;
+  } else if (buffer[offsetof(FileSystemIoBase::preamble_1_1, hardwareMajorVersion)] != HMV) {
+    return WRONG_HMV;
+  } else if (buffer[offsetof(FileSystemIoBase::preamble_1_1, hardwareMinorVersion)] < hmv) {
+    return WRONG_hmv;
+  } else {
+    return OK;
+  }
+}
+
+msg_t
+FileSystemIoBase::initPreamble(const uint8_t BSMV, const uint8_t bsmv, const bool strict)
+{
+  uint8_t buffer[3] = {};
+
+  if (strict) {
+    // initialize the magic byte
+    chFileStreamSeek((BaseFileStream*)&this->at24c01, offsetof(FileSystemIoBase::preamble_1_1, magicByte));
+    buffer[0] = magicByteValue();
+    if (chSequentialStreamWrite((BaseFileStream*)&this->at24c01, buffer, 1) != 1) {
+      return IO_ERROR;
+    }
+  }
+
+  switch (BSMV)
+  {
+    case 1:
+      if (strict) {
+        // initialize the BSMV
+        chFileStreamSeek((BaseFileStream*)&this->at24c01, offsetof(FileSystemIoBase::preamble_1_1, byteStructureMajorVersion));
+        buffer[0] = BSMV;
+        if (chSequentialStreamWrite((BaseFileStream*)&this->at24c01, buffer, 1) != 1) {
+          return IO_ERROR;
+        }
+      }
+      // initialize the bsmv
+      chFileStreamSeek((BaseFileStream*)&this->at24c01, offsetof(FileSystemIoBase::preamble_1_1, byteStructureMinorVersion));
+      buffer[0] = bsmv;
+      if (chSequentialStreamWrite((BaseFileStream*)&this->at24c01, buffer, 1) != 1) {
+        return IO_ERROR;
+      }
+      // initialize the content
+      switch (bsmv)
+      {
+        case 2:
+          // initialize the board ID
+          chFileStreamSeek((BaseFileStream*)&this->at24c01, offsetof(FileSystemIoBase::preamble_1_2, boardId));
+          buffer[0] = 0;
+          if (chSequentialStreamWrite((BaseFileStream*)&this->at24c01, buffer, 1) != 1) {
+            return IO_ERROR;
+          }
+          if (!strict) {
+            break;
+          }
+
+        case 1:
+          // initialize the bsmv, the HMV and the hmv
+          chFileStreamSeek((BaseFileStream*)&this->at24c01, offsetof(FileSystemIoBase::preamble_1_1, byteStructureMinorVersion));
+          buffer[0] = bsmv;
+          buffer[1] = this->HMV;
+          buffer[2] = this->hmv;
+          if (chSequentialStreamWrite((BaseFileStream*)&this->at24c01, buffer, 3) != 3) {
+            return IO_ERROR;
+          }
+          if (!strict) {
+            break;
+          }
+
+        default:
+          break;
+
+      }
+      break;
+
+    default:
+      break;
+  }
+
+  return OK;
+}
+
+msg_t
+FileSystemIoBase::
+resetMemory() {
+  return initPreamble(this->BSMV, this->bsmv, true);
+}
+
+msg_t
+FileSystemIoBase::
+getBoardId (uint8_t *buffer) {
+
+  msg_t bytesRead = 0;
+  // Work around, because stm32f1 cannot read a single byte
+  const uint8_t bufferSize = 2;
+  uint8_t bufferTmp[bufferSize] = {};
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FileSystemIoBase::preamble_1_2, boardId));
+      if (this->bsmv >= 0x02u) {
+        bytesRead = chSequentialStreamRead((BaseFileStream*)&at24c01, (uint8_t*) bufferTmp, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+    }
+
+  // Store only the board id
+  *buffer = bufferTmp[0];
+
+    if (bytesRead != bufferSize) {
+      return IO_ERROR;
+    } else {
+      return OK;
+    }
+}
+
+msg_t
+FileSystemIoBase::
+setBoardId (uint8_t buffer) {
+
+  msg_t bytesWritten = 0;
+  const uint8_t bufferSize = sizeof(uint8_t);
+
+  // Get the data out of the memory
+  switch (this->BSMV) {
+    case 0x01u:
+      // Set the pointer to the address
+      chFileStreamSeek((BaseFileStream*)&at24c01, offsetof(FileSystemIoBase::preamble_1_2, boardId));
+      if (this->bsmv >= 0x02u) {
+        bytesWritten = chSequentialStreamWrite((BaseFileStream*)&at24c01, (const uint8_t*) &buffer, bufferSize);
+      } else {
+        return NOT_IMPLEMENTED;
+      }
+      break;
+    }
+
+    if (bytesWritten != bufferSize) {
+      return IO_ERROR;
+    } else {
+      return OK;
+    }
+}
diff --git a/components/Lidar.cpp b/components/Lidar.cpp
new file mode 100644
index 0000000..7d5a273
--- /dev/null
+++ b/components/Lidar.cpp
@@ -0,0 +1,326 @@
+#include <amiro/Lidar.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+uint8_t Lidar::scannedData[NUMBER_OF_CHARACTERS + 1] = {};
+
+Lidar::Lidar(const uint8_t boardId, Lidar::SETUP setup)
+    : BaseStaticThread<256>(),
+      boardId(boardId),
+      setup(setup) {
+
+}
+
+Lidar::
+~Lidar() {
+
+  // Power down the LIDAR
+  // TODO Is it correct anyway
+  switch (boardId) {
+    case(CAN::LIGHT_RING_ID):
+      palWritePad(GPIOB, GPIOB_LASER_EN, PAL_LOW);
+      break;
+    default:
+      break;
+  }
+  this->isReady = false;
+};
+
+void Lidar::flushSD2InputQueue() {
+  chSysLock();
+  chIQResetI(&SD2.iqueue);
+  chSysUnlock();
+}
+
+msg_t Lidar::main(void) {
+
+
+  switch (this->boardId) {
+    case(CAN::LIGHT_RING_ID): {
+
+      // Power up the LIDAR
+      palWritePad(GPIOB, GPIOB_LASER_EN, PAL_HIGH);
+      BaseThread::sleep(MS2ST(5000));
+
+      // Setup the driver and lidar, if we want to communicate with it
+      if (setup != SETUP::POWER_ONLY) {
+        // Flush the queue because there is a "0" in it
+        flushSD2InputQueue();
+
+        // Configure LIDAR serial interface speed
+//       chprintf((BaseSequentialStream*) &SD1, "Speed switch to " STR(SD_SPEED) "\n");
+        chprintf((BaseSequentialStream*) &SD2, "SS" STR(SD_SPEED_PREFIX) STR(SD_SPEED) LF);
+
+        // Check if the switch went well, otherwise terminate the thread
+        if (checkDataString("SS" STR(SD_SPEED_PREFIX) STR(SD_SPEED) "\n00P\n\n")) {
+          chprintf((BaseSequentialStream*) &SD1, "Lidar speed switch OK\n");
+          // Configure serial interface of STM32
+          sdStop(&SD2);
+          SerialConfig sdLidarconf = { SD_SPEED, 0, 0, 0 };
+          sdStart(&SD2, &sdLidarconf);
+        } else {
+          chprintf((BaseSequentialStream*) &SD1, "Lidar speed switch NOT OK: Terminating Lidar \n");
+          palWritePad(GPIOB, GPIOB_LASER_EN, PAL_LOW);
+          return -1;
+        }
+      }
+      break;
+    }
+    default:
+      break;
+  }
+
+  evtInit(&this->evtimer, UPDATE_LIDAR_PERIOD_MSEC);
+
+  EvtSource *eventTimerEvtSource = reinterpret_cast<EvtSource *>(&this->evtimer.et_es);
+
+  EvtListener eventTimerEvtListener;
+
+  // TODO Does PERIODIC_TIMER_ID has something to do with ControllerAreNetwork.h ?
+  eventTimerEvtSource->registerOne(&eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
+
+  evtStart(&this->evtimer);
+
+  this->setName("Lidar");
+
+  while (!this->shouldTerminate()) {
+
+    eventmask_t eventMask = this->waitOneEvent(ALL_EVENTS);
+    switch (eventMask) {
+      case EVENT_MASK(CAN::PERIODIC_TIMER_ID):
+//         printDetails();
+        if (setup != SETUP::POWER_ONLY) {
+          updateSensorVal();
+        }
+        break;
+    }
+  }
+
+  evtStop(&this->evtimer);
+  eventTimerEvtSource->unregister(&eventTimerEvtListener);
+
+  return RDY_OK;
+}
+
+bool_t Lidar::getScan(uint16_t (&scannedData)[NUMBER_OF_STEPS]) {
+  if (this->isReady && !this->isFail) {
+    chSysLock();
+      memcpy(&scannedData, &(Lidar::scannedData[0]), NUMBER_OF_CHARACTERS);
+      this->isReady = false;
+    chSysUnlock();
+    return true;
+  } else {
+    return false;
+  }
+}
+
+uint16_t Lidar::getNumberOfValues() {
+  return NUMBER_OF_STEPS;
+}
+
+uint16_t Lidar::twoCharacterEncoding(uint8_t &char1, uint8_t &char2) {
+  return uint16_t((((char1 - 0x30) & 0b00111111) << 6) | ((char2 - 0x30) & 0b00111111));
+}
+
+bool_t Lidar::getData(uint8_t &data, uint32_t timeoutMs) {
+
+//     data = sdGet(&SD2);
+    msg_t dataTmp = chIQGetTimeout(&SD2.iqueue, MS2ST(timeoutMs));
+    if  (dataTmp == Q_TIMEOUT || dataTmp == Q_RESET) {
+      return false;
+    } else {
+      data = uint8_t(dataTmp);
+      ++this->dataCounter;
+      return true;
+    }
+}
+
+msg_t Lidar::updateSensorVal() {
+
+  chSysLock();
+    // Set this flag so that the everyone knows, that scannedData will be filed right now
+    this->isReady = false;
+    // Reset the FAIL flag
+    this->isFail = false;
+  chSysUnlock();
+
+  while (!this->isReady) {
+    switch(step) {
+      case SEND_SCAN_CMD:
+        flushSD2InputQueue();
+//                     Read (725-(44-1))=682 values with two-character encoding what makes 1364 datapoints
+//                     1456 in total:
+//                     16 (Cmd-Echo) + ? (Remaining Scans) + 1 (LF) + ? (String Characters) + 1 (LF) + 2 (Status) + 1 (Sum) + 1 (LF) + ? (Timestamp) + 1 (LF) + 1364 (Data for a certain setup) + 1 (Sum) + 1 (LF) + 1 (LF)
+        chprintf((BaseSequentialStream*) &SD2,DATA_ACQ_CODE STR(STARTING_STEP_PREFIX) STR(STARTING_STEP) STR(END_STEP_PREFIX) STR(END_STEP) STR(CLUSTER_COUNT) STR(SCAN_INTERVALL) STR(NUMBER_OF_INTERVALL) LF);
+        step = DATA_VALID_CHECK;
+        this->dataCounter = 0;
+        break;
+      case DATA_VALID_CHECK:
+        if(getData(this->newInput, 200)) {
+          // TODO check for last data, if scan has a few errors
+          // HACK The timeout fix this issue at least, because after a while there
+          //      wont be any transmitts
+          if (this->newInput == validScanTag[this->checkStatusIdx]) {
+            if (++this->checkStatusIdx == 5) {
+              // scan_data is without error: Found the sting "\n99b\n"
+              step = DATA_START_CHECK;
+              this->checkStatusIdx = 0;
+            }
+          } else {
+            this->checkStatusIdx = 0;
+          }
+        } else {
+          step = FAIL;
+        }
+        break;
+      case DATA_START_CHECK:
+        // Check for the next linefeed to start the record
+        if(getData(this->newInput, 200)) {
+          if (this->newInput == 10) {
+            step = DATA_RECORD;
+            this->dataIdx = 0;
+          }
+        } else {
+          step = FAIL;
+        }
+        break;
+      case DATA_RECORD:
+        if(getData(this->newInput, 200)) {
+          if (this->lastInput == 10 && this->newInput == 10) {
+            // end of data
+            this->lastInput = 0x0;  // Just to delete this->lastInput value
+            step = DATA_DECODE;
+          } else if  (this->newInput != 10) {
+            Lidar::scannedData[this->dataIdx++] = this->newInput;
+            this->lastInput = this->newInput;
+          } else {  // this->lastInput != 10 && this->newInput == 10
+            // This is the case, where we know that the last character was the checksum.
+            // Therefore, we need to decrement the counter
+            --this->dataIdx;
+            this->lastInput = this->newInput;
+          }
+          //chprintf((BaseSequentialStream*) &SD1, "%d ", this->newInput);
+        } else {
+          step = FAIL;
+        }
+        break;
+      case DATA_DECODE:
+        // Decode the recorded data
+        for (uint16_t idx=0; idx < NUMBER_OF_CHARACTERS; idx += 2) {
+          //TODO Check if +1 is right
+          *((uint16_t*) &(Lidar::scannedData[idx])) = twoCharacterEncoding(Lidar::scannedData[idx],Lidar::scannedData[idx+1]);
+        }
+        // To print out the data in this thread, choose "step = DATA_SHOW"
+        step = FINISH;
+        break;
+      case DATA_SHOW:
+        // Show the decoded data
+        chprintf((BaseSequentialStream*) &SD1, "\n%d", this->dataCounter);
+        for (uint32_t idx=0; idx < this->dataIdx; idx+=2) {
+          chprintf((BaseSequentialStream*) &SD1, "\n%d", *((uint16_t*) &(Lidar::scannedData[idx])));
+        }
+        step = FINISH;
+        break;
+      case FAIL:
+        // Set the FAIL flag
+        chSysLock();
+          // Set this flag so that the everyone knows, that scannedData holds wrong information
+          this->isFail = true;
+        chSysUnlock();
+        break;
+      case FINISH:
+        // Clean up
+        this->step = SEND_SCAN_CMD;
+        chSysLock();
+          // Set this flag so that the everyone knows, that scannedData holds the actual data
+          this->isReady = true;
+        chSysUnlock();
+        break;
+      default:
+        break;
+    }
+  }
+
+  return RDY_OK;
+}
+
+void Lidar::printData() {
+  uint8_t lastInput = 0xFF, newInput = 0xFF;
+  while (true) {
+    if(getData(newInput, 200)) {
+      if (lastInput == 10 && newInput == 10) {
+        return;
+      } else {
+        chprintf((BaseSequentialStream*) &SD1, "%c", newInput);
+      }
+      lastInput = newInput;
+    } else {
+      chprintf((BaseSequentialStream*) &SD1, "TIMEOUT\n", newInput);
+      return;
+    }
+  }
+}
+
+bool_t Lidar::checkDataString(const char compareString[]) {
+  uint8_t lastInput = 0xFF, newInput = 0xFF, recIdx = 0;
+  bool_t dataOk = true, processFlag = true;
+  while (processFlag) {
+    if (getData(newInput, 200)) {
+      // End of data received, quit processing
+      if (lastInput == 10 && newInput == 10) {
+        processFlag = false;
+      }
+      // Compare data if the characters before have been ok
+      // Otherwise do not compare anymore, because of possible
+      // segmentation fault
+      if (dataOk) {
+        if (newInput != compareString[recIdx++]) {
+          dataOk = false;
+        }
+      }
+      // Save the received character
+      lastInput = newInput;
+    } else {
+      processFlag = false;
+      dataOk = false;
+    }
+  }
+  return dataOk;
+}
+
+void Lidar::printDetails() {
+
+  chprintf((BaseSequentialStream*) &SD1, "Print sensor details:\n");
+
+  // Tell the sensor to transmit its details
+  chprintf((BaseSequentialStream*) &SD2, "VV\n");
+
+  // Print the transmitted data
+  printData();
+}
+
+void Lidar::printSpecification() {
+
+  chprintf((BaseSequentialStream*) &SD1, "Print sensor specification:\n");
+
+  // Tell the sensor to transmit its specifications
+  chprintf((BaseSequentialStream*) &SD2, "PP\n");
+
+  // Print the transmitted data
+  printData();
+}
+
+void Lidar::printInformation() {
+  chprintf((BaseSequentialStream*) &SD1, "Print sensor information:\n");
+
+  // Tell the sensor to transmit its information
+  chprintf((BaseSequentialStream*) &SD2, "II\n");
+
+  // Print the transmitted data
+  printData();
+}
+
+bool_t Lidar::getIsReady() {
+  return this->isReady;
+}
diff --git a/components/MotorControl.cpp b/components/MotorControl.cpp
new file mode 100644
index 0000000..1da2d2a
--- /dev/null
+++ b/components/MotorControl.cpp
@@ -0,0 +1,520 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <qei.h>
+#include <chprintf.h>
+#include <amiro/MotorControl.h>
+#include <global.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+using namespace types;
+using namespace constants;
+using namespace constants::DiWheelDrive;
+
+float MotorControl::wheelDiameterCorrectionFactor[2] = {1.0f, 1.0f};
+float MotorControl::actualWheelBaseDistanceSI = wheelBaseDistanceSI;
+extern Global global;
+
+MotorControl::MotorControl(PWMDriver* pwm, MotorIncrements* mi, GPIO_TypeDef* port, int pad, fileSystemIo::FSIODiWheelDrive *memory)
+    : BaseStaticThread<512>(),
+      pwmDriver(pwm),
+      motorIncrements(mi),
+      powerEnablePort(port),
+      powerEnablePad(pad),
+      eventSource(),
+      period(10),
+      memory(memory) {
+
+  this->pwmConfig.frequency = 7200000;
+  this->pwmConfig.period = 360;
+  this->pwmConfig.callback = NULL;
+  this->pwmConfig.channels[0].mode = PWM_OUTPUT_ACTIVE_HIGH;
+  this->pwmConfig.channels[0].callback = NULL;
+  this->pwmConfig.channels[1].mode = PWM_OUTPUT_ACTIVE_HIGH;
+  this->pwmConfig.channels[1].callback = NULL;
+  this->pwmConfig.channels[2].mode = PWM_OUTPUT_ACTIVE_HIGH;
+  this->pwmConfig.channels[2].callback = NULL;
+  this->pwmConfig.channels[3].mode = PWM_OUTPUT_ACTIVE_HIGH;
+  this->pwmConfig.channels[3].callback = NULL;
+  this->pwmConfig.cr2 = 0;
+
+  this->increment[0] = 0;
+  this->increment[1] = 0;
+
+  this->errorSum[0] = 0;
+  this->errorSum[1] = 0;
+
+  this->errorSumDiff = 0;
+
+  // Init the velocities
+  this->currentVelocity.x = 0;
+  this->currentVelocity.y = 0;
+  this->currentVelocity.z = 0;
+  this->currentVelocity.w_x = 0;
+  this->currentVelocity.w_y = 0;
+  this->currentVelocity.w_z = 0;
+  this->targetVelocity.x = 0;
+  this->targetVelocity.w_z = 0;
+
+  this->newTargetVelocities = false;
+  this->lastVelocitiesV[0] = 0;
+  this->lastVelocitiesV[1] = 0;
+  this->lastVelocitiesV[2] = 0;
+  this->lastVelocitiesV[3] = 0;
+  this->lastVelocitiesV[4] = 0;
+  this->lastVelocitiesV[5] = 0;
+
+  this->newTargetVelocities = true;
+
+  // calibration stuff;
+  for (int i =0; i<3; i++){
+    this->leftWValues[i] = i;
+    this->rightWValues[i] = i;
+  }
+
+}
+
+int MotorControl::getCurrentRPMLeft() {
+  return this->actualSpeed[LEFT_WHEEL];
+}
+
+int MotorControl::getCurrentRPMRight() {
+  return this->actualSpeed[RIGHT_WHEEL];
+}
+
+kinematic MotorControl::getCurrentVelocity() {
+  return this->currentVelocity;
+}
+
+msg_t MotorControl::setWheelDiameterCorrectionFactor(float Ed /* = 1.0f */, bool_t storeEdToMemory /* = false */) {
+  // cl (Eq. 17a)
+  MotorControl::wheelDiameterCorrectionFactor[LEFT_WHEEL] = 2.0f / (Ed + 1.0f);
+  // cl (Eq. 17a)
+  MotorControl::wheelDiameterCorrectionFactor[RIGHT_WHEEL] = 2.0f / ((1.0f / Ed) + 1.0f);
+  // Store Ed to memory
+  if (storeEdToMemory)
+    return memory->setEd(Ed);
+  else
+    return RDY_OK;
+}
+
+msg_t MotorControl::setActualWheelBaseDistance(float Eb /* = 1.0f */, bool_t storeEbToMemory /* = false */) {
+  // bActual (Eq. 4)
+  MotorControl::actualWheelBaseDistanceSI = wheelBaseDistanceSI * Eb;
+  // Store Eb to memory
+  if (storeEbToMemory)
+    return memory->setEb(Eb);
+  else
+    return RDY_OK;
+}
+
+EvtSource* MotorControl::getEventSource() {
+  return &this->eventSource;
+}
+
+void MotorControl::setTargetRPM(int32_t targetURpmLeft, int32_t targetURpmRight) {
+  // Velocity in µm/s in x direction
+  int32_t targetVelocityX = (wheelCircumferenceSI * (targetURpmLeft + targetURpmRight)) / secondsPerMinute / 2.0f;
+  // Angular velocity around z in µrad/s
+  int32_t targetVelocityWz = (wheelCircumferenceSI * (targetURpmRight - targetURpmLeft)) / secondsPerMinute / MotorControl::actualWheelBaseDistanceSI;
+  chSysLock();
+  this->targetVelocity.x = targetVelocityX;
+  this->targetVelocity.w_z = targetVelocityWz;
+  this->newTargetVelocities = true;
+  chSysUnlock();
+}
+
+void MotorControl::setTargetSpeed(const kinematic &targetVelocity) {
+  chSysLock();
+  this->targetVelocity.x = targetVelocity.x;
+  this->targetVelocity.w_z = targetVelocity.w_z;
+  this->newTargetVelocities = true;
+  chSysUnlock();
+}
+
+msg_t MotorControl::main(void) {
+  systime_t time = System::getTime();
+  this->setName("MotorControl");
+
+  // load controller parameters from memory
+  this->memory->getWheelFactor(&this->motorCalibrationFactor);
+  this->memory->getpGain(&this->pGain);
+  this->memory->getiGain(&this->iGain);
+  this->memory->getdGain(&this->dGain);
+  this->memory->getEb(&this->Eb);
+  this->memory->getEd(&this->Ed);
+
+  pwmStart(this->pwmDriver, &this->pwmConfig);
+
+  palSetPad(this->powerEnablePort, this->powerEnablePad);
+
+  while (!this->shouldTerminate()) {
+    time += MS2ST(this->period);
+
+    // Get the increments from the QEI
+    MotorControl::updateIncrements(this->motorIncrements, this->increment, this->incrementDifference);
+
+    // Get the actual speed from the gathered increments
+    MotorControl::updateSpeed(this->incrementDifference, this->actualSpeed, this->period);
+
+    // Calculate velocities
+    this->calcVelocity();
+
+    // updates past velocities for the derivate part of the controller
+    updateDerivativeLastVelocities();
+
+    controllerAndCalibrationLogic();
+
+    // Write the calculated duty cycle to the pwm driver
+    this->writePulseWidthModulation();
+
+    chThdSleepUntil(time);
+
+    delay ++;
+    if (delay > 50){
+      delay = 0;
+    }
+
+  }
+
+  // Reset the PWM befor shutdown
+  this->pwmPercentage[LEFT_WHEEL] = 0;
+  this->pwmPercentage[RIGHT_WHEEL] = 0;
+  this->writePulseWidthModulation();
+
+  return true;
+}
+
+void MotorControl::controllerAndCalibrationLogic(){
+  if (!isCalibrating){
+    this->PIDController();
+    startedZieglerCalibration = true;
+    startedWheelCalibration = true;
+  } else {
+    if (motorCalibration){
+      if (startedWheelCalibration){
+        wheelCalibrationTime = System::getTime();
+        startedWheelCalibration = false;
+      }
+      calibrate();
+    } else {
+      this->PIDController();
+      this->calibrateZiegler();
+      this->updatePastVelocities();
+
+      if (startedZieglerCalibration){
+        zieglerCalibrationTime = System::getTime();
+        startedZieglerCalibration = false;
+      }
+    }
+  }
+}
+
+void MotorControl::calibrate() {
+
+  this->pwmPercentage[LEFT_WHEEL] = 3000;
+  this->pwmPercentage[RIGHT_WHEEL] = 3000;
+
+  this->rightWValues[0] = this->rightWValues[1];
+  this->rightWValues[1] = this->rightWValues[2];
+  this->rightWValues[2] = getRightWheelSpeed();
+
+  this->leftWValues[0] = this->leftWValues[1];
+  this->leftWValues[1] = this->leftWValues[2];
+  this->leftWValues[2] = getLeftWheelSpeed();
+
+
+  if (this->rightWValues[0] == this->rightWValues[1] &&
+      this->rightWValues[1] == this->rightWValues[2] &&
+      this->leftWValues[0] == this->leftWValues[1] &&
+      this->leftWValues[1] == this->leftWValues[2] &&
+      System::getTime() - this->wheelCalibrationTime > 1500) {
+    this->motorCalibrationFactor = (float)this->rightWValues[0]/(float)this->leftWValues[0];
+
+    chprintf((BaseSequentialStream*) &SD1, "motorCalibrationFactor =   %f  \n" ,this->motorCalibrationFactor);
+    chprintf((BaseSequentialStream*) &SD1, "rw =   %i  \n" ,this->rightWValues[0]);
+    chprintf((BaseSequentialStream*) &SD1, "lw =   %i  \n" ,this->leftWValues[0]);
+
+    this->pwmPercentage[LEFT_WHEEL] = 0;
+    this->pwmPercentage[RIGHT_WHEEL] = 0;
+    this->motorCalibration = false;
+    this->memory->setwheelfactor(motorCalibrationFactor);
+  }
+}
+
+void MotorControl::calibrateZiegler() {
+
+    this->iGain =0;
+    this->dGain = 0;
+    int nsc  = this->getNumberofSignChanges();
+
+    if (System::getTime() - this->zieglerCalibrationTime > 1000){
+        this->zieglerCalibrationTime = System::getTime() ;
+        this->ziegler2 =true;
+    }
+
+    if (ziegler && ziegler2){
+        this->targetVelocity.x = 200000 * ((zieglerHelp%2 == 0) ? 1 : -1);
+        this->pGain = 1000 + 100 * zieglerHelp;
+        chprintf((BaseSequentialStream*) &SD1, "pgain =  %i  \n" , this->pGain);
+        zieglerHelp++;
+        ziegler = false;
+        ziegler2 = false;
+    }
+
+    if (!ziegler  && ziegler2){
+        this->targetVelocity.x = 0;
+        ziegler2= false;
+        ziegler = true;
+    }
+
+    if (zieglerHelp > 20){
+        this->isCalibrating = false;
+        this->targetVelocity.x = 0;
+        this->iGain = 0.08;
+        this->pGain = 1000;
+    }
+
+
+    if ( nsc > 8){
+        zieglerHelp2++;
+        if (zieglerHelp2 > 20){
+           this->zieglerPeriod  = numberOfLastVelocitiesV * this->period / nsc;
+           chprintf((BaseSequentialStream*) &SD1, "zieglerPeriod =   %f  \n" ,this->zieglerPeriod);
+
+           this->targetVelocity.x = 0;
+           this->pGain = (int) (this->pGain* 0.6);
+           this->iGain = (float) ((1/(0.5*(this->zieglerPeriod/1000))/this->pGain));
+           this->dGain = (float) ((1/(0.125*(this->zieglerPeriod/1000))/this->pGain));
+           this->memory->setpGain(this->pGain);
+           this->memory->setiGain(this->iGain);
+           this->memory->setdGain(this->dGain);
+           chprintf((BaseSequentialStream*) &SD1, "pgain =   %i  \n" ,this->pGain);
+           chprintf((BaseSequentialStream*) &SD1, "igain =   %f  \n" ,this->iGain);
+           chprintf((BaseSequentialStream*) &SD1, "dgain =   %f  \n" ,this->dGain);
+
+           this->motorCalibration = true;
+           ziegler = true;
+           ziegler2 = true;
+           this->isCalibrating = false;
+           zieglerHelp = 0;
+           zieglerCalibrationTime = 0;
+           zieglerHelp2 = 0;
+
+           for (int i = 0; i< numberOfLastVelocitiesV ; i ++){
+               lastVelocitiesVBig[i] = 0;
+           }
+        }
+    }
+
+}
+
+void MotorControl::PIDController(){
+
+    //pgain #####################################
+    chSysLock();
+    int diffv =this->targetVelocity.x - this->currentVelocity.x;
+    int diffw = this->targetVelocity.w_z - this->currentVelocity.w_z;
+    chSysUnlock();
+
+    //igain ####################################
+   this->accumulatedErrorV += diffv;
+   this->accumulatedErrorW += diffw;
+
+    if (this->accumulatedErrorV > this->antiWindupV)
+      this->accumulatedErrorV = this->antiWindupV;
+    else if (this->accumulatedErrorV < -this->antiWindupV)
+      this->accumulatedErrorV = -this->antiWindupV;
+
+    if (this->accumulatedErrorW > this->antiWindupW)
+      this->accumulatedErrorW = this->antiWindupW;
+    else if (this->accumulatedErrorW < -this->antiWindupW)
+      this->accumulatedErrorW = -this->antiWindupW;
+
+    diffv += (int) (this->accumulatedErrorV*this->iGain);
+    diffw += (int) (this->accumulatedErrorW*this->iGain);
+
+    //dgain ###################################
+    int derivativeV;
+    int derivativeW;
+    int tmp1;
+    int tmp2;
+
+   tmp1 = static_cast<int32_t>((lastVelocitiesV[0]+lastVelocitiesV[1]+lastVelocitiesV[2])/3);
+   tmp2 = static_cast<int32_t>((lastVelocitiesV[3]+lastVelocitiesV[4]+lastVelocitiesV[5])/3);
+   derivativeV = static_cast<int32_t> ((tmp2-tmp1)/(int)(this->period));
+   tmp1 = static_cast<int32_t>((lastVelocitiesW[0]+lastVelocitiesW[1]+lastVelocitiesW[2])/3);
+   tmp2 = static_cast<int32_t>((lastVelocitiesW[3]+lastVelocitiesW[4]+lastVelocitiesW[5])/3);
+   derivativeW = static_cast<int32_t> ((tmp2-tmp1)/(int)(this->period));
+
+
+    diffv += (int) (dGain*derivativeV);
+    diffw += (int) (dGain*derivativeW);
+
+    setLeftWheelSpeed(diffv,diffw);
+    setRightWheelSpeed(diffv, diffw);
+}
+
+
+
+void MotorControl::setLeftWheelSpeed(int diffv, int diffw){
+    this->pwmPercentage[LEFT_WHEEL] = (int) (this->pGain*2*(diffv-0.5*diffw*wheelBaseDistanceSI*this->Eb)/(wheelDiameter*this->wheelDiameterCorrectionFactor[LEFT_WHEEL]));
+
+}
+
+void MotorControl::setRightWheelSpeed(int diffv, int diffw){
+    this->pwmPercentage[RIGHT_WHEEL] = (int) (motorCalibrationFactor*this->pGain*2*(diffv+0.5*diffw*wheelBaseDistanceSI*this->Eb)/(wheelDiameter*this->wheelDiameterCorrectionFactor[RIGHT_WHEEL]));
+}
+
+
+// speed in um
+int MotorControl::getRightWheelSpeed(){
+    int omega = 2*M_PI*this->actualSpeed[RIGHT_WHEEL]/60; // syslock noetig ? todo
+    return  omega * wheelDiameter*this->wheelDiameterCorrectionFactor[RIGHT_WHEEL];
+}
+// speed in um
+int MotorControl::getLeftWheelSpeed(){
+    int omega = 2*M_PI*this->actualSpeed[LEFT_WHEEL]/60;
+    return  omega * wheelDiameter*this->wheelDiameterCorrectionFactor[LEFT_WHEEL];
+}
+
+
+void MotorControl::updatePastVelocities(){
+    for (int i=0; i<numberOfLastVelocitiesV-1;i++){
+        lastVelocitiesVBig[i] = lastVelocitiesVBig[i+1];
+    }
+
+    lastVelocitiesVBig[numberOfLastVelocitiesV-1] = this->currentVelocity.x;
+
+}
+
+
+void MotorControl::updateDerivativeLastVelocities(){
+    for (int i=0; i<5;i++){
+        lastVelocitiesV[i] = lastVelocitiesV[i+1];
+        lastVelocitiesW[i] = lastVelocitiesW[i+1];
+    }
+
+
+    lastVelocitiesV[5] = this->currentVelocity.x;
+    lastVelocitiesW[5] = this->currentVelocity.w_z;
+
+
+}
+
+int MotorControl::getNumberofSignChanges(){
+    int nsc= 0;
+    bool ispositive = true;
+    bool tmpbool = true;
+    if (lastVelocitiesVBig[0] < 0){
+        ispositive =false;
+        tmpbool =false;
+    }
+    for (int i=0; i<numberOfLastVelocitiesV-1; i++){
+        if (lastVelocitiesVBig[i] < 0){
+            ispositive= false;
+        } else {
+            ispositive = true;
+        }
+        if (ispositive != tmpbool){
+            nsc++;
+            tmpbool = ispositive;
+        }
+
+    }
+
+    return nsc;
+}
+
+void MotorControl::printGains(){
+    chprintf((BaseSequentialStream*)&SD1, "motorCalibrationFactor %f\n", this->motorCalibrationFactor );
+    chprintf((BaseSequentialStream*)&SD1, "pGain %i\n", this->pGain );
+    chprintf((BaseSequentialStream*)&SD1, "iGain %f\n", this->iGain );
+    chprintf((BaseSequentialStream*)&SD1, "dGain %f\n", this->dGain );
+}
+
+
+
+void MotorControl::calcVelocity() {
+  // Velocity in µm/s in x direction
+  currentVelocity.x = (1.0f*wheelCircumference * (this->actualSpeed[LEFT_WHEEL] + this->actualSpeed[RIGHT_WHEEL])) / secondsPerMinute / 2.0f;
+  // Angular velocity around z in µrad/s
+  currentVelocity.w_z = (1.0f*wheelCircumference * (this->actualSpeed[RIGHT_WHEEL] - this->actualSpeed[LEFT_WHEEL])) / (1.0f*secondsPerMinute) / (wheelBaseDistanceSI*this->Eb);
+
+}
+
+
+
+void MotorControl::updateIncrements(MotorIncrements* motorIncrements, int32_t (&oldIncrement)[2], float (&incrementDifference)[2]) {
+  int32_t currentIncrement[2];
+
+  chSysLock();
+  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
+    currentIncrement[idxWheel] = motorIncrements->qeiGetPosition(idxWheel);
+  }
+  chSysUnlock();
+
+  // Calculate the difference between the last and
+  // actual increments and therefor the actual speed or distance
+  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
+
+    // Get the difference
+    int32_t tmpIncrementDifference =  oldIncrement[idxWheel] - currentIncrement[idxWheel];
+
+    // Handle overflow of increments
+    int range = motorIncrements->getQeiConfigRange();
+    if (tmpIncrementDifference > (range >> 1))
+      tmpIncrementDifference -= motorIncrements->getQeiConfigRange();
+    else if (tmpIncrementDifference < -(range >> 1))
+      tmpIncrementDifference += motorIncrements->getQeiConfigRange();
+
+    // Correct the difference
+    incrementDifference[idxWheel] =  static_cast<float>(tmpIncrementDifference) * MotorControl::wheelDiameterCorrectionFactor[idxWheel];
+
+    // Save the actual increments
+    oldIncrement[idxWheel] = currentIncrement[idxWheel];
+  }
+}
+
+void MotorControl::updateSpeed(const float (&incrementDifference)[2], int32_t (&actualSpeed)[2], const uint32_t period) {
+  const int32_t updatesPerMinute = 60 * 1000 / period;
+
+  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
+    // Save the actual speed
+    actualSpeed[idxWheel] = static_cast<int32_t>(static_cast<float>(updatesPerMinute * incrementDifference[idxWheel])) / incrementsPerRevolution;
+  }
+}
+
+void MotorControl::updateDistance(const float (&incrementDifference)[2], float (&actualDistance)[2]) {
+
+  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
+    // Calc. the distance per wheel in meter
+    actualDistance[idxWheel] = wheelCircumferenceSI * incrementDifference[idxWheel] / static_cast<float>(incrementsPerRevolution);
+  }
+}
+
+void MotorControl::writePulseWidthModulation() {
+  for (int idxWheel = 0; idxWheel < 2; idxWheel++) {
+    int percentage = this->pwmPercentage[idxWheel];
+    unsigned int widths[2];
+
+    // 10000 is the max. duty cicle
+    if (percentage > 10000) {
+      percentage = 10000;
+    } else if (percentage < -10000) {
+      percentage = -10000;
+    }
+
+    if (percentage < 0) {
+      widths[0] = 0;
+      widths[1] = PWM_PERCENTAGE_TO_WIDTH(this->pwmDriver, -percentage);
+    } else {
+      widths[0] = PWM_PERCENTAGE_TO_WIDTH(this->pwmDriver, percentage);
+      widths[1] = 0;
+    }
+
+    for (int idxPWM = 0; idxPWM < 2; idxPWM++)
+      pwmEnableChannel(this->pwmDriver, (idxWheel * 2) + idxPWM, widths[idxPWM]);
+  }
+}
diff --git a/components/MotorIncrements.cpp b/components/MotorIncrements.cpp
new file mode 100644
index 0000000..ef6cc00
--- /dev/null
+++ b/components/MotorIncrements.cpp
@@ -0,0 +1,35 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <qei.h>
+
+#include <amiro/MotorIncrements.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+MotorIncrements::MotorIncrements(QEIDriver* qei0, QEIDriver* qei1) {
+  this->qeiDriver[0] = qei0;
+  this->qeiDriver[1] = qei1;
+
+  this->qeiConfig.mode = QEI_COUNT_BOTH;
+  this->qeiConfig.channels[0].mode = QEI_INPUT_INVERTED;
+  this->qeiConfig.channels[1].mode = QEI_INPUT_NONINVERTED;
+  this->qeiConfig.range = 0x10000;
+}
+
+void MotorIncrements::start() {
+  qeiStart(this->qeiDriver[0], &this->qeiConfig);
+  qeiStart(this->qeiDriver[1], &this->qeiConfig);
+
+  qeiEnable(this->qeiDriver[0]);
+  qeiEnable(this->qeiDriver[1]);
+}
+
+int MotorIncrements::qeiGetPosition(int idxQei) {
+  return qeiGetPositionI(this->qeiDriver[idxQei]);
+}
+
+int MotorIncrements::getQeiConfigRange() {
+  return this->qeiConfig.range;
+}
\ No newline at end of file
diff --git a/components/Odometry.cpp b/components/Odometry.cpp
new file mode 100644
index 0000000..414ca17
--- /dev/null
+++ b/components/Odometry.cpp
@@ -0,0 +1,236 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <qei.h>
+
+#include <amiro/Odometry.h>
+
+#include <math.h> // cos(), sin()
+#include <Matrix.h> // Matrixoperations "Matrix::*"
+#include <amiro/Constants.h> // Constants "constants::*"
+#include <chprintf.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+using namespace constants::DiWheelDrive;
+
+
+Odometry::Odometry(MotorIncrements* mi)
+    : BaseStaticThread<512>(),
+      motorIncrements(mi),
+      eventSource(),
+      period(50),
+      incrementsPerRevolution(incrementsPerRevolution),
+      updatesPerMinute(constants::secondsPerMinute * constants::millisecondsPerSecond / this->period),
+      wheelCircumference(wheelCircumferenceSI),
+      wheelBaseDistanceSI(wheelBaseDistanceSI) {
+
+
+//  this-> = constants::secondsPerMinute * constants::millisecondsPerSecond / this->period;
+//  this->wheelCircumference = constants::wheelCircumferenceSI;
+//  this->wheelBaseDistanceSI = constants::wheelBaseDistanceSI;
+
+  this->distance[LEFT_WHEEL] = 0.0f;
+  this->distance[RIGHT_WHEEL] = 0.0f;
+  this->increment[LEFT_WHEEL] = 0;
+  this->increment[RIGHT_WHEEL] = 0;
+  this->incrementDifference[LEFT_WHEEL] = 0.0f;
+  this->incrementDifference[RIGHT_WHEEL] = 0.0f;
+  this->distance[LEFT_WHEEL] = 0.0f;
+  this->distance[RIGHT_WHEEL] = 0.0f;
+
+  this->wheelError[LEFT_WHEEL] = wheelErrorSI[LEFT_WHEEL];
+  this->wheelError[RIGHT_WHEEL] = wheelErrorSI[RIGHT_WHEEL];
+
+  this->resetPosition(); // Init position
+
+  this->resetError(); // Init error Cp
+
+}
+
+types::position Odometry::getPosition() {
+  types::position robotPosition;
+  const int32_t piScaled = int32_t(2 * M_PI * 1e6);
+  chSysLock();
+    // Conversion from standard unit to µ unit
+    robotPosition.x = this->pX * 1e6;
+    robotPosition.y = this->pY * 1e6;
+    robotPosition.f_z = (int32_t(this->pPhi * 1e6) % piScaled) + ((this->pPhi < 0) ? piScaled : 0);  // Get only the postitve angel f_z in [0 .. 2 * pi]
+  chSysUnlock();
+//     chprintf((BaseSequentialStream*) &SD1, "X:%d Y:%d Phi:%d", robotPosition.x,robotPosition.y, robotPosition.f_z);
+//     chprintf((BaseSequentialStream*) &SD1, "\r\n");
+//     chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);
+//     chprintf((BaseSequentialStream*) &SD1, "\r\n");
+  return robotPosition;
+}
+
+void Odometry::setPosition(float pX, float pY, float pPhi) {
+  chSysLock();
+    this->pX = pX;
+    this->pY = pY;
+    this->pPhi = pPhi;
+  chSysUnlock();
+}
+
+void Odometry::resetPosition() {
+  this->setPosition(0.0f,0.0f,0.0f);
+}
+
+void Odometry::setError(float* Cp3x3) {
+  chSysLock();
+//   float** test;
+    Matrix::copy<float>(Cp3x3,3,3, &(this->Cp3x3[0]),3,3);
+//     Matrix::copy<float>(Cp3x3,3,3, test,3,3);
+  chSysUnlock();
+}
+
+void Odometry::resetError() {
+//   float zeroMatrix[9] = {};
+//   this->setError(&(zeroMatrix[0]));
+  Matrix::init<float>(&(this->Cp3x3[0]),3,3,0.0f);
+}
+
+EvtSource* Odometry::getEventSource() {
+  return &this->eventSource;
+}
+
+msg_t Odometry::main(void) {
+  systime_t time = System::getTime();
+  this->setName("Odometry");
+
+  while (!this->shouldTerminate()) {
+    time += MS2ST(this->period);
+
+    // Update the base distance, because it may change after an calibration
+    this->updateWheelBaseDistance();
+
+    // Get the actual speed
+    this->updateDistance();
+
+    // Calculate the odometry
+    this->updateOdometry();
+
+//     chprintf((BaseSequentialStream*) &SD1, "X:%f Y:%f Phi:%f", this->pX,this->pY, this->pPhi);
+//     chprintf((BaseSequentialStream*) &SD1, "\r\n");
+//     chprintf((BaseSequentialStream*) &SD1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);
+//     chprintf((BaseSequentialStream*) &SD1, "\r\n");
+
+    chThdSleepUntil(time);
+  }
+
+  return true;
+}
+
+void Odometry::updateOdometry() {
+
+  // Get the temporary position and error
+  float Cp3x3[9];
+  chSysLock();
+    float pX = this->pX;
+    float pY = this->pY;
+    float pPhi = this->pPhi;
+    Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);
+  chSysUnlock();
+
+  ////////////////
+  // Temporary calculations
+  ////////////////
+
+  // TMP: Rotated angular
+  float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
+
+  // TMP: Moved distance
+  float dDistance = (this->distance[RIGHT_WHEEL] + this->distance[LEFT_WHEEL]) / 2.0f;
+
+  // TMP: Argument for the trigonometric functions
+  float trigArg = pPhi + dPhi / 2.0f;
+
+  // TMP: Trigonometric functions
+  float cosArg = cos(trigArg);
+  float sinArg = sin(trigArg);
+
+  // TMP: Delta distance
+  float dPX = dDistance * cosArg;
+  float dPY = dDistance * sinArg;
+
+  ////////////////
+  // Position Update
+  ////////////////
+
+  // Update distance
+  pX += dPX;
+  pY += dPY;
+  pPhi += dPhi;
+
+  ////////////////
+  // Temporary error calculations
+  ////////////////
+
+  // position propagation error (3x3 matrix)
+  float Fp3x3[9]  = {1.0f, 0.0f, -dPY,
+                     0.0f, 1.0f,  dPX,
+                     0.0f, 0.0f, 1.0f};
+  // steering error (2x2 matrix)
+  float Cs2x2[4] = {abs(this->distance[RIGHT_WHEEL])*wheelError[RIGHT_WHEEL],0.0f,
+                    0.0f, abs(this->distance[LEFT_WHEEL])*wheelError[LEFT_WHEEL]};
+  // steering propagation error (3x2 matrix)
+  float Fs3x2[6] = {(cosArg+dDistance*sinArg/this->wheelBaseDistanceSI)/2.0f, (sinArg+dDistance*cosArg/this->wheelBaseDistanceSI)/2.0f,
+                 (sinArg-dDistance*cosArg/this->wheelBaseDistanceSI)/2.0f, (cosArg-dDistance*sinArg/this->wheelBaseDistanceSI)/2.0f,
+                 -1.0f/this->wheelBaseDistanceSI                         , 1.0f/this->wheelBaseDistanceSI};
+
+  ////////////////
+  // Error calculations tmpCp = Fp*Cp*~Fp
+  ////////////////
+  // New position error
+  float tmpCp3x3[9] = {0.0f};
+  float tmpFpCp3x3[9] = {0.0f};
+  // tmpFpCp = Fp*Cp
+  Matrix::XdotY<float>(&(Fp3x3[0]),3,3,&(Cp3x3[0]),3,3,&(tmpFpCp3x3[0]),3,3);
+  // tmpCp = tmpFpCp*~Fp
+  Matrix::XdotYtrans<float>(&(tmpFpCp3x3[0]),3,3,&(Fp3x3[0]),3,3,&(tmpCp3x3[0]),3,3);
+
+  ////////////////
+  // Error calculations tmpCs = Fs*Cs*~Fs
+  ////////////////
+  // New steering error
+  float tmpCs3x3[9] = {0.0f};
+  float tmpFsCs3x2[6] = {0.0f};
+  // tmpFsCs = Fs*Cs
+  Matrix::XdotY<float>(&(Fs3x2[0]),3,2,&(Cs2x2[0]),2,2,&(tmpFsCs3x2[0]),3,2);
+  // tmpCs = tmpFsCs*~Fs
+  Matrix::XdotYtrans<float>(&(tmpFsCs3x2[0]),3,2,&(Fs3x2[0]),3,2,&(tmpCs3x3[0]),3,3);
+
+  ////////////////
+  // Error calculations Cp = Fp*Cp*~Fp + Fs*Cs*~Fs
+  ////////////////
+  Matrix::XplusY<float>(tmpCp3x3,3,3,tmpCs3x3,3,3,Cp3x3,3,3);
+
+  ////////////////
+  // Write back
+  ////////////////
+
+  // Write back
+  this->setPosition(pX,pY,pPhi);
+  chSysLock();
+    Matrix::copy<float>(Cp3x3,3,3,this->Cp3x3,3,3);
+  chSysUnlock();
+
+}
+
+void Odometry::updateWheelBaseDistance() {
+  this->wheelBaseDistanceSI = MotorControl::actualWheelBaseDistanceSI;
+}
+
+void Odometry::updateDistance() {
+
+  // Get the current increments of the QEI
+  MotorControl::updateIncrements(this->motorIncrements, this->increment, this->incrementDifference);
+//
+//  chprintf((BaseSequentialStream*) &SD1, "\ni_right = %d \t i_left = %d", this->increment[RIGHT_WHEEL], this->increment[LEFT_WHEEL]);
+//  chprintf((BaseSequentialStream*) &SD1, "\niDiff_right = %d \t iDiff_left = %d", this->incrementDifference[RIGHT_WHEEL], this->incrementDifference[LEFT_WHEEL]);
+
+  // Get the driven distance for each wheel
+  MotorControl::updateDistance(this->incrementDifference, this->distance);
+
+//  chprintf((BaseSequentialStream*) &SD1, "\nx_right = %f \t x_left = %f", this->distance[RIGHT_WHEEL], this->distance[LEFT_WHEEL]);
+}
diff --git a/components/accel/lis331dlh.cpp b/components/accel/lis331dlh.cpp
new file mode 100644
index 0000000..9cd2ea3
--- /dev/null
+++ b/components/accel/lis331dlh.cpp
@@ -0,0 +1,340 @@
+#include <string.h>
+
+#include <amiro/util/util.h>
+#include <amiro/bus/spi/HWSPIDriver.hpp>
+#include <amiro/accel/lis331dlh.hpp>
+#include <chprintf.h>
+#include <cmath>  // abs()
+
+namespace amiro {
+
+LIS331DLH::
+LIS331DLH(HWSPIDriver *driver) :
+  driver(driver),
+  currentFullScaleConfiguration(FS_2G) {
+
+}
+
+LIS331DLH::
+~LIS331DLH() {
+
+}
+
+chibios_rt::EvtSource*
+LIS331DLH::
+getEventSource() {
+  return &this->eventSource;
+}
+
+int16_t
+LIS331DLH::
+getAcceleration(const uint8_t axis) {
+
+  return this->accelerations[axis];
+
+}
+
+int16_t
+LIS331DLH::
+getAccelerationForce(const uint8_t axis) {
+
+  int16_t result;
+
+  switch(this->currentFullScaleConfiguration) {
+  default:
+  case(FS_2G):
+    // Should be this->accelerations[axis] * 0.9765625
+    result = this->accelerations[axis];
+    break;
+  case(FS_4G):
+      // Should be this->accelerations[axis] * 1.953125
+      result =  this->accelerations[axis] << 1;
+    break;
+  case(FS_8G):
+      // Should be this->accelerations[axis] * 3.90625
+      result =  this->accelerations[axis] << 2;
+    break;
+  }
+
+  return result;
+
+}
+
+msg_t
+LIS331DLH::
+main() {
+
+  this->setName("Lis331dlh");
+
+  while (!this->shouldTerminate()) {
+
+    updateSensorData();
+
+    this->eventSource.broadcastFlags(0);
+
+    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));
+
+  }
+  return RDY_OK;
+}
+
+void
+LIS331DLH::
+updateSensorData() {
+
+  const size_t buffer_size = offsetof(LIS331DLH::registers, out_z)
+                             - offsetof(LIS331DLH::registers, status_reg)
+                             + MEMBER_SIZE(LIS331DLH::registers, out_z)
+                             + 1; /* addressing */
+  uint8_t buffer[buffer_size];
+  uint8_t sreg;
+
+  // this might be three-wire so we need to send ones
+  memset(buffer, 0xFFu, buffer_size);
+  buffer[0] = offsetof(LIS331DLH::registers, status_reg) | LIS331DLH::SPI_MULT | LIS331DLH::SPI_READ;
+  this->driver->exchange(buffer, buffer, buffer_size);
+
+  // assemble data
+  sreg = buffer[1];
+
+  if (sreg & LIS331DLH::XDA)
+    this->accelerations[LIS331DLH::AXIS_X] = int16_t((buffer[3] << 8) | buffer[2]) >> 4;
+
+  if (sreg & LIS331DLH::YDA)
+    this->accelerations[LIS331DLH::AXIS_Y] = int16_t((buffer[5] << 8) | buffer[4]) >> 4;
+
+  if (sreg & LIS331DLH::ZDA)
+    this->accelerations[LIS331DLH::AXIS_Z] = int16_t((buffer[7] << 8) | buffer[6]) >> 4;
+
+}
+
+msg_t
+LIS331DLH::
+configure(LIS331DLHConfig *config) {
+
+  const size_t ctrl_reg_size = offsetof(LIS331DLH::registers, ctrl_reg5)
+                               - offsetof(LIS331DLH::registers, ctrl_reg1)
+                               + MEMBER_SIZE(LIS331DLH::registers, ctrl_reg5)
+                               + 1; /* addressing */
+  const size_t int_reg_size = offsetof(LIS331DLH::registers, int1_duration)
+                              - offsetof(LIS331DLH::registers, int1_cfg)
+                              + MEMBER_SIZE(LIS331DLH::registers, int1_duration) /* linear interrupt config */
+                              + MEMBER_SIZE(LIS331DLH::registers, int1_src)
+                              + MEMBER_SIZE(LIS331DLH::registers, int1_src) /* 2x size of int1_src */
+                              + 1; /* addressing */
+  const size_t buffer_size = ctrl_reg_size > int_reg_size ? ctrl_reg_size : int_reg_size;
+
+  uint8_t buffer[buffer_size];
+  InterruptConfig *int_cfg = &config->int1_cfg;
+  uint8_t i;
+
+  // write interrupt config first
+  for (i = 0x00u; i < 2; i++, int_cfg++) {
+    // this might be three-wire so we need to send ones
+    memset(buffer, 0xFFu, buffer_size);
+    if (!i)
+      buffer[0] = offsetof(LIS331DLH::registers, int1_cfg);
+    else
+      buffer[0] = offsetof(LIS331DLH::registers, int2_cfg);
+    buffer[5] = buffer[0] + offsetof(LIS331DLH::registers, int1_src) - offsetof(LIS331DLH::registers, int1_cfg);
+    buffer[5] |= LIS331DLH::SPI_READ;
+    buffer[6] = 0xFFu,
+    buffer[0] |= LIS331DLH::SPI_MULT | LIS331DLH::SPI_WRITE;
+    buffer[1] = int_cfg->config;
+    buffer[2] = 0xFFu; /* skip source register */
+    buffer[3] = int_cfg->ths;
+    buffer[4] = int_cfg->duration;
+    this->driver->write(&buffer[0], 5);
+    // read intN_src register to clear IA
+    this->driver->exchange(&buffer[5], &buffer[5], 2);
+  }
+
+  // write control config
+  // this might be three-wire so we need to send ones
+  memset(buffer, 0xFFu, buffer_size);
+  buffer[0] = offsetof(LIS331DLH::registers, ctrl_reg1) | LIS331DLH::SPI_MULT | LIS331DLH::SPI_WRITE;
+  buffer[1] = config->ctrl1;
+  buffer[2] = config->ctrl2;
+  buffer[3] = config->ctrl3;
+  buffer[4] = config->ctrl4;
+  buffer[5] = config->ctrl5;
+  this->driver->write(buffer, 6);
+
+  // reset hp filter
+  buffer[0] = offsetof(LIS331DLH::registers, hp_filter_reset) | LIS331DLH::SPI_WRITE;
+  this->driver->write(buffer, 1);
+
+  // Store the full scale configuration for acceleration decoding
+  this->currentFullScaleConfiguration = config->ctrl4 & LIS331DLH::FS_8G;
+
+  return RDY_OK;
+
+}
+
+uint8_t
+LIS331DLH::
+getCheck() {
+
+  const size_t buffer_size = 1 /* addressing */
+                             + 1; /* who am i */
+  uint8_t buffer[buffer_size];
+
+  // Exchange the data with the LIS331DLH accelerometer
+  // Specify the adress and the mode
+  buffer[0] = offsetof(LIS331DLH::registers, who_am_i) | LIS331DLH::SPI_READ;
+  this->driver->exchange(buffer, buffer, buffer_size);
+  // Check
+  if (buffer[1] == LIS331DLH::LIS331DLH_ID) {
+    return LIS331DLH::CHECK_OK;
+  } else {
+    return LIS331DLH::CHECK_FAIL;
+  }
+
+}
+
+void
+LIS331DLH::
+printSelfTest(LIS331DLHConfig* config) {
+
+  // Running the build in test from LIS331DLH manual and compare the
+  // measured values against table 3
+
+  const uint8_t amountOfMeanValues = 10;
+  const uint32_t sleepBetweenMeasurementMs = 500;
+  int32_t accel;
+
+  // Choose a defined config, if none is given
+  LIS331DLH::LIS331DLHConfig test_accel_run_cfg;
+  if (config == NULL) {
+    LIS331DLH::LIS331DLHConfig accel_run_cfg_tmp = {
+    /* ctrl1    */LIS331DLH::PM_ODR | LIS331DLH::DR_50HZ_37LP | LIS331DLH::ZEN
+        | LIS331DLH::YEN | LIS331DLH::XEN,
+    /* ctrl2    */0x00u,
+    /* ctrl3    */LIS331DLH::INT_LOW | LIS331DLH::I1_CFG_DRY,
+    /* ctrl4    */LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE | LIS331DLH::FS_8G
+        | LIS331DLH::SIM_4WI,
+    /* ctrl5    */LIS331DLH::SLEEP_TO_WAKE_OFF,
+    /* int1_cfg */
+    {
+    /* config   */LIS331DLH::AOI_OR_INT,
+    /* ths      */0x00u,
+    /* duration */0x00u, },
+    /* int2_cfg */
+    {
+    /* config   */LIS331DLH::AOI_OR_INT,
+    /* ths      */0x00u,
+    /* duration */0x00u, }, };
+    test_accel_run_cfg = accel_run_cfg_tmp;
+  } else {
+    // Save the given config
+    test_accel_run_cfg = *config;
+  }
+
+  // 1. Get some standard values
+  // Configure for the test
+  test_accel_run_cfg.ctrl4 = LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE
+      | LIS331DLH::FS_2G | LIS331DLH::SIM_4WI;
+  this->configure(&test_accel_run_cfg);
+  BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
+  // Grep some values and build the mean value
+  chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc std values\n");
+  int32_t stdAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
+      ->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration(
+      LIS331DLH::AXIS_Z) };
+
+  for (uint8_t n = 1; n < amountOfMeanValues; ++n) {
+    BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
+    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
+      accel = int32_t(this->getAcceleration(i));
+      stdAccValues[i] = (stdAccValues[i] * n + accel) / (n + 1);
+      chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
+                accel < 0 ? -accel : accel, stdAccValues[i]);
+    }
+    chprintf((BaseSequentialStream*) &SD1, "\n");
+  }
+
+  // 2. Apply negative offset
+  // Configure for the test
+  test_accel_run_cfg.ctrl4 = LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE
+      | LIS331DLH::FS_2G | LIS331DLH::SIM_4WI | LIS331DLH::ST_ENABLE
+      | LIS331DLH::STSIGN_NEG;
+  this->configure(&test_accel_run_cfg);
+  BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
+  // Grep some values and build the mean value
+  chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc neg values\n");
+  int16_t negAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
+      ->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration(
+      LIS331DLH::AXIS_Z) };
+
+  for (uint8_t n = 1; n < amountOfMeanValues; ++n) {
+    BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
+    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
+      accel = int32_t(this->getAcceleration(i));
+      negAccValues[i] = (negAccValues[i] * n + accel) / (n + 1);
+      chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
+                accel < 0 ? -accel : accel, negAccValues[i]);
+    }
+    chprintf((BaseSequentialStream*) &SD1, "\n");
+  }
+
+  // 2. Apply positive offset
+  // Configure for the test
+  test_accel_run_cfg.ctrl4 = LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE
+      | LIS331DLH::FS_2G | LIS331DLH::SIM_4WI | LIS331DLH::ST_ENABLE
+      | LIS331DLH::STSIGN_POS;
+  this->configure(&test_accel_run_cfg);
+  BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
+  // Grep some values and build the mean value
+  chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc pos values\n");
+  int16_t posAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
+      ->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration(
+      LIS331DLH::AXIS_Z) };
+
+  for (uint8_t n = 1; n < amountOfMeanValues; ++n) {
+    BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs));
+    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
+      accel = int32_t(this->getAcceleration(i));
+      posAccValues[i] = (posAccValues[i] * n + accel) / (n + 1);
+      chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
+                accel < 0 ? -accel : accel, posAccValues[i]);
+    }
+    chprintf((BaseSequentialStream*) &SD1, "\n");
+  }
+
+  // Get the amplitude change and compare it to the standard values from LIS331DLH manual table 3
+  int32_t amp;
+  const int32_t ampXmax = 550, ampYmax = 550, ampZmax = 750;
+  const int32_t ampXmin = 120, ampYmin = 120, ampZmin = 140;
+
+  // TEST
+  chprintf((BaseSequentialStream*) &SD1, "\n\nACC: Testresult\n");
+  amp = std::abs(
+      stdAccValues[LIS331DLH::AXIS_X] - negAccValues[LIS331DLH::AXIS_X]);
+  if (amp < ampXmin || amp > ampXmax)
+    chprintf((BaseSequentialStream*) &SD1, "ACC: Negative x-axis faulty\n");
+  amp = std::abs(
+      stdAccValues[LIS331DLH::AXIS_Y] - negAccValues[LIS331DLH::AXIS_Y]);
+  if (amp < ampYmin || amp > ampYmax)
+    chprintf((BaseSequentialStream*) &SD1, "ACC: Negative y-axis faulty\n");
+  amp = std::abs(
+      stdAccValues[LIS331DLH::AXIS_Z] - negAccValues[LIS331DLH::AXIS_Z]);
+  if (amp < ampZmin || amp > ampZmax)
+    chprintf((BaseSequentialStream*) &SD1, "ACC: Negative z-axis faulty\n");
+  amp = std::abs(
+      stdAccValues[LIS331DLH::AXIS_X] - posAccValues[LIS331DLH::AXIS_X]);
+  if (amp < ampXmin || amp > ampXmax)
+    chprintf((BaseSequentialStream*) &SD1, "ACC: Positive x-axis faulty\n");
+  amp = std::abs(
+      stdAccValues[LIS331DLH::AXIS_Y] - posAccValues[LIS331DLH::AXIS_Y]);
+  if (amp < ampYmin || amp > ampYmax)
+    chprintf((BaseSequentialStream*) &SD1, "ACC: Positive y-axis faulty\n");
+  amp = std::abs(
+      stdAccValues[LIS331DLH::AXIS_Z] - posAccValues[LIS331DLH::AXIS_Z]);
+  if (amp < ampZmin || amp > ampZmax)
+    chprintf((BaseSequentialStream*) &SD1, "ACC: Positive z-axis faulty\n");
+
+  // Write back the original config
+  this->configure(config);
+}
+
+} /* amiro */
diff --git a/components/bluetooth/bluetooth-connector.cpp b/components/bluetooth/bluetooth-connector.cpp
new file mode 100644
index 0000000..1ad73e4
--- /dev/null
+++ b/components/bluetooth/bluetooth-connector.cpp
@@ -0,0 +1,99 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <string.h>
+#include <chprintf.h>
+
+#include <amiro/bluetooth/bluetooth-connector.hpp>
+#include <amiro/bluetooth/bluetooth-serial.hpp>
+#include <amiro/bluetooth/bluetooth-wiimote.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+/*
+ * Class constructor
+ */
+BluetoothConnector::BluetoothConnector(BLUETOOTH *bluetooth, void *bluetoothDev, const char *typeConn) {
+  iwrap = &(bluetooth->iwrap);
+  objDev = bluetoothDev;
+  typeDev = typeConn;
+}
+
+//----------------------------------------------------------------
+
+
+void BluetoothConnector::bluetoothConnectorListen(const char *addr) {
+  BluetoothProfile profile;
+  profile.addr = (strstr(addr, "ALL")) ? "free" : addr;
+  profile.connector = this;
+  profile.connect = &BluetoothConnector::bluetoothConnectorActivate;
+  profile.disconnect = &BluetoothConnector::bluetoothConnectorDeactivate;
+  profile.linkid = 0xFF;
+
+  /* Listen all address can have only one profile at the last profile,
+     otherwise a fixed address will take it at first */
+  if (strstr(addr, "ALL") && (iwrap->profiles[7].addr == NULL)) {
+    iwrap->profiles[7] = profile;
+    return;
+  }
+
+  for(int i = 0; i < 8; i++) {
+    if(iwrap->profiles[i].addr == NULL) {
+      iwrap->profiles[i] = profile;
+      break;
+    }
+  }
+}
+
+void BluetoothConnector::bluetoothConnectorConnect(const char *addr) {
+  int size = strlen(addr) + 17;
+  char cmd[size];
+  strcpy(cmd, "CALL ");
+  strcat(cmd, addr);
+
+  for (int i = 0; i < 8; i++) {
+    if (strstr(iwrap->profiles[i].addr, addr) && (iwrap->profiles[i].linkid == 0xFF)) {
+      switch (typeDev[0]) {
+      case 'S':           // 'S' for "SERIAL"
+        strcat(cmd, " 1101 RFCOMM");
+        break;
+      case 'W':           // 'W' for "WIIMOTE"
+        strcat(cmd, " 0013 L2CAP ");
+        break;
+      }
+      iwrap->bluetoothIwrapSendCommand(cmd);
+      BaseThread::sleep(MS2ST(5000));    // waiting connection establish
+      break;
+    }
+  }
+}
+
+void BluetoothConnector::bluetoothConnectorDisconnect(const char *addr) {
+  char cmd[8];
+  strcpy(cmd, "CLOSE ");
+
+  for (int i = 0; i < 8; i++) {
+    if (strstr(iwrap->profiles[i].addr, addr) && (iwrap->profiles[i].linkid != 0xFF)) {
+      cmd[6] = iwrap->profiles[i].linkid + 0x30;
+      cmd[7]= '\0';
+      iwrap->bluetoothIwrapSendCommand(cmd);
+      BaseThread::sleep(MS2ST(2000));    // waiting disconnection
+      break;
+    }
+  }
+}
+
+void BluetoothConnector::bluetoothConnectorActivate(uint8_t linkid) {
+  if (strstr(typeDev, "SERIAL"))
+    ((BluetoothSerial *) objDev)->bluetoothSerialStart(linkid);
+  else
+    ((BluetoothWiimote *) objDev)->bluetoothWiimoteStart(linkid);
+}
+
+void BluetoothConnector::bluetoothConnectorDeactivate() {
+  if (strstr(typeDev, "SERIAL"))
+    ((BluetoothSerial *) objDev)->bluetoothSerialStop();
+  else
+    ((BluetoothWiimote *) objDev)->bluetoothWiimoteStop();
+}
diff --git a/components/bluetooth/bluetooth-descriptor.cpp b/components/bluetooth/bluetooth-descriptor.cpp
new file mode 100644
index 0000000..cb425b7
--- /dev/null
+++ b/components/bluetooth/bluetooth-descriptor.cpp
@@ -0,0 +1,44 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <amiro/bluetooth/bluetooth-descriptor.hpp>
+
+
+using namespace chibios_rt;
+using namespace amiro;
+
+
+BluetoothDescriptor::BluetoothDescriptor() :
+  linkId(0), length(0), buffer{0} {
+}
+
+//----------------------------------------------------------------
+
+
+void BluetoothDescriptor::bluetoothDescriptorSetLinkId(uint8_t linkId) {
+  this->linkId = linkId;
+}
+
+void BluetoothDescriptor::bluetoothDescriptorSetPayloadLength(size_t length) {
+  this->length = length;
+}
+
+uint8_t BluetoothDescriptor::bluetoothDescriptorGetLinkId() {
+  return this->linkId;
+}
+
+size_t BluetoothDescriptor::bluetoothDescriptorGetBufferLength() {
+  return sizeof(this->buffer);
+}
+
+size_t BluetoothDescriptor::bluetoothDescriptorGetPayloadLength() {
+  return this->length;
+}
+
+uint8_t* BluetoothDescriptor::bluetoothDescriptorGetBuffer() {
+  return this->buffer;
+}
+
+uint8_t* BluetoothDescriptor::bluetoothDescriptorGetPayload() {
+  return this->buffer;
+}
diff --git a/components/bluetooth/bluetooth-iwrap.cpp b/components/bluetooth/bluetooth-iwrap.cpp
new file mode 100644
index 0000000..f2925fb
--- /dev/null
+++ b/components/bluetooth/bluetooth-iwrap.cpp
@@ -0,0 +1,151 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <string.h>
+
+#include <amiro/bluetooth/bluetooth-iwrap.hpp>
+#include <amiro/bluetooth/bluetooth-connector.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+/*
+ * Class constructor
+ */
+BluetoothIwrap::BluetoothIwrap(UARTDriver* uart) :
+  BaseStaticThread<128>(), transport(uart), profiles{},
+  mailbox(mailboxBuffer, BLUETOOTH_IWRAP_MAILBOX_SIZE), connAddr{0} {
+  iwrapblock = 0;
+}
+
+//----------------------------------------------------------------
+
+msg_t BluetoothIwrap::main(void) {
+  setName("BluetoothIwrap");
+
+  // To set the receive mailbox[9] for plain communication (WT12->host)
+  transport.bluetoothTransportSetReceiveMailbox(HOST_IWRAP_PLAIN_LINKID, &this->mailbox);
+
+  transport.start(NORMALPRIO);
+
+  while (!this->shouldTerminate()) {
+    iwrapReceive();
+  }
+  return RDY_OK;
+}
+
+/*
+ * Member functions
+ */
+msg_t BluetoothIwrap::iwrapReceive() {
+  BluetoothDescriptor* descriptor = NULL;
+  const char* buffer;
+  size_t length;
+  msg_t msg;
+
+  msg = mailbox.fetch((msg_t*) &descriptor, TIME_INFINITE);
+  if (msg == RDY_RESET)
+    return RDY_RESET;
+
+  buffer = (const char*)descriptor->bluetoothDescriptorGetPayload();
+  length = descriptor->bluetoothDescriptorGetPayloadLength();
+
+  for (size_t i = 0; i < length; i++) {
+    // print out received chars
+    chSequentialStreamPut((BaseSequentialStream*) &SD1, buffer[i]);
+  }
+
+  // reset block
+  iwrapblock = 0;
+
+  // if RING or CONNECT event is received, then look for profile with correct MAC-address and connect.
+  if (strstr(buffer, "CONNECT ") && (strstr(buffer, " RFCOMM 1") || strstr(buffer, " L2CAP 1"))) {
+  // if CONNECT event serial RFCOMM 1 or wiimote (control) L2CAP 11 or wiimote (data) L2CAP 13
+    for (int i = 0; i < 8; i++) {
+      if ((profiles[i].addr != NULL) && strstr(connAddr, profiles[i].addr)) {
+        BluetoothProfile* bp =  &(profiles[i]);
+        bp->linkid = buffer[8] - '0';     // look for linkid and convert linkid to int
+        (((BluetoothConnector*)(bp->connector))->*(bp->connect))(bp->linkid);
+        break;
+      }
+    }
+  } else if (strstr(buffer, "RING") && strstr(buffer, "1 RFCOMM ")) {
+  // if RING event (be called by other device) in 1 RFCOMM mode
+    for (int i = 0; i < 8; i++) {
+      if ((profiles[i].addr != NULL) && (strstr(buffer, profiles[i].addr) || strstr(profiles[i].addr, "free"))) {
+        BluetoothProfile* bp =  &(profiles[i]);
+        bp->linkid  = buffer[5] - '0';    // look for linkid and convert linkid to int
+        (((BluetoothConnector*)(bp->connector))->*(bp->connect))(bp->linkid );
+        break;
+      }
+    }
+  } else if (strstr(buffer, "NO CARRIER ") && strstr(buffer, "ERROR ")) {
+  // if NO CARRIER ERROR 0 event, then look for profile and disconnect
+    uint8_t linkid = buffer[11] - '0';
+    for (int i = 0; i < 8; i++) {
+      if ((profiles[i].addr != NULL) && (profiles[i].linkid == linkid)) {
+        BluetoothProfile* bp =  &(profiles[i]);
+        bp->linkid = 0xFF;
+        (((BluetoothConnector*)(bp->connector))->*(bp->disconnect))();
+        break;
+      }
+    }
+  }
+
+  msg = (transport.bluetoothTransportGetStorageMailbox())->post((msg_t) descriptor, TIME_INFINITE);
+  if (msg == RDY_RESET)
+    return RDY_RESET;
+
+  return RDY_OK;
+}
+
+msg_t BluetoothIwrap::iwrapTransmit(uint8_t linkid, const uint8_t* txdata, size_t length) {
+  BluetoothDescriptor* descriptor = NULL;
+  uint8_t *buffer;
+  msg_t msg;
+
+  msg = transport.bluetoothTransportGetStorageMailbox()->fetch((msg_t*) &descriptor, TIME_INFINITE);
+  if (msg == RDY_RESET)
+    return RDY_RESET;
+
+  buffer = descriptor->bluetoothDescriptorGetPayload();
+  memcpy(buffer, txdata, length);
+
+  descriptor->bluetoothDescriptorSetLinkId(linkid);
+  descriptor->bluetoothDescriptorSetPayloadLength(length);
+
+  msg = transport.bluetoothTransportGetTransmitMailbox()->post((msg_t) descriptor, TIME_INFINITE);
+  if (msg == RDY_RESET)
+    return RDY_RESET;
+
+  return RDY_OK;
+}
+
+void BluetoothIwrap::bluetoothIwrapSendCommand(const char* cmd) {
+  // Wait until another previous command finished.
+  while(iwrapblock == 1) {
+    BaseThread::sleep(MS2ST(500));
+  }
+
+  iwrapblock = 1;
+
+  if (strstr(cmd, "CALL ") && (strstr(cmd, " RFCOMM") || strstr(cmd, " L2CAP"))) {
+    memcpy(connAddr, cmd+5, 17);
+  }
+
+  size_t length = strlen(cmd);
+//  for (size_t i = 0; i < length; i++)
+//    chSequentialStreamPut((BaseSequentialStream*) &SD1, cmd[i]);   // To display the command
+
+  // To send the string command to WT12 bluetooth chip
+  iwrapTransmit(HOST_IWRAP_PLAIN_LINKID,(uint8_t*) cmd, length);
+
+  // Clear iwrapblock flag because these commands don't return an answer
+  if(strstr(cmd, "SET BT NAME") || strstr(cmd, "SET BT AUTH") || strstr(cmd, "SET BT SSP")
+    || strstr(cmd, "SET BT PAGEMODE") || strstr(cmd, "SET BT PAIR *")
+    || strstr(cmd, "SET CONTROL MUX") || (strstr(cmd, "SET") && strlen(cmd) == 3)) {
+    iwrapblock = 0;
+  }
+
+  BaseThread::sleep(MS2ST(500));
+}
diff --git a/components/bluetooth/bluetooth-serial.cpp b/components/bluetooth/bluetooth-serial.cpp
new file mode 100644
index 0000000..9efe73b
--- /dev/null
+++ b/components/bluetooth/bluetooth-serial.cpp
@@ -0,0 +1,103 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <amiro/bluetooth/bluetooth-serial.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+/*
+ * Class constructor
+ */
+BluetoothSerial::BluetoothSerial(BLUETOOTH *bluetooth, uint8_t rxtx) :
+  BaseStaticThread<128>(), serialConn(bluetooth, this, "SERIAL"),
+  mailbox(mailboxBuffer, BLUETOOTH_SERIAL_MAILBOX_SIZE) {
+  iwrap = &(bluetooth->iwrap);
+  rx_tx = rxtx;
+  linkId = 0xFF;
+  stopflag = 0;
+}
+
+//----------------------------------------------------------------
+
+msg_t BluetoothSerial::main(void) {
+  setName("BluetoothSerial");
+
+  while (!this->shouldTerminate()) {
+    if (serialReceive())
+      this->requestTerminate();
+  }
+  return RDY_OK;
+}
+
+/*
+ * Member functions
+ */
+msg_t BluetoothSerial::serialReceive() {
+  BluetoothDescriptor* recv_descriptor = NULL;
+  uint8_t *buffer;
+  size_t length;
+  msg_t msg;
+
+  msg = mailbox.fetch((msg_t*) &recv_descriptor, TIME_INFINITE);
+  if ((msg == RDY_RESET) || stopflag)
+    return RDY_RESET;
+
+  buffer = recv_descriptor->bluetoothDescriptorGetPayload();
+  length = recv_descriptor->bluetoothDescriptorGetPayloadLength();
+
+  for (size_t i = 0; i < length; i++)
+    chSequentialStreamPut((BaseSequentialStream*) &SD1, buffer[i]);
+
+  msg = iwrap->transport.bluetoothTransportGetStorageMailbox()->post((msg_t) recv_descriptor, TIME_INFINITE);
+  if ((msg == RDY_RESET) || stopflag)
+    return RDY_RESET;
+
+  return RDY_OK;
+}
+
+msg_t BluetoothSerial::serialTransmit(const uint8_t* serialdata, size_t length) {
+  msg_t msg;
+
+  if ((rx_tx & 0x01) && (linkId != 0xFF) && (!stopflag))
+    msg = iwrap->iwrapTransmit(linkId, serialdata, length);
+  else
+    msg = RDY_RESET;
+
+  return msg;
+}
+
+void BluetoothSerial::bluetoothSerialStart(uint8_t linkid) {
+  linkId = linkid;
+  iwrap->transport.bluetoothTransportSetReceiveMailbox(linkId, &this->mailbox);
+
+  if ((rx_tx & 0x02)) { // && (!stopflag)) {
+    this->start(NORMALPRIO);
+  }
+  stopflag = 0;
+}
+
+void BluetoothSerial::bluetoothSerialStop() {
+  linkId = 0xFF;
+  stopflag = 1;
+  mailbox.post(RDY_RESET, TIME_INFINITE);        // TIME_IMMEDIATE TIME_INFINITE
+}
+
+bool BluetoothSerial::bluetoothSerialIsConnected() {
+  if (linkId == 0xFF)
+    return false;
+
+  return true;
+}
+
+void BluetoothSerial::bluetoothSerialListen(const char *addr) {
+  serialConn.bluetoothConnectorListen(addr);
+}
+
+void BluetoothSerial::bluetoothSerialConnect(const char *addr){
+  serialConn.bluetoothConnectorConnect(addr);
+}
+
+void BluetoothSerial::bluetoothSerialDisconnect(const char *addr) {
+  serialConn.bluetoothConnectorDisconnect(addr);
+}
diff --git a/components/bluetooth/bluetooth-transport.cpp b/components/bluetooth/bluetooth-transport.cpp
new file mode 100644
index 0000000..1b8a740
--- /dev/null
+++ b/components/bluetooth/bluetooth-transport.cpp
@@ -0,0 +1,330 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <uart.h>
+#include <chprintf.h>
+
+#include <amiro/bluetooth/bluetooth-transport.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+/*
+ * Class constructor
+ */
+BluetoothTransport::BluetoothTransport(UARTDriver* uart) :
+  BaseStaticThread<128>(), UARTConfig(), uart(uart), receiveMailbox{NULL},
+  storageMailbox(storageMailboxBuffer, BLUETOOTH_TRANSPORT_STORAGE_MAILBOX_SIZE),
+  transmitMailbox(transmitMailboxBuffer, BLUETOOTH_TRANSPORT_TRANSMIT_MAILBOX_SIZE) {
+
+  UARTConfig::txend1_cb = bluetoothTransportTransmitCompleted_cb;
+  UARTConfig::txend2_cb = NULL;
+  UARTConfig::rxend_cb  = bluetoothTransportReceiveCompleted_cb;
+  UARTConfig::rxchar_cb = bluetoothTransportCharacterReceived_cb;
+  UARTConfig::rxerr_cb  = NULL;
+  speed = 115200;
+  cr1   = 0;
+  cr2   = 0;
+  cr3   = 0;
+
+  transmitState = BLUETOOTH_TRANSPORT_PLAIN;
+  receiveState = BLUETOOTH_TRANSPORT_PLAIN;
+
+  transmit = NULL;
+  receive = NULL;
+}
+
+//----------------------------------------------------------------
+
+msg_t BluetoothTransport::main(void) {
+  setName("BluetoothTransport");
+
+  for (int i = 0; i < BLUETOOTH_TRANSPORT_STORAGE_MAILBOX_SIZE; i++)
+    storageMailbox.post((msg_t) &descriptor[i], TIME_INFINITE);
+
+  BaseThread::sleep(MS2ST(200));
+  uartStart(uart, this);
+
+  while (!this->shouldTerminate()) {
+
+    if (!this->transmitMailbox.fetch((msg_t *) &transmit, TIME_INFINITE)) {
+
+      if (!transmit->bluetoothDescriptorGetPayloadLength()) {
+        storageMailbox.post((msg_t) transmit, TIME_INFINITE);
+            continue;
+      }
+
+      if (transmitState == BLUETOOTH_TRANSPORT_PLAIN) {
+        uint8_t* buffer = transmit->bluetoothDescriptorGetPayload();
+        size_t length = transmit->bluetoothDescriptorGetPayloadLength();
+
+        memcpy(buffer+length, "\r\n", 2);
+
+        uartStartSend(uart, length + 2, buffer);
+      } else {
+        transmitState = BLUETOOTH_TRANSPORT_PAYLOAD;
+        uartStartSend(uart, FRAME_HEADER_SIZE, getEncodeHeader());
+      }
+      waitAnyEvent((eventmask_t) TRANSMIT_COMPLETED_EVENT);
+    }
+  }
+  return RDY_OK;
+}
+
+/*
+ * Interrupt callback handlers
+ */
+void BluetoothTransport::bluetoothTransportTransmitCompleted_cb(UARTDriver *uart) {
+  BluetoothTransport *u = (BluetoothTransport *) uart->config;
+  u->bluetoothTransportTransmitCompleted();
+}
+
+void BluetoothTransport::bluetoothTransportTransmitCompleted() {
+  switch (transmitState) {
+    case BLUETOOTH_TRANSPORT_PLAIN: {
+      postTransmitDescriptorI();
+
+      System::lockFromIsr();
+      signalEventsI((eventmask_t) TRANSMIT_COMPLETED_EVENT);
+      System::unlockFromIsr();
+      break;
+    }
+    case BLUETOOTH_TRANSPORT_HEADER: {
+      System::lockFromIsr();
+      uartStartSendI(uart, FRAME_HEADER_SIZE, getEncodeHeader());
+      System::unlockFromIsr();
+      transmitState = BLUETOOTH_TRANSPORT_PAYLOAD;
+      /* no break */
+    }
+    case BLUETOOTH_TRANSPORT_PAYLOAD: {
+      System::lockFromIsr();
+      uartStartSendI(uart, transmit->bluetoothDescriptorGetPayloadLength(), transmit->bluetoothDescriptorGetPayload());
+      System::unlockFromIsr();
+      transmitState = BLUETOOTH_TRANSPORT_TRAILER;
+      break;
+    }
+    case BLUETOOTH_TRANSPORT_TRAILER: {
+      postTransmitDescriptorI();
+
+      System::lockFromIsr();
+      uartStartSendI(uart, FRAME_TRAILER_SIZE, getEncodeTailer());
+
+      // check whether any new transmision in the mailbox during the present tranmmision
+      msg_t msg = transmitMailbox.fetchI((msg_t *) &transmit);
+      System::unlockFromIsr();
+
+      if (msg == RDY_OK) {
+        if (transmit->bluetoothDescriptorGetPayloadLength()) {
+          transmitState = BLUETOOTH_TRANSPORT_HEADER;
+        } else {
+          postTransmitDescriptorI();
+          transmitState = BLUETOOTH_TRANSPORT_IDLE;
+        }
+      } else {
+        transmitState = BLUETOOTH_TRANSPORT_IDLE;
+      }
+      break;
+    }
+    case BLUETOOTH_TRANSPORT_IDLE: {
+      System::lockFromIsr();
+      signalEventsI((eventmask_t) TRANSMIT_COMPLETED_EVENT);
+      System::unlockFromIsr();
+      break;
+    }
+    default: {
+      break;
+    }
+  }
+}
+
+void BluetoothTransport::bluetoothTransportCharacterReceived_cb(UARTDriver *uart, uint16_t c) {
+  BluetoothTransport *u = (BluetoothTransport *) uart->config;
+  u->bluetoothTransportCharacterReceived(c);
+}
+
+void BluetoothTransport::bluetoothTransportCharacterReceived(uint16_t c) {
+  if (c == 0xbf) {
+    receiveFrameBuffer[1] = 0xbf;
+    System::lockFromIsr();
+    uartStartReceiveI(uart, 3, (void *) &receiveFrameBuffer[2]);
+    System::unlockFromIsr();
+
+    receiveState = BLUETOOTH_TRANSPORT_DECODE;
+    if (transmitState == BLUETOOTH_TRANSPORT_PLAIN)
+      transmitState = BLUETOOTH_TRANSPORT_IDLE;
+  }
+
+  if (receiveState != BLUETOOTH_TRANSPORT_PLAIN)
+    return;
+
+  if (!receive) {
+    System::lockFromIsr();
+    msg_t msg = storageMailbox.fetchI((msg_t *) &receive);
+    System::unlockFromIsr();
+
+    if (msg)
+      return;
+
+    receive->bluetoothDescriptorSetLinkId(0xFF);
+    rcvlength = 0;
+  }
+
+  uint8_t* buffer = receive->bluetoothDescriptorGetPayload();
+  buffer[rcvlength++] = c;
+
+  if (rcvlength == receive->bluetoothDescriptorGetBufferLength() || c == '\n') {
+    receive->bluetoothDescriptorSetPayloadLength(rcvlength);
+    postReceiveDescriptorI();
+  }
+}
+
+void BluetoothTransport::bluetoothTransportReceiveCompleted_cb(UARTDriver *uart) {
+  BluetoothTransport *u = (BluetoothTransport *) uart->config;
+  u->bluetoothTransportReceiveCompleted();
+}
+
+void BluetoothTransport::bluetoothTransportReceiveCompleted() {
+  switch (receiveState) {
+    case BLUETOOTH_TRANSPORT_HEADER: {
+      if ((uint8_t) ~receiveFrameBuffer[0] != receive->bluetoothDescriptorGetLinkId()) {
+        receiveState = BLUETOOTH_TRANSPORT_IDLE;
+        break;
+      }
+      System::lockFromIsr();
+      uartStartReceiveI(uart, 4, (void *) &receiveFrameBuffer[1]);
+      System::unlockFromIsr();
+
+      receiveState = BLUETOOTH_TRANSPORT_DECODE;
+      break;
+    }
+    case BLUETOOTH_TRANSPORT_DECODE: {
+      if (decodeHeader()) {
+        receiveState = BLUETOOTH_TRANSPORT_IDLE;
+        break;
+      }
+      receiveState = BLUETOOTH_TRANSPORT_PAYLOAD;
+      /* no break */
+    }
+    case BLUETOOTH_TRANSPORT_PAYLOAD: {
+      if (receive)
+        postReceiveDescriptorI();
+
+      System::lockFromIsr();
+      msg_t msg = storageMailbox.fetchI((msg_t *) &receive);
+      System::unlockFromIsr();
+
+      if (msg) {
+        receiveState = BLUETOOTH_TRANSPORT_IDLE;
+        break;
+      }
+
+      receive->bluetoothDescriptorSetLinkId(receiveFrameBuffer[2]);
+      if (rcvlength > receive->bluetoothDescriptorGetBufferLength())
+        receive->bluetoothDescriptorSetPayloadLength(receive->bluetoothDescriptorGetBufferLength());
+      else
+        receive->bluetoothDescriptorSetPayloadLength(rcvlength);
+
+      System::lockFromIsr();
+      uartStartReceiveI(uart, receive->bluetoothDescriptorGetPayloadLength(), receive->bluetoothDescriptorGetPayload());
+      System::unlockFromIsr();
+
+      rcvlength -= receive->bluetoothDescriptorGetPayloadLength();
+
+      if (!rcvlength)
+        receiveState = BLUETOOTH_TRANSPORT_TRAILER;
+
+      break;
+    }
+    case BLUETOOTH_TRANSPORT_TRAILER: {
+      postReceiveDescriptorI();
+
+      System::lockFromIsr();
+      uartStartReceiveI(uart, 1, (void *) &receiveFrameBuffer[0]);
+      System::unlockFromIsr();
+
+      receiveState = BLUETOOTH_TRANSPORT_HEADER;
+      break;
+    }
+    default: {
+      break;
+    }
+  }
+}
+
+/*
+ * Member functions
+ */
+
+void BluetoothTransport::bluetoothTransportResetState() {
+  transmitState = BLUETOOTH_TRANSPORT_PLAIN;
+  receiveState = BLUETOOTH_TRANSPORT_PLAIN;
+}
+
+bool BluetoothTransport::bluetoothTransportIsMuxMode() {
+  if (receiveState == BLUETOOTH_TRANSPORT_PLAIN)
+    return false;
+
+  return true;
+}
+
+chibios_rt::Mailbox* BluetoothTransport::bluetoothTransportGetStorageMailbox() {
+  return &storageMailbox;
+}
+
+chibios_rt::Mailbox* BluetoothTransport::bluetoothTransportGetTransmitMailbox() {
+  return &transmitMailbox;
+}
+
+void BluetoothTransport::bluetoothTransportSetReceiveMailbox(uint8_t linkId, chibios_rt::Mailbox* mailbox) {
+  System::lock();
+  receiveMailbox[(linkId > 9) ? 9 : linkId] = mailbox;
+  System::unlock();
+}
+
+void BluetoothTransport::postReceiveDescriptorI() {
+  uint8_t linkId = receive->bluetoothDescriptorGetLinkId();
+  linkId > 9 ? linkId = 9 : linkId;
+
+  System::lockFromIsr();
+  msg_t msg = receiveMailbox[linkId]->postI((msg_t) receive);
+  if (msg)
+    storageMailbox.postI((msg_t) receive);
+  receive = NULL;
+  System::unlockFromIsr();
+}
+
+void BluetoothTransport::postTransmitDescriptorI() {
+  System::lockFromIsr();
+  storageMailbox.postI((msg_t) transmit);
+  transmit = NULL;
+  System::unlockFromIsr();
+}
+
+/*
+ * The header encode/decode are in compliance with of the multiplexing mode
+ * Reference: iWRAP4 user guide Version 4.1 page 142-143
+ */
+uint8_t*  BluetoothTransport::getEncodeHeader() {
+  transmitFrameBuffer[1] = 0xbf;
+  transmitFrameBuffer[2] = transmit->bluetoothDescriptorGetLinkId();
+  transmitFrameBuffer[3] = (transmit->bluetoothDescriptorGetPayloadLength() >> 8) & 0x3;
+  transmitFrameBuffer[4] = transmit->bluetoothDescriptorGetPayloadLength() & 0xFF;
+  return &transmitFrameBuffer[1];
+}
+
+uint8_t* BluetoothTransport::getEncodeTailer() {
+  transmitFrameBuffer[0] = ~transmitFrameBuffer[2];
+  return &transmitFrameBuffer[0];
+}
+
+int BluetoothTransport::decodeHeader() {
+  if ((receiveFrameBuffer[1] != 0xbf) ||
+    ((receiveFrameBuffer[2] > 8) && (receiveFrameBuffer[2] != 0xFF)))
+    return -1;
+
+  rcvlength = (receiveFrameBuffer[3] << 8) | receiveFrameBuffer[4];
+  if (rcvlength > 1023)
+    return -1;
+
+  return RDY_OK;
+}
diff --git a/components/bluetooth/bluetooth-wiimote.cpp b/components/bluetooth/bluetooth-wiimote.cpp
new file mode 100644
index 0000000..d96e59a
--- /dev/null
+++ b/components/bluetooth/bluetooth-wiimote.cpp
@@ -0,0 +1,216 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <amiro/bluetooth/bluetooth-wiimote.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+/*
+ * Class constructor
+ */
+BluetoothWiimote::BluetoothWiimote(BLUETOOTH *bluetooth, uint8_t rxtx) :
+  BaseStaticThread<128>(), wiimoteConn(bluetooth, this, "WIIMOTE"),
+  mailbox(mailboxBuffer, BLUETOOTH_WIIMOTE_MAILBOX_SIZE), accelerometer{0,0,0} {
+  iwrap = &(bluetooth->iwrap);
+  rx_tx = rxtx;
+  linkId = 0xFF;
+  stopflag = 0;
+}
+
+//----------------------------------------------------------------
+
+msg_t BluetoothWiimote::main(void) {
+  setName("BluetoothWiimote");
+
+  while (!this->shouldTerminate()) {
+    if (wiimoteReceive())
+      this->requestTerminate();
+  }
+  return RDY_OK;
+}
+
+/*
+ * Member functions
+ */
+msg_t BluetoothWiimote::wiimoteReceive() {
+  BluetoothDescriptor* recv_descriptor = NULL;
+  uint8_t *buffer;
+  size_t length;
+  msg_t msg;
+
+  static uint8_t button_up;
+  static uint8_t button_down;
+  static uint8_t button_right;
+  static uint8_t button_left;
+  static uint8_t button_plus;
+  static uint8_t button_home;
+  static uint8_t button_minus;
+  static uint8_t button_A;
+  static uint8_t button_B;
+  static uint8_t button_1;
+  static uint8_t button_2;
+
+  msg = mailbox.fetch((msg_t*) &recv_descriptor, TIME_INFINITE);
+  if ((msg == RDY_RESET) || stopflag)
+    return RDY_RESET;
+
+  buffer = recv_descriptor->bluetoothDescriptorGetPayload();
+  length = recv_descriptor->bluetoothDescriptorGetPayloadLength();
+
+  if (buffer[0] == 0xA1 && buffer[1] == 0x31) {
+    accelerometer.x_axis = (buffer[4] << 2) + ((buffer[2] & 0x60) >> 5) - 0x1EC;
+    accelerometer.y_axis = (buffer[5] << 2) + ((buffer[3] & 0x20) >> 4) - 0x1EA;
+    accelerometer.z_axis = (buffer[6] << 2) + ((buffer[3] & 0x40) >> 5) - 0x1EE;
+
+    if (buffer[3] & 0x80) {             // Press home to return button reporting
+      bluetoothWiimoteDataBtn();
+      accelerometer.x_axis = 0;
+      accelerometer.y_axis = 0;
+      accelerometer.z_axis = 0;
+    }
+
+  } else if (buffer[0] == 0xA1 && buffer[1] == 0x30) {
+    button_up    = (buffer[2] & 0x08) >> 3;
+    button_down  = (buffer[2] & 0x04) >> 2;
+    button_right = (buffer[2] & 0x02) >> 1;
+    button_left  = (buffer[2] & 0x01) >> 0;
+    button_plus  = (buffer[2] & 0x10) >> 4;
+    button_home  = (buffer[3] & 0x80) >> 7;
+    button_minus = (buffer[3] & 0x10) >> 4;
+    button_A     = (buffer[3] & 0x08) >> 3;
+    button_B     = (buffer[3] & 0x04) >> 2;
+    button_1     = (buffer[3] & 0x02) >> 1;
+    button_2     = (buffer[3] & 0x01) >> 0;
+
+    if (button_up)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'U');
+
+    if (button_down)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'D');
+
+    if (button_right)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'R');
+
+    if (button_left)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'L');
+
+    if (button_plus)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, '+');
+
+    if (button_home)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'H');
+
+    if (button_minus)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, '-');
+
+    if (button_A)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'A');
+
+    if (button_B)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'B');
+
+    if (button_1)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, '1');
+
+    if (button_2)
+      chSequentialStreamPut((BaseSequentialStream*) &SD1, '2');
+
+    if (button_minus && button_plus)   // Press minus and plue to return accelerometer reporting
+      bluetoothWiimoteDataBtnAcc();
+  } else {
+    chSequentialStreamWrite((BaseSequentialStream*) &SD1, buffer, length);
+  }
+
+  msg = iwrap->transport.bluetoothTransportGetStorageMailbox()->post((msg_t) recv_descriptor, TIME_INFINITE);
+  if ((msg == RDY_RESET) || stopflag)
+    return RDY_RESET;
+
+  return RDY_OK;
+}
+
+msg_t BluetoothWiimote::wiimoteTransmit(const uint8_t* wiimotecmd, size_t length) {
+  msg_t msg;
+
+  if ((rx_tx & 0x01) && (linkId != 0xFF) && (!stopflag))
+    msg = iwrap->iwrapTransmit(linkId, wiimotecmd, length);
+  else
+    msg = RDY_RESET;
+
+  return msg;
+}
+
+void BluetoothWiimote::bluetoothWiimoteStart(uint8_t linkid) {
+  linkId = linkid;
+  iwrap->transport.bluetoothTransportSetReceiveMailbox(linkId, &this->mailbox);
+
+  if ((rx_tx & 0x02)) {   // && (!stopflag)
+    this->start(NORMALPRIO);
+  }
+  stopflag = 0;
+}
+
+void BluetoothWiimote::bluetoothWiimoteStop() {
+  linkId = 0xFF;
+  stopflag = 1;
+  mailbox.post(RDY_RESET, TIME_INFINITE);        // TIME_IMMEDIATE TIME_INFINITE
+}
+
+bool BluetoothWiimote::bluetoothWiimoteIsConnected() {
+  if (linkId == 0xFF)
+    return false;
+
+  return true;
+}
+
+void BluetoothWiimote::bluetoothWiimoteListen(const char *addr) {
+  wiimoteConn.bluetoothConnectorListen(addr);
+}
+
+void BluetoothWiimote::bluetoothWiimoteConnect(const char *addr) {
+  wiimoteConn.bluetoothConnectorConnect(addr);
+}
+
+void BluetoothWiimote::bluetoothWiimoteDisconnect(const char *addr) {
+  wiimoteConn.bluetoothConnectorDisconnect(addr);
+}
+
+BluetoothWiimote::Accelerometer * BluetoothWiimote::getAccelerometer() {
+  return &accelerometer;
+}
+
+/*
+ * @brief :  On-off LEDs and Motor of Wiimote.
+ *
+ * @param[in] ledid_flag     On-off flag of LEDs 1,2,3,4
+ *                           (LED1: 0x10, LED2: 0x20, LED3: 0x40, LED4: 0x80)
+ * @param[in] vibrate_flag   On-off flag of Vibration Motor (On: 1, off: 0)
+ */
+void BluetoothWiimote::bluetoothWiimoteLedVibrate(uint8_t ledid_flag, uint8_t vibrate_flag) {
+  const uint8_t data[] = {0xA2, 0x11, (uint8_t) (ledid_flag | vibrate_flag)};
+  wiimoteTransmit(data, 3);
+}
+
+/*
+ * @brief :  Show status information of Wiimote.
+ */
+void BluetoothWiimote::bluetoothWiimoteStatusInfo() {
+  const uint8_t data[] = {0xA2, 0x15, 0x00};
+  wiimoteTransmit(data, 3);
+}
+
+/*
+ * @brief :  Send button status (change event) of Wiimote.
+ */
+void BluetoothWiimote::bluetoothWiimoteDataBtn() {
+  const uint8_t data[] = {0xA2, 0x12, 0x00, 0x30};
+  wiimoteTransmit(data, 4);
+}
+
+/*
+ * @brief :  Send button & accelerometer status (change event) of Wiimote.
+ */
+void BluetoothWiimote::bluetoothWiimoteDataBtnAcc() {
+  const uint8_t data[] = {0xA2, 0x12, 0x00, 0x31};
+  wiimoteTransmit(data, 4);
+}
diff --git a/components/bluetooth/bluetooth.cpp b/components/bluetooth/bluetooth.cpp
new file mode 100644
index 0000000..d3e8bf9
--- /dev/null
+++ b/components/bluetooth/bluetooth.cpp
@@ -0,0 +1,82 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <string.h>
+
+#include <amiro/bluetooth/bluetooth.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+/*
+ * Class constructor
+ */
+BLUETOOTH::BLUETOOTH(UARTDriver* uart) : iwrap(uart) {
+}
+
+/*
+ * Member functions
+ */
+void BLUETOOTH::bluetoothStart() {
+  iwrap.start(NORMALPRIO);
+  BaseThread::sleep(MS2ST(1000));
+  // Reset bluetooth chip to get the first msg. (cmd/data mode or mux Mode)
+  bluetoothReset();
+}
+
+void BLUETOOTH::bluetoothReset() {
+  palWritePad(GPIOC, GPIOC_BT_RST, 1);
+  BaseThread::sleep(MS2ST(10));
+  palWritePad(GPIOC, GPIOC_BT_RST, 0);
+  iwrap.transport.bluetoothTransportResetState();
+    BaseThread::sleep(MS2ST(1000));
+}
+
+void BLUETOOTH::bluetoothEnableMux() {
+  iwrap.bluetoothIwrapSendCommand("SET CONTROL MUX 1");
+}
+
+void BLUETOOTH::bluetoothDisableMux() {
+  iwrap.bluetoothIwrapSendCommand("SET CONTROL MUX 0");
+}
+
+void BLUETOOTH::bluetoothDiscoverDevices() {
+  iwrap.bluetoothIwrapSendCommand("INQUIRY 5");
+  BaseThread::sleep(MS2ST(6000));
+}
+
+void BLUETOOTH::bluetoothListAllConnections() {
+  iwrap.bluetoothIwrapSendCommand("List");
+}
+
+void BLUETOOTH::bluetoothSetName(const char *name) {
+  int size = strlen(name) + 14;
+  char cmd[size];
+  strcpy(cmd, "SET BT NAME ");
+  strcat(cmd, name);
+  iwrap.bluetoothIwrapSendCommand(cmd);
+}
+
+void BLUETOOTH::bluetoothSetPin(const char *pin) {
+  int size = strlen(pin) + 15;
+  char cmd[size];
+  strcpy(cmd, "SET BT AUTH * ");
+  strcat(cmd, pin);
+  iwrap.bluetoothIwrapSendCommand(cmd);
+}
+
+void BLUETOOTH::bluetoothCloseConnection(uint8_t linkid) {
+  char cmd[8];
+  strcpy(cmd, "CLOSE ");
+  cmd[6] = linkid + '0';
+  cmd[7]= '\0';
+  iwrap.bluetoothIwrapSendCommand(cmd);
+}
+
+void BLUETOOTH::bluetoothSendCommand(const char *cmd) {
+  iwrap.bluetoothIwrapSendCommand(cmd);
+}
+
+bool BLUETOOTH::bluetoothIsMuxMode() {
+  return iwrap.transport.bluetoothTransportIsMuxMode();
+}
diff --git a/components/bluetooth/checkutility.cpp b/components/bluetooth/checkutility.cpp
new file mode 100644
index 0000000..114436e
--- /dev/null
+++ b/components/bluetooth/checkutility.cpp
@@ -0,0 +1,243 @@
+#include <amiro/bluetooth/checkutility.hpp>
+
+#include <chprintf.h>
+#include <hal.h>
+
+#include <cstring>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+WT12CheckUtility::WT12CheckUtility()
+{}
+
+WT12CheckUtility::~WT12CheckUtility()
+{}
+
+void
+WT12CheckUtility::reset()
+{
+  palWritePad(GPIOC, GPIOC_BT_RST, PAL_HIGH);
+  BaseThread::sleep(MS2ST(10));
+  palWritePad(GPIOC, GPIOC_BT_RST, PAL_LOW);
+
+  return;
+}
+
+bool
+WT12CheckUtility::waitForReady(const bool print)
+{
+  char buffer[8];
+  const uint16_t length = this->receiveMessage(buffer, 8);
+
+  if (print) {
+    msgPrint(buffer, length);
+  }
+
+  return (strcmp(buffer, "READY.\r\n") == 0);
+}
+
+uint8_t
+WT12CheckUtility::selftest()
+{
+  return 0;
+}
+
+void
+WT12CheckUtility::msgPrint(const char* message, const uint16_t length)
+{
+  if (length > 0)
+  {
+    chSequentialStreamWrite((BaseSequentialStream*)&SD1, (uint8_t*)message, length);
+//    chprintf((BaseSequentialStream*) &SD1, "[%d]\n", length);
+  }
+  return;
+}
+
+void
+WT12CheckUtility::msgPrintMulti(const char* message_buf, const uint16_t* length_buf, const uint16_t num_messages)
+{
+  uint16_t pos = 0;
+  for (uint16_t i = 0; i < num_messages; ++i)
+  {
+    this->msgPrint(&message_buf[pos], length_buf[i]);
+    pos += length_buf[i];
+  }
+
+  return;
+}
+
+void
+WT12CheckUtility::sendMessage(char* message, uint8_t link_id)
+{
+  const uint16_t length = strlen(message);
+  if (length >= 1 << 10) {
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message too long!\n", __FILE__ , __LINE__);
+    return;
+  }
+
+  Message msg;
+  msg.head.start_of_frame = MUX_StartOfFrame;
+  msg.head.link_id = link_id;
+  msg.head.flags = 0x00u;
+  msg.head.length = length;
+  msg.message = message;
+  msg.n_link = ~link_id;
+
+  chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.head.start_of_frame);
+  chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.head.link_id);
+  chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.head.flags | (msg.head.length >> 8));
+  chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.head.length & 0xFFu);
+  chSequentialStreamWrite((BaseSequentialStream*)&SD3, (uint8_t*)msg.message, msg.head.length);
+  chSequentialStreamPut((BaseSequentialStream*)&SD3, msg.n_link);
+
+  return;
+}
+
+void
+WT12CheckUtility::sendMessageRaw(const char* message)
+{
+  chSequentialStreamWrite((BaseSequentialStream*)&SD3, (uint8_t*)message, strlen(message));
+  return;
+}
+
+uint16_t
+WT12CheckUtility::receiveMessage(char* message, const uint16_t max_length)
+{
+  Message msg;
+  msg.message = message;
+
+  msg.head.start_of_frame = chSequentialStreamGet((BaseSequentialStream*)&SD3);
+  msg.head.link_id = chSequentialStreamGet((BaseSequentialStream*)&SD3);
+  msg.head.flags = chSequentialStreamGet((BaseSequentialStream*)&SD3);
+  msg.head.length = (msg.head.flags & 0x03u) << 8;
+  msg.head.flags = msg.head.flags >> 2;
+  msg.head.length += uint8_t(chSequentialStreamGet((BaseSequentialStream*)&SD3));
+
+  if (msg.head.length > max_length)
+  {
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message too long (%d of max %d bytes)!\n", __FILE__ , __LINE__, msg.head.length, max_length);
+
+    for (uint16_t i = 0; i < msg.head.length; ++i) {
+      chSequentialStreamGet((BaseSequentialStream*)&SD3);
+    }
+    msg.head.length = 0;
+  }
+  else
+  {
+    chSequentialStreamRead((BaseSequentialStream*)&SD3, (uint8_t*)msg.message, msg.head.length);
+  }
+
+  msg.n_link = chSequentialStreamGet((BaseSequentialStream*)&SD3);
+
+  if (uint8_t(msg.head.link_id ^ msg.n_link) != 0xFFu)
+  {
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: message link id does not match (0x%02X vs 0x%02X)!\n", __FILE__ , __LINE__, msg.head.link_id, msg.n_link);
+  }
+
+  return msg.head.length;
+}
+
+uint16_t
+WT12CheckUtility::receiveMessages(char* message_buf, const uint16_t msg_buf_size, uint16_t* length_buf, const uint16_t len_buf_size, const char* last_msg, const char* filter_in, const char* filter_out)
+{
+  if (msg_buf_size == 0 || len_buf_size == 0) {
+    return 0;
+  }
+
+  for (uint16_t i = 0; i < len_buf_size; ++i) {
+    length_buf[i] = 0;
+  }
+
+  uint16_t msg_pos = 0;
+  uint16_t msg_count = 0;
+  while (true)
+  {
+    length_buf[msg_count] = this->receiveMessage(&message_buf[msg_pos], msg_buf_size-msg_pos);
+
+//    if (msg_count >= 0) {
+//      msgPrint(&message_buf[msg_pos], length_buf[msg_count]);
+//    }
+
+    if (length_buf[msg_count] == 0) {
+      chprintf((BaseSequentialStream*) &SD1, "%s(%d): WARNING: receiving aborted after %d messages (%d bytes)\n", __FILE__ , __LINE__, msg_count, msg_pos);
+      return msg_count;
+    }
+
+    if (strncmp(&message_buf[msg_pos], last_msg, strlen(last_msg)) == 0) {
+      return msg_count+1;
+    }
+
+    if (strncmp(&message_buf[msg_pos], filter_in, strlen(filter_in)) == 0)
+    {
+      if (filter_out == NULL || strncmp(&message_buf[msg_pos], filter_out, strlen(filter_out)) != 0)
+      {
+        msg_pos += length_buf[msg_count];
+        ++msg_count;
+      }
+    }
+
+    if (msg_count > len_buf_size) {
+      chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: maximum messages received (%d)!\n", __FILE__ , __LINE__, len_buf_size);
+      return len_buf_size;
+    }
+
+  }
+
+  return 0xFFFFu; // unreachable
+}
+
+void
+WT12CheckUtility::receiveRaw(uint8_t* buffer, uint16_t num_bytes, const bool print)
+{
+  uint16_t i;
+
+  for (i = 0; i < num_bytes; ++i)
+  {
+    buffer[i] = chSequentialStreamGet((BaseSequentialStream*)&SD3);
+  }
+
+  if (print)
+  {
+    for (i = 0; i < num_bytes; ++i)
+    {
+      chprintf((BaseSequentialStream*) &SD1, "%02X ", buffer[i]);
+    }
+    chprintf((BaseSequentialStream*) &SD1, "\n");
+  }
+
+  return;
+}
+
+void
+WT12CheckUtility::receiveRawLine(const bool print)
+{
+  uint8_t byte = 0;;
+  while (byte != '\n')
+  {
+    byte = chSequentialStreamGet((BaseSequentialStream*) &SD3);
+    if (print) {
+      chprintf((BaseSequentialStream*) &SD1, "%c", byte);
+    }
+  }
+
+  return;
+}
+
+void
+WT12CheckUtility::clearMessageBuffer(const bool print)
+{
+  uint8_t byte;
+  while (!sdGetWouldBlock(&SD3))
+  {
+    byte = sdGet(&SD3);
+    //byte = chSequentialStreamGet((BaseSequentialStream*) &SD3);
+    if (print) {
+      chprintf((BaseSequentialStream*) &SD1, "0x%02X <%c>\n", byte, byte);
+    }
+    BaseThread::sleep(MS2ST(1));
+  }
+
+  return;
+}
+
diff --git a/components/bus/i2c/HWI2CDriver.cpp b/components/bus/i2c/HWI2CDriver.cpp
new file mode 100644
index 0000000..2c99e32
--- /dev/null
+++ b/components/bus/i2c/HWI2CDriver.cpp
@@ -0,0 +1,84 @@
+#include <amiro/bus/i2c/I2CParams.hpp>
+#include <amiro/bus/i2c/HWI2CDriver.hpp>
+
+namespace amiro {
+
+
+HWI2CDriver::
+HWI2CDriver(::I2CDriver *d) :
+  driver(d) {
+
+};
+
+void
+HWI2CDriver::
+start(const ::I2CConfig *config) {
+
+  i2cStart(this->driver, config);
+
+};
+
+void
+HWI2CDriver::
+stop() {
+
+  i2cStop(this->driver);
+
+}
+
+i2cflags_t
+HWI2CDriver::
+getErrors() {
+
+  return i2cGetErrors(this->driver);
+
+}
+
+msg_t
+HWI2CDriver::
+masterTransmit(const I2CTxParams *params, systime_t timeout) {
+
+  return i2cMasterTransmitTimeout(
+                                  this->driver,
+                                  params->addr,
+                                  params->txbuf,
+                                  params->txbytes,
+                                  params->rxbuf,
+                                  params->rxbytes,
+                                  timeout);
+
+}
+
+msg_t
+HWI2CDriver::
+masterReceive(const I2CRxParams *params, systime_t timeout) {
+
+  return i2cMasterReceiveTimeout(
+                                 this->driver,
+                                 params->addr,
+                                 params->rxbuf,
+                                 params->rxbytes,
+                                 timeout);
+
+}
+
+void
+HWI2CDriver::
+acquireBus() {
+
+#if I2C_USE_MUTUAL_EXCLUSION
+  i2cAcquireBus(this->driver);
+#endif /* I2C_USE_MUTUAL_EXCLUSION */
+}
+
+void
+HWI2CDriver::
+releaseBus() {
+
+#if I2C_USE_MUTUAL_EXCLUSION
+  i2cReleaseBus(this->driver);
+#endif /* I2C_USE_MUTUAL_EXCLUSION */
+
+}
+
+} /* amiro */
diff --git a/components/bus/i2c/I2CMultiplexer.cpp b/components/bus/i2c/I2CMultiplexer.cpp
new file mode 100644
index 0000000..aab0f3f
--- /dev/null
+++ b/components/bus/i2c/I2CMultiplexer.cpp
@@ -0,0 +1,73 @@
+#include <amiro/bus/i2c/I2CMultiplexer.hpp>
+
+namespace amiro {
+
+I2CMultiplexer::
+I2CMultiplexer(I2CDriver *driver) :
+  driver(driver) {
+
+#if CH_USE_MUTEXES
+  chMtxInit(&this->mutex);
+#else
+  chSemInit(&this->semaphore, 1);
+#endif /* CH_USE_MUTEXES */
+
+};
+
+I2CMultiplexer::
+~I2CMultiplexer() {
+
+}
+
+i2cflags_t
+I2CMultiplexer::
+getErrors() {
+
+  return this->driver->getErrors();
+
+}
+
+msg_t
+I2CMultiplexer::
+masterTransmit(const I2CTxParams *params, systime_t timeout) {
+
+  return this->driver->masterTransmit(params, timeout);
+
+}
+
+msg_t
+I2CMultiplexer::
+masterReceive(const I2CRxParams *params, systime_t timeout) {
+
+  return this->driver->masterReceive(params, timeout);
+
+}
+
+void
+I2CMultiplexer::
+acquireBus() {
+
+#if CH_USE_MUTEXES
+  chMtxLock(&this->mutex);
+#elif CH_USE_SEMAPHORES
+  chSemWait(&this->semaphore);
+#endif
+
+  this->driver->acquireBus();
+}
+
+void
+I2CMultiplexer::
+releaseBus() {
+
+  this->driver->releaseBus();
+
+#if CH_USE_MUTEXES
+  chMtxUnlock();
+#elif CH_USE_SEMAPHORES
+  chSemSignal(&this->semaphore);
+#endif
+
+}
+
+} /* amiro */
diff --git a/components/bus/i2c/VI2CDriver.cpp b/components/bus/i2c/VI2CDriver.cpp
new file mode 100644
index 0000000..7f40874
--- /dev/null
+++ b/components/bus/i2c/VI2CDriver.cpp
@@ -0,0 +1,109 @@
+#include <chdebug.h>
+#include <amiro/bus/i2c/I2CMultiplexer.hpp>
+#include <amiro/bus/i2c/VI2CDriver.hpp>
+
+namespace amiro {
+
+VI2CDriver::
+VI2CDriver(I2CMultiplexer *driver, const uint8_t bus_id) :
+  driver(driver),
+  id(bus_id) {
+
+#if CH_USE_MUTEXES
+  chMtxInit(&this->mutex);
+#else
+  chSemInit(&this->semaphore, 1);
+#endif /* CH_USE_MUTEXES */
+
+};
+
+
+void
+VI2CDriver::
+start(const I2CConfig* config) {
+
+  /* silently discard config */
+  (void) config;
+  /* ignore starts */
+
+}
+
+void
+VI2CDriver::
+stop() {
+
+  /* ignore stops */
+
+}
+
+i2cflags_t
+VI2CDriver::
+getErrors() {
+
+  return this->driver->getErrors();
+
+}
+
+msg_t
+VI2CDriver::
+masterTransmit(const I2CTxParams *params, systime_t timeout) {
+
+  msg_t ret;
+
+  this->driver->acquireBus();
+
+  if (!(ret = this->driver->select(this->id))) {
+    ret = this->driver->masterTransmit(params, timeout);
+  }
+
+  this->driver->deselect();
+  this->driver->releaseBus();
+
+  return ret;
+}
+
+msg_t
+VI2CDriver::
+masterReceive(const I2CRxParams *params, systime_t timeout) {
+
+  msg_t ret;
+
+  this->driver->acquireBus();
+
+  if (!(ret = this->driver->select(this->id)))
+    ret = this->driver->masterReceive(params, timeout);
+
+  this->driver->deselect();
+  this->driver->releaseBus();
+
+  return ret;
+
+}
+
+
+void
+VI2CDriver::
+acquireBus() {
+
+#if CH_USE_MUTEXES
+  chMtxLock(&this->mutex);
+#elif CH_USE_SEMAPHORES
+  chSemWait(&this->semaphore);
+#endif
+
+}
+
+void
+VI2CDriver::
+releaseBus() {
+
+#if CH_USE_MUTEXES
+  chMtxUnlock();
+#elif CH_USE_SEMAPHORES
+  chSemSignal(&this->semaphore);
+#endif
+
+}
+
+}
+/* amiro */
diff --git a/components/bus/i2c/mux/pca9544.tpp b/components/bus/i2c/mux/pca9544.tpp
new file mode 100644
index 0000000..c121b85
--- /dev/null
+++ b/components/bus/i2c/mux/pca9544.tpp
@@ -0,0 +1,118 @@
+#include <chdebug.h>
+
+namespace amiro {
+
+template <bool ignore_deselect>
+PCA9544<ignore_deselect>::
+PCA9544(I2CDriver *driver, const uint8_t master_id) :
+	I2CMultiplexer(driver),
+	master_id(master_id),
+	selected(-1) {
+
+	chDbgCheck(master_id <= 0x07u, "PCA9544 ctor master_id");
+	this->tx_params.addr = (PCA9544::SLA | this->master_id);
+	this->rx_params.addr = (PCA9544::SLA | this->master_id);
+
+}
+
+template <bool ignore_deselect>
+msg_t
+PCA9544<ignore_deselect>::
+select(const uint8_t bus_id) {
+
+	chDbgCheck(bus_id <= 0x03u, "PCA9544 select bus_id");
+
+	uint8_t tmp = PCA9544::BUS_ON | bus_id;
+
+	if (bus_id == this->selected)
+		return RDY_OK;
+
+	this->selected = bus_id;
+
+	this->tx_params.txbuf = &tmp;
+	this->tx_params.txbytes = 1;
+	this->tx_params.rxbytes = 0;
+	return this->masterTransmit(&this->tx_params); // TODO select timeout
+
+}
+
+template <>
+inline msg_t
+PCA9544<true>::
+deselect() {
+
+	return RDY_OK;
+
+}
+
+template <>
+inline msg_t
+PCA9544<false>::
+deselect() {
+
+	uint8_t tmp = PCA9544::BUS_OFF;
+
+	this->selected = -1;
+	this->tx_params.txbuf = &tmp;
+	this->tx_params.txbytes = 1;
+	this->tx_params.rxbytes = 0;
+	return this->masterTransmit(&this->tx_params); // TODO select timeout
+
+}
+
+template <bool ignore_deselect>
+uint8_t
+PCA9544<ignore_deselect>::
+getInterruptSource() {
+
+	uint8_t tmp;
+	this->rx_params.rxbuf = &tmp;
+	this->rx_params.rxbytes = 1;
+	this->masterReceive(&this->rx_params); // TODO select timeout
+	return tmp;
+
+}
+
+template <bool ignore_deselect>
+uint8_t
+PCA9544<ignore_deselect>::
+getCheck(void) {
+
+	const int8_t rxBufferMask = 0x07; /** We are only intrested in the first three bits*/
+	int8_t txBuffer, resTx, resRx;
+	uint8_t rxBuffer[2];
+
+	// Copy the data, because some virtual driver can write into it
+	I2CTxParams tx_params = this->tx_params;
+	I2CRxParams rx_params = this->rx_params;
+
+	tx_params.txbuf = (uint8_t*) &txBuffer;
+	tx_params.txbytes = 1;
+	tx_params.rxbytes = 0;
+	rx_params.rxbuf = rxBuffer;
+	rx_params.rxbytes = 2; /** This is a bug workaround: 1 byte is impossible to read*/
+
+	for (txBuffer = rxBufferMask; txBuffer >= 0x00; txBuffer--) {
+		this->acquireBus();
+		resTx = this->masterTransmit(&tx_params);
+		resRx = this->masterReceive(&rx_params);
+		this->releaseBus();
+		// Failed to transmit
+		if (resTx != RDY_OK || resRx != RDY_OK) {
+			break;
+		}
+		// Check data written to MUX register
+		if (txBuffer != (rxBuffer[0] & rxBufferMask)) {
+			break;
+		}
+	}
+
+	// Data analysis
+	if (txBuffer != -1) {
+		return PCA9544::CHECK_FAIL;
+	} else {
+		return PCA9544::CHECK_OK;
+	}
+}
+
+} /* amiro */
diff --git a/components/bus/spi/HWSPIDriver.cpp b/components/bus/spi/HWSPIDriver.cpp
new file mode 100644
index 0000000..b33ab2a
--- /dev/null
+++ b/components/bus/spi/HWSPIDriver.cpp
@@ -0,0 +1,54 @@
+#include <hal.h>
+#include <amiro/bus/spi/HWSPIDriver.hpp>
+
+using namespace chibios_rt;
+
+namespace amiro {
+
+HWSPIDriver::
+HWSPIDriver(::SPIDriver *driver, const ::SPIConfig *config) :
+  driver(driver),
+  config(config) {
+
+}
+
+HWSPIDriver::
+~HWSPIDriver() {
+
+
+}
+
+void
+HWSPIDriver::
+exchange(uint8_t *readData, const uint8_t *writeData, size_t length) {
+  spiAcquireBus(this->driver);
+  spiStart(this->driver, this->config);
+  spiSelect(this->driver);
+  spiExchange(this->driver, length, writeData, readData);
+  spiUnselect(this->driver);
+  spiReleaseBus(this->driver);
+}
+
+void
+HWSPIDriver::
+read(uint8_t *data, size_t length) {
+  spiAcquireBus(this->driver);
+  spiStart(this->driver, this->config);
+  spiSelect(this->driver);
+  spiReceive(this->driver, length, data);
+  spiUnselect(this->driver);
+  spiReleaseBus(this->driver);
+}
+
+void
+HWSPIDriver::
+write(const uint8_t *data, size_t length) {
+  spiAcquireBus(this->driver);
+  spiStart(this->driver, this->config);
+  spiSelect(this->driver);
+  spiSend(this->driver, length, data);
+  spiUnselect(this->driver);
+  spiReleaseBus(this->driver);
+}
+
+} /* amiro */
diff --git a/components/eeprom/at24.cpp b/components/eeprom/at24.cpp
new file mode 100644
index 0000000..0db4e9a
--- /dev/null
+++ b/components/eeprom/at24.cpp
@@ -0,0 +1,272 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <type_traits>
+
+#include <amiro/bus/i2c/I2CDriver.hpp>
+#include <amiro/bus/i2c/I2CParams.hpp>
+
+#include <amiro/eeprom/at24.hpp>
+
+#define AT24_GET_PAGE_OFFSET(_ADDRESS_, _PAGE_SIZE_) (_ADDRESS_ & (_PAGE_SIZE_ - 1u))
+
+namespace amiro {
+
+const struct BaseFileStreamVMT eeprom_at24_base_file_stream_methods = {
+
+    /* .write       */ AT24::write,
+    /* .read        */ AT24::read,
+    /* .put         */ 0,
+    /* .get         */ 0,
+    /* .close       */ AT24::close,
+    /* .geterror    */ EEPROM::geterror,
+    /* .getsize     */ EEPROM::getsize,
+    /* .getposition */ EEPROM::getposition,
+    /* .lseek       */ EEPROM::lseek,
+
+};
+
+AT24::
+AT24(size_t size, uint8_t page_size, uint16_t max_t_wr, I2CDriver* i2c_driver) :
+  EEPROM(&eeprom_at24_base_file_stream_methods, size, page_size, max_t_wr, AT24_SLA, i2c_driver) {
+
+}
+
+AT24::
+~AT24() {
+
+}
+
+msg_t
+AT24::
+poll_ack(void* instance) {
+
+  EEPROM* bfs = (EEPROM*) instance;
+
+  register I2CDriver* bfs_i2c_driver = bfs->i2c_driver;
+  register msg_t ret_val = RDY_OK;
+  register i2cflags_t i2c_err_flags;
+
+  uint8_t dummy_buf[2];
+
+  I2CRxParams i2c_rxparams;
+  i2c_rxparams.addr = bfs->i2c_txparams.addr;
+  i2c_rxparams.rxbuf = dummy_buf;
+  i2c_rxparams.rxbytes = sizeof(dummy_buf);
+
+  for (register uint16_t wr_time = 0x0000u; wr_time < bfs->max_t_wr; wr_time++) {
+
+    bfs_i2c_driver->acquireBus();
+
+    ret_val = bfs_i2c_driver->masterReceive(&i2c_rxparams);
+
+    i2c_err_flags = bfs->i2c_driver->getErrors();
+
+    bfs_i2c_driver->releaseBus();
+
+    // Wait cycle over
+    if (ret_val == RDY_OK)
+      break;
+
+    // Check for errors, ignoring only I2CD_ACK_FAILURE
+    if (i2c_err_flags & ~I2CD_ACK_FAILURE) {
+      break;
+    }
+
+    chThdSleepMicroseconds(10);
+
+  }
+
+  // This can only be the case when
+  // * timeout reached
+  // * i2cError other than I2CD_ACK_FAILURE within time limit
+  if (ret_val != RDY_OK)
+    bfs->error = i2c_err_flags;
+
+  return ret_val;
+
+}
+
+
+size_t
+AT24::
+write(void* instance, const uint8_t* bp, size_t n) {
+
+  EEPROM* bfs = (EEPROM*) instance;
+
+  i2cflags_t i2c_err_flags;
+  uint8_t i;
+  uint8_t j;
+  uint8_t scratchpad[2 + AT24_MAX_PAGE_SIZE];
+  msg_t ret_val = RDY_OK;
+  fileoffset_t cur_pos = bfs->position;
+
+  register size_t tx_bytes;
+  register const uint8_t* ptr = bp;
+  register size_t num_bytes;
+  register uint8_t page_size = bfs->page_size;
+  register I2CTxParams* bfs_i2c_txparams = &bfs->i2c_txparams;
+
+  // If no bytes are to be written, shortcut stop
+  if (!n)
+    return 0;
+
+  // Clear error
+  bfs->error = EEPROM_ERROR_NONE;
+
+#if HAL_USE_I2C
+
+  // number of bytes remaining in page starting at cur_pos
+  // b/c of address rollover withing current page
+  tx_bytes = AT24_GET_PAGE_OFFSET(cur_pos, page_size);
+  // adjust for AT24_MAX_PAGE_SIZE < page_size
+  tx_bytes = AT24_MAX_PAGE_SIZE - AT24_GET_PAGE_OFFSET(tx_bytes, AT24_MAX_PAGE_SIZE);
+
+  for (num_bytes = n; num_bytes > 0;) {
+
+    // write address
+    i = 0;
+    // Support for 16bit-addressable devices
+    if (bfs->size > 0x0080u)
+      scratchpad[i++] = (cur_pos >> 8) & 0xFFu;
+
+    scratchpad[i++] = cur_pos & 0xFFu;
+
+    // adjust number of bytes to transfer if end of buffer
+    if (num_bytes < tx_bytes)
+      tx_bytes = num_bytes;
+
+    // copy data
+    for (j = 0; j < tx_bytes; j++) {
+      scratchpad[i++] = *ptr++;
+    }
+
+    // acknowledge polling
+
+    // acknowledge polling and
+    // address device
+    for (register uint16_t wr_time = 0x0000u; wr_time < bfs->max_t_wr; wr_time++) {
+
+      bfs->i2c_driver->acquireBus();
+
+      bfs_i2c_txparams->txbuf = scratchpad;
+      bfs_i2c_txparams->txbytes = i;
+      bfs_i2c_txparams->rxbytes = 0;
+
+      ret_val = bfs->i2c_driver->masterTransmit(bfs_i2c_txparams);
+
+      i2c_err_flags = bfs->i2c_driver->getErrors();
+
+      bfs->i2c_driver->releaseBus();
+
+      // Write cycle over
+      if (ret_val == RDY_OK)
+        break;
+
+      // Check for errors, ignoring only I2CD_ACK_FAILURE
+      if (i2c_err_flags & ~I2CD_ACK_FAILURE) {
+        break;
+      }
+
+      chThdSleepMicroseconds(10);
+
+    }
+
+    // This can only be the case when
+    // * timeout reached
+    // * i2cError other than I2CD_ACK_FAILURE within time limit
+    if (ret_val != RDY_OK)
+      break;
+
+    cur_pos += tx_bytes;
+    num_bytes -= tx_bytes;
+    tx_bytes = AT24_MAX_PAGE_SIZE;
+
+  }
+
+  if (ret_val != RDY_OK)
+    bfs->error = i2c_err_flags;
+
+#endif
+
+  bfs->position = cur_pos;
+
+  // number of bytes to transfer - number of bytes not transferred
+  return n - num_bytes;
+
+}
+
+size_t
+AT24::
+read(void* instance, uint8_t* bp, size_t n) {
+
+  EEPROM* bfs = (EEPROM*) instance;
+
+  uint8_t i;
+  uint8_t scratchpad[2];
+  msg_t ret_val;
+  register I2CTxParams* bfs_i2c_txparams = &bfs->i2c_txparams;
+
+  // If no bytes are to be read, shortcut stop
+  if (!n)
+    return 0;
+
+  // Clear error
+  bfs->error = EEPROM_ERROR_NONE;
+
+#if HAL_USE_I2C
+
+  // Fill address buffer
+  i = 0;
+  // Support for 16bit-addressable devices
+  if (bfs->size > 0x0080u)
+    scratchpad[i++] = (bfs->position >> 8) & 0xFFu;
+
+  scratchpad[i++] = bfs->position & 0xFFu;
+
+  // if device does not answer within timeout, don't read anything
+  if (poll_ack(bfs) != RDY_OK)
+    return 0;
+
+  bfs->i2c_driver->acquireBus();
+
+  bfs_i2c_txparams->txbuf = scratchpad;
+  bfs_i2c_txparams->txbytes = i;
+  bfs_i2c_txparams->rxbuf = bp;
+  bfs_i2c_txparams->rxbytes = n;
+
+  // address device
+  // and read data
+  ret_val = bfs->i2c_driver->masterTransmit(bfs_i2c_txparams);
+
+  if (ret_val != RDY_OK)
+    bfs->error = bfs->i2c_driver->getErrors();
+
+  bfs->i2c_driver->releaseBus();
+
+  // We cannot tell where I²C transfer went wrong
+  // therefore report 0 bytes read
+  if (ret_val != RDY_OK)
+    return 0;
+
+#endif
+
+  return n;
+
+}
+
+/**
+ * Close EEPROM device.
+ * \note EEPROMs do not support close semantics.
+ * \return FILE_OK Closing an EEPROM device will never fail.
+ */
+uint32_t
+AT24::
+close(void* instance) {
+
+  (void) instance;
+  return FILE_OK;
+
+}
+
+} /* amiro */
diff --git a/components/eeprom/eeprom.cpp b/components/eeprom/eeprom.cpp
new file mode 100644
index 0000000..8a5c340
--- /dev/null
+++ b/components/eeprom/eeprom.cpp
@@ -0,0 +1,114 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <amiro/eeprom/eeprom.hpp>
+
+namespace amiro {
+
+#if HAL_USE_I2C
+EEPROM::
+EEPROM(const struct BaseFileStreamVMT* vmt, size_t size, uint8_t page_size, uint16_t max_t_wr, uint8_t sla, I2CDriver* i2c_driver) :
+  error(EEPROM_ERROR_NONE),
+  position(0x00u),
+  size(size),
+  page_size(page_size),
+  max_t_wr(max_t_wr),
+  i2c_driver(i2c_driver) {
+
+#if CH_USE_MUTEXES
+  chMtxInit(&this->mutex);
+#elif CH_USE_SEMAPHORES
+  chSemInit(&this->semaphore, 1);
+#endif
+
+  this->bfs.vmt = vmt;
+  this->i2c_txparams.addr = sla;
+
+}
+#endif /* HAL_USE_I2C */
+
+#if HAL_USE_SPI
+EEPROM::
+EEPROM(const struct BaseFileStreamVMT* vmt, size_t size, uint8_t page_size, uint16_t max_t_wr, HWSPIDriver* spi_driver) :
+  error(EEPROM_ERROR_NONE),
+  position(0x00u),
+  size(size),
+  page_size(page_size),
+  max_t_wr(max_t_wr),
+  spi_driver(spi_driver) {
+
+#if CH_USE_MUTEXES
+  chMtxInit(&this->mutex);
+#elif CH_USE_SEMAPHORES
+  chSemInit(&this->semaphore, 1);
+#endif
+
+  this->bfs.vmt = vmt;
+
+}
+#endif /* HAL_USE_SPI */
+
+EEPROM::
+~EEPROM() {
+
+}
+
+void
+EEPROM::
+acquire() {
+
+#if CH_USE_MUTEXES
+  chMtxLock(&this->mutex);
+#elif CH_USE_SEMAPHORES
+  chSemWait(&this->semaphore);
+#endif
+
+}
+
+void
+EEPROM::
+release() {
+
+#if CH_USE_MUTEXES
+  chMtxUnlock();
+#elif CH_USE_SEMAPHORES
+  chSemSignal(&this->semaphore);
+#endif
+
+}
+
+int
+EEPROM::
+geterror(void* instance) {
+
+  return (int) ((EEPROM*) (instance))->error;
+
+}
+
+fileoffset_t
+EEPROM::
+getsize(void *instance) {
+
+  return ((EEPROM*) (instance))->size;
+
+}
+
+fileoffset_t
+EEPROM::
+getposition(void* instance) {
+
+  return ((EEPROM*) (instance))->position;
+
+
+}
+
+fileoffset_t
+EEPROM::
+lseek(void *instance, fileoffset_t offset) {
+
+  ((EEPROM*) (instance))->position = offset;
+  return FILE_OK;
+
+}
+
+};
diff --git a/components/gyro/l3g4200d.cpp b/components/gyro/l3g4200d.cpp
new file mode 100644
index 0000000..31d4f5f
--- /dev/null
+++ b/components/gyro/l3g4200d.cpp
@@ -0,0 +1,121 @@
+#include <string.h>
+
+#include <amiro/util/util.h>
+#include <amiro/bus/spi/HWSPIDriver.hpp>
+#include <amiro/gyro/l3g4200d.hpp>
+
+namespace amiro {
+
+L3G4200D::L3G4200D(HWSPIDriver *driver)
+    : driver(driver) {
+
+}
+
+L3G4200D::~L3G4200D() {
+
+}
+
+chibios_rt::EvtSource*
+L3G4200D::getEventSource() {
+  return &this->eventSource;
+}
+
+msg_t L3G4200D::main() {
+
+  this->setName("L3g4200d");
+
+  while (!this->shouldTerminate()) {
+
+    updateSensorData();
+
+    this->eventSource.broadcastFlags(1);
+
+    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));
+
+  }
+  return RDY_OK;
+}
+
+int16_t
+L3G4200D::
+getAngularRate(const uint8_t axis) {
+
+  return this->angularRate[axis];
+
+}
+
+void L3G4200D::updateSensorData() {
+
+  const size_t buffer_size = offsetof(L3G4200D::registers, OUT_Z)
+                             - offsetof(L3G4200D::registers, STATUS_REG)
+                             + MEMBER_SIZE(L3G4200D::registers, OUT_Z)
+                             + 1; /* addressing */
+  uint8_t buffer[buffer_size];
+  uint8_t sreg;
+
+  /*Address of first data register*/
+  memset(buffer, 0xFF, sizeof(buffer));
+  buffer[0] = offsetof(L3G4200D::registers, STATUS_REG) | L3G4200D::SPI_READ | L3G4200D::SPI_MULT;
+
+  this->driver->exchange(buffer, buffer, buffer_size);
+
+  // assemble data
+  sreg = buffer[1];
+
+  if (sreg & L3G4200D::XDA)
+    this->angularRate[L3G4200D::AXIS_X] = (buffer[3] << 8) + buffer[2];
+
+  if (sreg & L3G4200D::YDA)
+    this->angularRate[L3G4200D::AXIS_Y] = (buffer[5] << 8) + buffer[4];
+
+  if (sreg & L3G4200D::ZDA)
+    this->angularRate[L3G4200D::AXIS_Z] = (buffer[7] << 8) + buffer[6];
+
+}
+
+msg_t L3G4200D::configure(const L3G4200DConfig *config) {
+
+  const size_t ctrl_reg_size = offsetof(L3G4200D::registers, CTRL_REG5)
+                               - offsetof(L3G4200D::registers, CTRL_REG1)
+                               + MEMBER_SIZE(L3G4200D::registers, CTRL_REG5)
+                               + 1; /* addressing */
+
+  const size_t buffer_size = ctrl_reg_size;
+
+  uint8_t buffer[buffer_size];
+
+  // write control config
+  // this might be three-wire so we need to send ones
+  memset(buffer, 0xFFu, buffer_size);
+  buffer[0] = offsetof(L3G4200D::registers, CTRL_REG1) | L3G4200D::SPI_MULT | L3G4200D::SPI_WRITE;
+  buffer[1] = config->ctrl1;
+  buffer[2] = config->ctrl2;
+  buffer[3] = config->ctrl3;
+  buffer[4] = config->ctrl4;
+  buffer[5] = config->ctrl5;
+  this->driver->write(buffer, 6);
+
+  return RDY_OK;
+
+}
+
+uint8_t L3G4200D::getCheck() {
+
+  const size_t buffer_size = 1 /* addressing */
+  + 1; /* who am i */
+  uint8_t buffer[buffer_size];
+
+  // Exchange the data with the L3G4200D gyroscope
+  // Specify the adress and the mode
+  buffer[0] = offsetof(L3G4200D::registers, WHO_AM_I) | L3G4200D::SPI_READ;
+  this->driver->exchange(buffer, buffer, buffer_size);
+  // Check
+  if (buffer[1] == L3G4200D::L3G4200D_ID) {
+    return L3G4200D::CHECK_OK;
+  } else {
+    return L3G4200D::CHECK_FAIL;
+  }
+
+}
+
+} /* amiro */
diff --git a/components/input/mpr121.cpp b/components/input/mpr121.cpp
new file mode 100644
index 0000000..cf5c8b7
--- /dev/null
+++ b/components/input/mpr121.cpp
@@ -0,0 +1,212 @@
+#include <amiro/bus/i2c/I2CDriver.hpp>
+#include <amiro/input/mpr121.hpp>
+
+namespace amiro {
+
+MPR121::
+MPR121(I2CDriver *driver, const uint8_t master_id) :
+  BaseStaticThread<256>(),
+  driver(driver),
+  master_id(master_id),
+  button_state(0x0000u) {
+
+  chDbgCheck(master_id <= 0x03u, "MPR121 ctor master_id");
+  this->tx_params.addr = MPR121::SLA | this->master_id;
+
+}
+
+MPR121::
+~MPR121() {
+
+}
+
+msg_t
+MPR121::
+updateButtonData() {
+
+  msg_t res;
+  const uint8_t offset_touch = offsetof(MPR121::registers, touch_status);
+  const uint8_t offset_electrode = offsetof(MPR121::registers, ele_filt_data);
+  const uint8_t offset_baseline = offsetof(MPR121::registers, ele_baseline);
+  uint8_t buffer[offsetof(MPR121::registers, mhd_rising) - offset_touch];
+  uint8_t *tmp = buffer;
+  uint8_t reg = offset_touch;
+  uint8_t i;
+  this->tx_params.txbuf = &reg;
+  this->tx_params.txbytes = 1;
+  this->tx_params.rxbuf = buffer;
+  this->tx_params.rxbytes = sizeof(buffer);
+
+  res = this->driver->masterTransmit(&this->tx_params);
+
+  if (!res) {
+    tmp = buffer + offset_touch;
+    this->button_state = (*(tmp + 1) << 8) | *tmp;
+    tmp = buffer + offset_electrode;
+    for (i = 0x00; i < 13; i++) {
+      this->electrode_data[i] = (*(tmp + 1) << 8) | *tmp;
+      tmp += 2;
+    }
+    tmp = buffer + offset_baseline;
+    for (i = 0x00; i < 13; i++)
+      this->baseline_data[i] = *tmp++;
+
+  }
+
+  return res;
+
+}
+
+msg_t
+MPR121::
+softReset() {
+
+  msg_t res;
+  uint8_t buffer[2];
+
+  buffer[0] = offsetof(MPR121::registers, soft_reset);
+  buffer[1] = SOFT_RST_MAGIC;
+
+  this->tx_params.txbuf = buffer;
+  this->tx_params.txbytes = 2;
+  this->tx_params.rxbytes = 0;
+
+  res = this->driver->masterTransmit(&this->tx_params);
+
+  if (res)
+    return res;
+
+  buffer[0] = offsetof(MPR121::registers, ele_config);
+  buffer[1] = 0x00u; /* ele_config: make sure we are in stop mode */
+
+  return this->driver->masterTransmit(&this->tx_params);
+
+}
+
+msg_t
+MPR121::
+writeConfig(const MPR121Config *cfg) {
+
+  msg_t res;
+  uint8_t buffer[6];
+
+  buffer[0] = offsetof(MPR121::registers, auto_cfg_ctrl);
+  buffer[1] = (cfg->auto_config & 0x00FFu);
+  buffer[2] = (cfg->auto_config & 0xFF00u) >> 8;
+  buffer[3] = cfg->up_side_limit;
+  buffer[4] = cfg->low_side_limit;
+  buffer[5] = cfg->target_level;
+
+  this->tx_params.txbuf = buffer;
+  this->tx_params.txbytes = 6;
+  this->tx_params.rxbytes = 0;
+
+  res = this->driver->masterTransmit(&this->tx_params);
+
+  if (res)
+    return res;
+
+  buffer[0] = offsetof(MPR121::registers, cdc_config);
+  buffer[1] = (cfg->global_config & 0x00FFu);
+  buffer[2] = (cfg->global_config & 0xFF00u) >> 8;
+  buffer[3] = cfg->ele_config;
+
+  this->tx_params.txbytes = 4;
+
+  return this->driver->masterTransmit(&this->tx_params);
+
+}
+
+uint16_t
+MPR121::
+getButtonStatus() {
+
+  return this->button_state;
+
+}
+
+uint8_t
+MPR121::
+getButtonStatus(uint8_t ix) {
+
+  return this->button_state & (1u << ix);
+
+}
+
+uint8_t
+MPR121::
+getBaselineData(uint8_t ix) {
+
+  return this->baseline_data[ix];
+
+}
+
+uint16_t
+MPR121::
+getElectrodeData(uint8_t ix) {
+
+  return this->electrode_data[ix];
+
+}
+
+/**
+ * Main Sensor Thread.
+ * Will not check whether sensor was actually configured prior to running.
+ * @note   You should configure the sensor while stopped, then start.
+ * @return RDY_OK on exit.
+ */
+msg_t
+MPR121::
+main(void) {
+
+  I2CDriver *drv = this->driver;
+
+  this->setName("MPR121");
+
+  while (!this->shouldTerminate()) {
+
+    drv->acquireBus();
+    this->updateButtonData();
+    drv->releaseBus();
+
+    this->eventSource.broadcastFlags(0);
+
+    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(100));
+  }
+
+  return RDY_OK;
+}
+
+/**
+ * Configure the MPR121 touch sensor.
+ * Will perform a soft reset and then apply the configuration \p cfg.
+ * @note      Call only when sensor is stopped.
+ * @param cfg The new configuration.
+ * @return    RDY_OK if everything went well. Appropriate error otherwise.
+ */
+msg_t
+MPR121::
+configure(const MPR121Config *cfg) {
+
+  I2CDriver *drv = this->driver;
+  msg_t res;
+
+  drv->acquireBus();
+
+  // do a soft reset, in case config changes config mode
+  // (auto config vs. configure all sensors individually)
+  res = this->softReset();
+
+  if (res == RDY_OK) {
+
+    res = this->writeConfig(cfg);
+
+  }
+
+  drv->releaseBus();
+
+  return res;
+}
+
+
+} /* amiro */
diff --git a/components/leds/tlc5947.cpp b/components/leds/tlc5947.cpp
new file mode 100644
index 0000000..89ca720
--- /dev/null
+++ b/components/leds/tlc5947.cpp
@@ -0,0 +1,97 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <amiro/bus/spi/HWSPIDriver.hpp>
+#include <amiro/leds/tlc5947.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+TLC5947::TLC5947(HWSPIDriver *spi, ioportid_t blankPort, int blankPad)
+    : BaseStaticThread<192>(),
+      spi(spi),
+      blankPort(blankPort),
+      blankPad(blankPad),
+      brightness(0x00u) {
+  for (int i = 0; i < 8; i++)
+    this->colors[i] = Color::BLACK;
+}
+
+TLC5947::
+~TLC5947() {
+
+}
+
+void TLC5947::disable() {
+  palWritePad(this->blankPort, this->blankPad, 1);
+}
+
+void TLC5947::enable() {
+  palWritePad(this->blankPort, this->blankPad, 0);
+}
+
+void TLC5947::setBrightness(int brightness) {
+  if (brightness < 0)
+    this->brightness = 0;
+  else if (brightness > 100)
+    this->brightness = 100;
+  else
+    this->brightness = brightness;
+}
+
+void TLC5947::setColor(int index, Color color) {
+  this->colors[index] = color;
+}
+
+void TLC5947::update() {
+  this->signalEvents(static_cast<eventmask_t>(1));
+}
+
+msg_t TLC5947::main(void) {
+  uint8_t buffer[36];
+
+  this->setName("Tlc5947");
+
+  while (!this->shouldTerminate()) {
+    int brightness = this->brightness;
+
+    for (int i = 0, j = 0; i < 8; i += 2) {
+      Color color1 = this->colors[i];
+      Color color2 = this->colors[i + 1];
+
+      int values[6];
+      values[0] = caluclateBlueGrayscale(color1, brightness);
+      values[1] = caluclateGreenGrayscale(color1, brightness);
+      values[2] = caluclateRedGrayscale(color1, brightness);
+      values[3] = caluclateBlueGrayscale(color2, brightness);
+      values[4] = caluclateGreenGrayscale(color2, brightness);
+      values[5] = caluclateRedGrayscale(color2, brightness);
+
+      for (int k = 0; k < 6; k += 2) {
+        int value1 = values[k];
+        int value2 = values[k + 1];
+
+        buffer[j++] = static_cast<uint8_t>((value1 >> 4) & 0xff);
+        buffer[j++] = static_cast<uint8_t>((value1 << 4) | ((value2 >> 8) & 0xff));
+        buffer[j++] = static_cast<uint8_t>(value2 & 0xff);
+      }
+    }
+    this->spi->write(buffer, 36);
+
+    this->waitAnyEvent(ALL_EVENTS);
+  }
+
+  return true;
+}
+
+int TLC5947::caluclateRedGrayscale(Color color, int brightness) const {
+  return color.getRed() * brightness * 16 / 100;
+}
+
+int TLC5947::caluclateGreenGrayscale(Color color, int brightness) const {
+  return color.getGreen() * brightness * 16 / 100;
+}
+
+int TLC5947::caluclateBlueGrayscale(Color color, int brightness) const {
+  return color.getBlue() * brightness * (16 * 3) / (100 * 4);
+}
diff --git a/components/magneto/hmc5883l.cpp b/components/magneto/hmc5883l.cpp
new file mode 100644
index 0000000..6a32911
--- /dev/null
+++ b/components/magneto/hmc5883l.cpp
@@ -0,0 +1,193 @@
+#include <ch.hpp>
+#include <hal.h>
+#include <chdebug.h>
+
+#include <amiro/bus/i2c/I2CParams.hpp>
+#include <amiro/bus/i2c/I2CDriver.hpp>
+#include <amiro/magneto/hmc5883l.hpp>
+
+using namespace chibios_rt;
+
+namespace amiro {
+
+HMC5883L::~HMC5883L() {
+
+}
+
+HMC5883L::HMC5883L(I2CDriver *driver, const HMC5883LConfig *config)
+    : BaseStaticThread<256>(),
+      driver(driver),
+      config(config) {
+
+  this->txParams.addr = HMC5883L::SLA;
+}
+
+EvtSource*
+HMC5883L::
+getEventSource() {
+
+  return &this->eventSource;
+}
+
+msg_t
+HMC5883L::
+main(void) {
+
+  msg_t res;
+
+  this->setName("Hmc5883l");
+
+  /* exit if writing configuration fails */
+  res = this->writeConf();
+
+  if (res) {
+    return RDY_RESET;
+  }
+
+  while (!this->shouldTerminate()) {
+
+    this->updateSensorData();
+
+    this->eventSource.broadcastFlags(0);
+
+    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));
+  }
+
+  return RDY_OK;
+}
+
+int16_t
+HMC5883L::
+getMagnetization(const uint8_t axis) {
+  int16_t returnBuffer;
+  chSysLock();
+  returnBuffer = this->data[axis];
+  chSysUnlock();
+  return returnBuffer;
+
+}
+
+int32_t
+HMC5883L::
+getMagnetizationGauss(const uint8_t axis) {
+
+  int16_t magLsbBuffer = this->getMagnetization(axis);
+  int32_t returnBuffer = 0;
+
+  // The gains are taken from table 9 of the manual
+  switch (this->config->ctrlB & GN_7_GA) {
+    case HMC5883L::GN_0_GA:
+      returnBuffer = int32_t(0.73 * 1000) * int32_t(magLsbBuffer);
+      break;
+    case HMC5883L::GN_1_GA:
+      returnBuffer = int32_t(0.92 * 1000) * int32_t(magLsbBuffer);
+      break;
+    case HMC5883L::GN_2_GA:
+      returnBuffer = int32_t(1.22 * 1000) * int32_t(magLsbBuffer);
+      break;
+    case HMC5883L::GN_3_GA:
+      returnBuffer = int32_t(1.52 * 1000) * int32_t(magLsbBuffer);
+      break;
+    case HMC5883L::GN_4_GA:
+      returnBuffer = int32_t(2.27 * 1000) * int32_t(magLsbBuffer);
+      break;
+    case HMC5883L::GN_5_GA:
+      returnBuffer = int32_t(2.56 * 1000) * int32_t(magLsbBuffer);
+      break;
+    case HMC5883L::GN_6_GA:
+      returnBuffer = int32_t(3.03 * 1000) * int32_t(magLsbBuffer);
+      break;
+    case HMC5883L::GN_7_GA:
+      returnBuffer = int32_t(4.35 * 1000) * int32_t(magLsbBuffer);
+      break;
+  }
+
+  return returnBuffer;
+
+}
+
+void
+HMC5883L::
+updateSensorData() {
+
+  const uint8_t sizeOfRxData =  offsetof(HMC5883L::registers, yLsb)
+                              - offsetof(HMC5883L::registers, xMsb)
+                              + 1;
+
+  uint8_t rxBuffer[sizeOfRxData];
+
+  /*Address of data register*/
+  uint8_t txBuffer = offsetof(HMC5883L::registers, xMsb); /* Address of XMSB */
+
+  // Define the RXTX structure
+  this->txParams.txbuf = &txBuffer;
+  this->txParams.txbytes = sizeof(txBuffer);
+  this->txParams.rxbuf = rxBuffer;
+  this->txParams.rxbytes = sizeof(rxBuffer);
+
+  this->driver->acquireBus();
+  this->driver->masterTransmit(&this->txParams);
+  this->driver->releaseBus();
+
+  // Copy the bytes in the right order
+  chSysLock(); // TODO: check if this is really necessary b/c it inserts a DMB!!
+  data[HMC5883L::AXIS_X] = (rxBuffer[0] << 8) + rxBuffer[1];
+  data[HMC5883L::AXIS_Z] = (rxBuffer[2] << 8) + rxBuffer[3];
+  data[HMC5883L::AXIS_Y] = (rxBuffer[4] << 8) + rxBuffer[5];
+  chSysUnlock();
+}
+
+uint8_t HMC5883L::getCheck() {
+
+  I2CTxParams txStructure;
+  msg_t res;
+  const uint8_t txBuffer = offsetof(HMC5883L::registers, idA);
+  const uint8_t sizeOfRxData = offsetof(HMC5883L::registers, idC) - offsetof(HMC5883L::registers, idA) + 1;
+  uint8_t rxBuffer[sizeOfRxData];
+
+  txStructure.addr = SLA;
+  txStructure.txbuf = &txBuffer;
+  txStructure.txbytes = sizeof(txBuffer);
+  txStructure.rxbuf = rxBuffer;
+  txStructure.rxbytes = sizeof(rxBuffer);
+
+  // get the identifier
+  this->driver->acquireBus();
+  res = this->driver->masterTransmit(&txStructure);
+  this->driver->releaseBus();
+  // Check
+  if (rxBuffer[0] == ID_IRA && rxBuffer[1] == ID_IRB && rxBuffer[2] == ID_IRC && res == RDY_OK) {
+    return HMC5883L::CHECK_OK;
+  } else {
+    return HMC5883L::CHECK_FAIL;
+  }
+
+}
+
+
+msg_t
+HMC5883L::
+writeConf() {
+
+  uint8_t txBuffer[4];
+  msg_t res;
+
+  // write control config
+  txBuffer[0] = offsetof(HMC5883L::registers, ctrlA);  /* Address of ctrlA */
+  txBuffer[1] = this->config->ctrlA;
+  txBuffer[2] = this->config->ctrlB;
+  txBuffer[3] = this->config->mode;
+
+  this->txParams.txbuf = txBuffer;
+  this->txParams.txbytes = sizeof(txBuffer);
+  this->txParams.rxbytes = 0;
+
+  this->driver->acquireBus();
+  res = this->driver->masterTransmit(&this->txParams);
+  this->driver->releaseBus();
+
+  return res;
+
+}
+
+} /* amiro */
diff --git a/components/power/adconverter.cpp b/components/power/adconverter.cpp
new file mode 100644
index 0000000..2a02c0c
--- /dev/null
+++ b/components/power/adconverter.cpp
@@ -0,0 +1,159 @@
+#include <amiro/power/adconverter.hpp>
+
+#include <global.hpp>
+#include <amiro/Constants.h>
+#include <amiro/power/ina219.hpp>
+#include <amiro/util/util.h>
+
+#include <chprintf.h>
+
+using namespace amiro;
+
+extern Global global;
+
+void adcVsysErrorCallback(ADCDriver *adcp, adcerror_t err)
+{
+  if (err == ADC_ERR_WATCHDOG) {
+    chSysLockFromIsr();
+    if (adcp->thread != NULL) {
+      chEvtSignalI(adcp->thread, EVENT_MASK(ADConverter::EVT_WATCHDOG));
+      adcp->thread = NULL;
+    }
+    chSysUnlockFromIsr();
+  }
+
+  return;
+}
+
+void adcTimerCallback(Thread *tp)
+{
+  if (tp != NULL) {
+    chEvtSignal(tp, EVENT_MASK(ADConverter::EVT_TIMER));
+  }
+  return;
+}
+
+uint16_t
+ADConverter::uV2Adc(const uint32_t uV)
+{
+  // NOTE: The factor 5.33 is an approximation of the voltage divider.
+  //       Its value should be 5.0, but due to too large resistors the factor is shifted.
+  // TODO: 5.33 should be replaced by a non linear function, which needs to be optained from the system.
+
+  // get the current VDD voltage
+  global.ina219[constants::PowerManagement::INA_VDD].update();
+  INA219::BusVoltage vdd = global.ina219[constants::PowerManagement::INA_VDD].getVoltage();
+  return float(uV)/1000000 / 5.33f / (float(vdd.voltage_uV)/1000000) * 4096;
+}
+
+uint32_t ADConverter::adc2uV(const uint16_t adc)
+{
+  // NOTE: The factor 5.33 is an approximation of the voltage divider.
+  //       Its value should be 5.0, but due to too large resistors the factor is shifted.
+  // TODO: 5.33 should be replaced by a non linear function, which needs to be optained from the system.
+
+  // get the current VDD voltage
+  global.ina219[constants::PowerManagement::INA_VDD].update();
+  INA219::BusVoltage vdd = global.ina219[constants::PowerManagement::INA_VDD].getVoltage();
+  return float(adc)/4096 * (float(vdd.voltage_uV)/1000000) * 5.33f * 1000000;
+}
+
+ADConverter::ADConverter(ADCDriver& adcdrv, ADCConversionGroup& adccgrp, const uint32_t threshold_uV) :
+  driver(adcdrv), conversion_group(adccgrp), watchdog_threshold_uV(threshold_uV)
+{
+}
+
+
+ADConverter::~ADConverter()
+{
+}
+
+msg_t
+ADConverter::main()
+{
+  // initialize a timer event for later use
+  evtInit(&this->evt_timer, MS2ST(1));
+  this->evt_source = reinterpret_cast<EvtSource*>(&this->evt_timer.et_es);
+  this->evt_source->registerOne(&this->evt_listener, EVT_TIMER);
+
+  // initialize the watchdog
+  this->enableWatchdog(true);
+  this->conversion_group.htr = ADC_HTR(uV2Adc(this->watchdog_threshold_uV));
+  this->conversion_group.ltr = 0;
+  this->driver.thread = this->thread_ref;
+
+  adcStartConversion(&this->driver, &this->conversion_group, global.adc1_buffer, ARRAY_SIZE(global.adc1_buffer));
+
+  while (!this->shouldTerminate())
+  {
+    eventmask_t event = this->waitOneEventTimeout(ALL_EVENTS, MS2ST(1000));
+
+    switch (event)
+    {
+      // watchdog event (fired by ADC interrupt)
+      case EVENT_MASK(EVT_WATCHDOG):
+      {
+        // disable the chargers
+        global.bq24103a[constants::PowerManagement::BAT_P7]->enable(false);
+        global.bq24103a[constants::PowerManagement::BAT_P8]->enable(false);
+
+        // to avoid multiple interrupts because of noise, wait 1ms before setting the chargers and ADC
+        this->enableWatchdog(false);
+        evtStart(&this->evt_timer);
+        break;
+      }
+
+      // timer event
+      case EVENT_MASK(EVT_TIMER):
+      {
+        evtStop(&this->evt_timer);
+
+        // read the current voltage
+        this->conversion_group.circular = FALSE;
+        adcConvert(&this->driver, &this->conversion_group, global.adc1_buffer, ARRAY_SIZE(global.adc1_buffer));
+        this->conversion_group.circular = TRUE;
+        this->enableWatchdog(true); // Watchdog was disabled in ISR
+
+        if (global.adc1_buffer[0] > uV2Adc(this->watchdog_threshold_uV))
+        {
+          chprintf((BaseSequentialStream*)&SD1, "%f > 9V detected: now charging\n", ((float)adc2uV(global.adc1_buffer[0]))/1000000.0f);
+          global.robot.getPowerStatus().charging_flags.content.vsys_higher_than_9V = true;
+          global.bq24103a[constants::PowerManagement::BAT_P7]->enable(true);
+          global.bq24103a[constants::PowerManagement::BAT_P8]->enable(true);
+          this->conversion_group.htr = ADC_HTR_HT;
+          this->conversion_group.ltr = ADC_LTR(uV2Adc(this->watchdog_threshold_uV));
+        } else {
+          chprintf((BaseSequentialStream*)&SD1, "%f < 9V detected: not charging\n", ((float)adc2uV(global.adc1_buffer[0]))/1000000.0f);
+          global.robot.getPowerStatus().charging_flags.content.vsys_higher_than_9V = false;
+          this->conversion_group.htr = ADC_HTR(uV2Adc(this->watchdog_threshold_uV));
+          this->conversion_group.ltr = 0;
+        }
+        this->driver.thread = this->thread_ref;
+        adcStartConversion(&this->driver, &this->conversion_group, global.adc1_buffer, ARRAY_SIZE(global.adc1_buffer));
+        break;
+      }
+
+      // timeout
+      case 0:
+
+      default:
+        break;
+    }
+  }
+
+  return RDY_RESET;
+}
+
+void
+ADConverter::enableWatchdog(const bool enable)
+{
+  if (enable) {
+    this->conversion_group.cr1 |= ADC_CR1_AWDEN | ADC_CR1_AWDIE;
+    this->conversion_group.error_cb = &adcVsysErrorCallback;
+  } else {
+    this->conversion_group.cr1 &= ~(ADC_CR1_AWDEN | ADC_CR1_AWDIE);
+    this->conversion_group.error_cb = NULL;
+  }
+}
+
+
diff --git a/components/power/bq27500.cpp b/components/power/bq27500.cpp
new file mode 100644
index 0000000..694ee79
--- /dev/null
+++ b/components/power/bq27500.cpp
@@ -0,0 +1,424 @@
+#include <amiro/power/bq27500.hpp>
+
+#include <ch.hpp>
+#include <chprintf.h>
+
+#include <cstring>
+
+using namespace chibios_rt;
+using namespace amiro;
+using namespace BQ27500;
+
+Driver::Driver(I2CDriver &i2c_driver, const GPIO_TypeDef &batgd_pingrp, const uint8_t batgd_pin, const GPIO_TypeDef &batlow_pingrp, const uint8_t batlow_pin) :
+  BaseSensor<BQ27500::InitData,BQ27500::CalibData>(), i2c_driver(&i2c_driver), tx_params({I2C_ADDR, NULL, 0, NULL, 0}),
+  batgd_pingrp(&batgd_pingrp), batgd_pin(batgd_pin), batlow_pingrp(&batlow_pingrp), batlow_pin(batlow_pin)
+{}
+
+Driver::~Driver()
+{}
+
+chibios_rt::EvtSource*
+Driver::getEventSource()
+{
+  return &this->eventSource;
+}
+
+msg_t
+Driver::init(InitData* initialization_data)
+{
+  chprintf((BaseSequentialStream*) &SD1, "%s(%d): TODO\n", __FILE__, __LINE__);
+  return NOT_IMPLEMENTED;
+}
+
+msg_t
+Driver::update()
+{
+  msg_t res = SUCCESS;
+  res |= this->stdCommand(STD_CMD_TimeToEmpty, this->status.minutes_to_empty);
+  res |= this->stdCommand(STD_CMD_TimeToFull, this->status.minutes_to_full);
+  res |= this->stdCommand(STD_CMD_AveragePower, this->status.average_power_mW);
+  uint16_t tmp;
+  res |= this->stdCommand(STD_CMD_StateOfCharge, tmp);
+  this->status.state_of_charge = tmp;
+  return (res == SUCCESS)? SUCCESS : ERROR;
+}
+
+msg_t
+Driver::wakeup()
+{
+  const msg_t res = this->subCommand(SUB_CMD_CLEAR_HIBERNATE);
+  return (res? ERROR : SUCCESS);
+}
+
+msg_t
+Driver::hibernate()
+{
+  const msg_t res = this->subCommand(SUB_CMD_SET_HIBERNATE);
+  return (res? ERROR : SUCCESS);
+}
+
+#ifndef AMIRO_NCALIBRATION
+msg_t
+Driver::calibration(CalibData* calibration_data)
+{
+  DataFlashBlock block;
+  this->readDataFlashBlock(block, CONFIGURATION_Safety);
+
+  chprintf((BaseSequentialStream*) &SD1, "%s(%d):\n", __FILE__, __LINE__);
+  for(uint8_t i = 0; i < 32; ++i) {
+    chprintf((BaseSequentialStream*) &SD1, "%02X ", block.content.data[i]);
+  }
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+  chprintf((BaseSequentialStream*) &SD1, "%02X\n", block.content.checksum);
+
+  chprintf((BaseSequentialStream*) &SD1, "%s(%d): TODO\n", __FILE__, __LINE__);
+  return NOT_IMPLEMENTED;
+}
+#endif
+
+#ifndef AMIRO_NSELFTEST
+msg_t
+Driver::selftest()
+{
+  uint16_t val = 0;
+  Version version;
+
+  // read hardware version
+  version.value = 0;
+  if (this->subCommand(SUB_CMD_HW_VERSION, &version.value))
+  {
+    return ST_FAIL_READ_HW_VERSION;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "hardware version: %X%X-%X%X (0x%04X)\n", version.content.major_high, version.content.major_low, version.content.minor_high, version.content.minor_low, version.value);
+
+  // read firmware version
+  version.value = 0;
+  if (this->subCommand(SUB_CMD_FW_VERSION, &version.value))
+  {
+    return ST_FAIL_READ_FW_VERSION;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "firmware version: %X%X-%X%X (0x%04X)\n", version.content.major_high, version.content.major_low, version.content.minor_high, version.content.minor_low, version.value);
+
+  // read device name
+  uint8_t name_length = 0;
+  if (this->extCommand(EXT_CMD_DNAMELEN, EXT_CMD_READ, &name_length))
+  {
+    return ST_FAIL_READ_DEVICENAMELENGTH;
+  }
+  char name_buffer[9]; // maximum name length is 8
+  if (this->extCommand(EXT_CMD_DNAME, EXT_CMD_READ, (uint8_t*)name_buffer, name_length))
+  {
+    return ST_FAIL_READ_DEVICENAME;
+  }
+  name_buffer[name_length] = '\0';
+  chprintf((BaseSequentialStream*) &SD1, "device name: %s (%u characters)\n", name_buffer, name_length);
+
+  // read the current flags
+  Flags flags;
+  if (this->stdCommand(STD_CMD_FLAGS, flags.value))
+  {
+    return ST_FAIL_READ_FLAGS;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "flags: 0x%04X\n", flags.value);
+  chprintf((BaseSequentialStream*) &SD1, "  OTC     : %u\n", flags.content.otc);
+  chprintf((BaseSequentialStream*) &SD1, "  OTD     : %u\n", flags.content.otd);
+  chprintf((BaseSequentialStream*) &SD1, "  CHG_INH : %u\n", flags.content.chg_inh);
+  chprintf((BaseSequentialStream*) &SD1, "  XCHG    : %u\n", flags.content.xchg);
+  chprintf((BaseSequentialStream*) &SD1, "  FC      : %u\n", flags.content.fc);
+  chprintf((BaseSequentialStream*) &SD1, "  CHG     : %u\n", flags.content.chg);
+  chprintf((BaseSequentialStream*) &SD1, "  OCV_GD  : %u\n", flags.content.ocv_gd);
+  chprintf((BaseSequentialStream*) &SD1, "  WAIT_ID : %u\n", flags.content.wait_id);
+  chprintf((BaseSequentialStream*) &SD1, "  BAT_DET : %u\n", flags.content.bat_det);
+  chprintf((BaseSequentialStream*) &SD1, "  SOC1    : %u\n", flags.content.soc1);
+  chprintf((BaseSequentialStream*) &SD1, "  SOCF    : %u\n", flags.content.socf);
+  chprintf((BaseSequentialStream*) &SD1, "  DSG     : %u\n", flags.content.dsg);
+
+  // read the current controller status
+  ControlStatus ctrl_status;
+  if (this->subCommand(SUB_CMD_CONTROL_STATUS, &ctrl_status.value))
+  {
+    return ST_FAIL_READ_STATUS;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "control status: 0x%04X\n", ctrl_status.value);
+  chprintf((BaseSequentialStream*) &SD1, "  FAS       : %u\n", ctrl_status.content.fas);
+  chprintf((BaseSequentialStream*) &SD1, "  SS        : %u\n", ctrl_status.content.ss);
+  chprintf((BaseSequentialStream*) &SD1, "  CSV       : %u\n", ctrl_status.content.csv);
+  chprintf((BaseSequentialStream*) &SD1, "  CSA       : %u\n", ctrl_status.content.cca);
+  chprintf((BaseSequentialStream*) &SD1, "  BCA       : %u\n", ctrl_status.content.bca);
+  chprintf((BaseSequentialStream*) &SD1, "  HIBERNATE : %u\n", ctrl_status.content.hibernate);
+  chprintf((BaseSequentialStream*) &SD1, "  SNOOZE    : %u\n", ctrl_status.content.snooze);
+  chprintf((BaseSequentialStream*) &SD1, "  SLEEP     : %u\n", ctrl_status.content.sleep);
+  chprintf((BaseSequentialStream*) &SD1, "  LDMD      : %u\n", ctrl_status.content.ldmd);
+  chprintf((BaseSequentialStream*) &SD1, "  RUP_DIS   : %u\n", ctrl_status.content.rup_dis);
+  chprintf((BaseSequentialStream*) &SD1, "  VOK       : %u\n", ctrl_status.content.vok);
+  chprintf((BaseSequentialStream*) &SD1, "  QEN       : %u\n", ctrl_status.content.qen);
+
+  // if no battery was detected, abort
+  if (!flags.content.bat_det)
+  {
+    return ST_ABORT_NO_BAT;
+  }
+
+  // read the BATGD_N pin
+  chprintf((BaseSequentialStream*) &SD1, "battery good: %s\n", (this->isBatteryGood()? "yes" : "no"));
+
+  // read temperature
+  if (this->stdCommand(STD_CMD_TEMP, val))
+  {
+    return ST_FAIL_READ_TEMP;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "temperature: %fK (%fC)\n", float(val)/10.0f, float(val)/10.0f - 273.15f);
+
+  // read the full available capacity
+  if (this->stdCommand(STD_CMD_FAC, val))
+  {
+    return ST_FAIL_READ_FAC;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "full available capacity: %umAh\n", val);
+
+  // read the full charge capacity
+  if (this->stdCommand(STD_CMD_FCC, val))
+  {
+    return ST_FAIL_READ_FCC;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "full charge capacity: %umAh\n", val);
+
+  // read the remaining capacity
+  if (this->stdCommand(STD_CMD_RM, val))
+  {
+    return ST_FAIL_READ_RM;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "remaining capacity capacity: %umAh\n", val);
+
+  // read the state of charge
+  if (this->stdCommand(STD_CMD_SOC, val))
+  {
+    return ST_FAIL_READ_SOC;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "state of charge: %3u%%\n", val);
+
+  // read voltage
+  if (this->stdCommand(STD_CMD_VOLT, val))
+  {
+    return ST_FAIL_READ_VOLT;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "voltage: %umV\n", val);
+
+  // read average current
+  if (this->stdCommand(STD_CMD_AI, val))
+  {
+    return ST_FAIL_READ_AI;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "average current: %dmA\n", *reinterpret_cast<int8_t*>(&val));
+
+  // read average power
+  if (this->stdCommand(STD_CMD_AP, val))
+  {
+    return ST_FAIL_READ_AP;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "average power: %dmW\n", *reinterpret_cast<int8_t*>(&val));
+
+  // read the BATLOW pin
+  chprintf((BaseSequentialStream*) &SD1, "battery low: %s\n", (this->isBatteryLow()? "yes" : "no"));
+
+  // read the time to empty
+  if (this->stdCommand(STD_CMD_TTE, val))
+  {
+    return ST_FAIL_READ_TTE;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "time to empty: ");
+  if (uint16_t(~val)) {
+    chprintf((BaseSequentialStream*) &SD1, "%u minutes", val);
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "(not discharging)");
+  }
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+
+  // read the time to full
+  if (this->stdCommand(STD_CMD_TTF, val))
+  {
+    return ST_FAIL_READ_TTF;
+  }
+  chprintf((BaseSequentialStream*) &SD1, "time to full: ");
+  if (uint16_t(~val)) {
+    chprintf((BaseSequentialStream*) &SD1, "%u minutes", val);
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "(not charging)");
+  }
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+
+  return ST_OK;
+}
+#endif
+
+bool
+Driver::isBatteryGood() const
+{
+  return (palReadPad(this->batgd_pingrp, this->batgd_pin) == PAL_LOW);
+}
+
+bool
+Driver::isBatteryLow() const
+{
+  return (palReadPad(this->batlow_pingrp, this->batlow_pin) == PAL_HIGH);
+}
+
+const Driver::UpdateData&
+Driver::getStatus() const
+{
+  return this->status;
+}
+
+msg_t
+Driver::main(void)
+{
+  while (!this->shouldTerminate())
+  {
+    this->update();
+    this->eventSource.broadcastFlags(0);
+
+    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(1000));
+  }
+
+  return RDY_OK;
+}
+
+msg_t
+Driver::stdCommand(const StandardCommand cmd, uint16_t &dst)
+{
+  uint8_t buffer[2];
+  this->tx_params.txbuf = reinterpret_cast<const uint8_t*>(&cmd);
+  this->tx_params.txbytes = 1;
+  this->tx_params.rxbuf = buffer;
+  this->tx_params.rxbytes = 2;
+
+  this->i2c_driver->acquireBus();
+  const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params);
+  this->i2c_driver->releaseBus();
+
+  if (!res) {
+    dst = uint16_t((buffer[1] << 8) | buffer[0]);
+#ifndef NDEBUG
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
+#endif
+  }
+
+  return res;
+}
+
+msg_t
+Driver::readName()
+{
+  uint8_t buffer[9];
+  uint8_t reg = 0x62u;
+  this->tx_params.txbuf = &reg;
+  this->tx_params.txbytes = 1;
+  this->tx_params.rxbuf = &buffer[0];
+  this->tx_params.rxbytes = 8;
+
+  this->i2c_driver->acquireBus();
+  const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params);
+  this->i2c_driver->releaseBus();
+
+  buffer[buffer[0] + 1] = '\0';
+
+  chprintf((BaseSequentialStream*) &SD1, "name: %u - %s\n", buffer[0], (char*)&buffer[1]);
+
+  return res;
+}
+
+msg_t
+Driver::subCommand(const ControlSubcommand cmd, uint16_t *dst)
+{
+  uint8_t buffer[3] = {STD_CMD_CNTL, uint8_t(cmd & 0x00FFu), uint8_t((cmd & 0xFF00u) >> 8)};
+  this->tx_params.txbuf = buffer;
+  this->tx_params.txbytes = 3;
+  this->tx_params.rxbytes = 0;
+
+  this->i2c_driver->acquireBus();
+  msg_t res = this->i2c_driver->masterTransmit(&this->tx_params);
+  this->i2c_driver->releaseBus();
+  if (dst) {
+    this->tx_params.txbytes = 1;
+
+    this->tx_params.rxbuf = &buffer[1];
+    this->tx_params.rxbytes = 2;
+
+    BaseThread::sleep(US2ST(2));
+    this->i2c_driver->acquireBus();
+    res |= this->i2c_driver->masterTransmit(&this->tx_params);
+    this->i2c_driver->releaseBus();
+  }
+
+#ifndef NDEBUG
+  if (res) {
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
+  }
+#endif
+
+  if (dst && !res) {
+    *dst = uint16_t((buffer[2] << 8) | buffer[1]);
+  }
+
+  return res;
+}
+
+msg_t
+Driver::extCommand(const ExtendedCommand cmd, const ExtendedCommandAccess rw, uint8_t* buf, const uint8_t length, const uint8_t offset)
+{
+  if (!buf) {
+#ifndef NDEBUG
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: received NULL-pointer as buffer\n", __FILE__ , __LINE__);
+#endif
+    return ~RDY_OK;
+  }
+  if (rw != EXT_CMD_WRITE && rw != EXT_CMD_READ) {
+#ifndef NDEBUG
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: invalid access mode selected\n", __FILE__ , __LINE__);
+#endif
+    return ~RDY_OK;
+  }
+  if (length > 33) {
+#ifndef NDEBUG
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: length exceeds maximum of 33 bytes\n", __FILE__ , __LINE__);
+#endif
+    return ~RDY_OK;
+  }
+
+  uint8_t in_buffer[34];
+  in_buffer[0] = cmd + offset;
+  if (rw == EXT_CMD_WRITE) {
+    strncpy((char*)&in_buffer[1], (char*)buf, length);
+  }
+  this->tx_params.txbuf = in_buffer;
+  this->tx_params.txbytes = 1 + ((rw == EXT_CMD_WRITE)? length : 0);
+  this->tx_params.rxbuf = (rw == EXT_CMD_READ)? buf : NULL;
+  this->tx_params.rxbytes = (rw == EXT_CMD_READ)? length : 0;
+
+  this->i2c_driver->acquireBus();
+  const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params);
+  this->i2c_driver->releaseBus();
+
+#ifndef NDEBUG
+  if (res) {
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d)\n", __FILE__ , __LINE__ , res);
+  }
+#endif
+
+  return res;
+}
+
+msg_t
+Driver::readDataFlashBlock(DataFlashBlock &block, const DataFlashSubClassID sub_id, const uint8_t sub_block)
+{
+  block.raw[0] = sub_id;
+  block.raw[1] = sub_block;
+  msg_t res = this->extCommand(EXT_CMD_DFCLS, EXT_CMD_WRITE, &block.raw[0]);
+  res |= this->extCommand(EXT_CMD_DFBLK, EXT_CMD_WRITE, &block.raw[1]);
+
+  BaseThread::sleep(US2ST(1)); // Without this delay the whole block is shifted and the first byte is lost. TODO: investigate
+  res |= this->extCommand(EXT_CMD_DFD, EXT_CMD_READ, block.raw, 33);
+
+  return res;
+}
diff --git a/components/power/ina219.cpp b/components/power/ina219.cpp
new file mode 100644
index 0000000..d1d3121
--- /dev/null
+++ b/components/power/ina219.cpp
@@ -0,0 +1,375 @@
+#include <amiro/power/ina219.hpp>
+
+#include <ch.hpp>
+#include <chprintf.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+using namespace INA219;
+
+Driver::Driver(I2CDriver &i2c_driver, const uint8_t i2c_address) :
+  BaseSensor<InitData,CalibData>(), i2c_driver(&i2c_driver), tx_params({i2c_address, NULL, 0, NULL, 0}), current_lsb_uA(0)
+{
+  this->config.content.brng = Configuration::BRNG_DEFAULT;
+  this->config.content.pg = Configuration::PGA_DEFAULT;
+  this->config.content.badc = Configuration::ADC_DEFAULT;
+  this->config.content.sadc = Configuration::ADC_DEFAULT;
+  this->config.content.mode = Configuration::MODE_DEFAULT;
+
+  return;
+}
+
+Driver::~Driver()
+{}
+
+msg_t
+Driver::init(InitData* initialization_data)
+{
+  if(!initialization_data)
+  {
+    return ERROR;
+  }
+
+  // write configuration
+  if (this->writeRegister(REG_CONFIGURATION, initialization_data->configuration.value & MASK_CONFIGURATION)) {
+    return ERROR;
+  }
+  this->config.value = initialization_data->configuration.value & MASK_CONFIGURATION;
+
+  if (this->writeRegister(REG_CALIBRATION, initialization_data->calibration & MASK_CALIBRATION)) {
+    return ERROR;
+  }
+
+  this->current_lsb_uA = initialization_data->current_lsb_uA;
+
+  return this->update();
+}
+
+msg_t
+Driver::update()
+{
+  this->status.power = 0;
+  msg_t res = this->readRegister(REG_BUS_VOLTAGE, this->status.bus_voltage.value);
+  res |= this->readRegister(REG_POWER, this->status.power);
+
+  // if the power register was not updated yet, try again
+  while (!this->status.bus_voltage.content.conversion_ready || this->status.power == 0 || res != 0)
+  {
+    BaseThread::sleep(MS2ST(10));
+    res |= this->readRegister(REG_BUS_VOLTAGE, this->status.bus_voltage.value);
+    res |= this->readRegister(REG_POWER, this->status.power);
+  }
+
+  return res ? ERROR : SUCCESS;
+}
+
+msg_t
+Driver::wakeup()
+{
+  if (this->writeRegister(REG_CONFIGURATION, this->config.value)) {
+    return ERROR;
+  } else {
+    return this->update();
+  }
+}
+
+msg_t
+Driver::hibernate()
+{
+  Configuration::Register tmp_config = this->config;
+  tmp_config.content.mode = Configuration::MODE_PowerDown;
+  if (this->writeRegister(REG_CONFIGURATION, tmp_config.value)) {
+    return ERROR;
+  } else {
+    return SUCCESS;
+  }
+
+}
+
+#ifndef AMIRO_NCALIBRATION
+msg_t
+Driver::calibration(CalibData* calibration_data)
+{
+  if (!calibration_data) {
+    return ERROR;
+  }
+
+  uint16_t current_lsb_uA = calibration_data->input.current_lsb_uA;
+  if (current_lsb_uA < calibration_data->input.max_expected_current_A / 0.032767f) {
+    current_lsb_uA = calibration_data->input.max_expected_current_A / 0.032767f;
+  } else if (current_lsb_uA > calibration_data->input.max_expected_current_A / 0.004096f) {
+    current_lsb_uA = calibration_data->input.max_expected_current_A / 0.004096f;
+  }
+
+  const uint16_t calibration_value = uint16_t(40960 / (current_lsb_uA * calibration_data->input.shunt_resistance_O));
+
+  float V_shunt_max;
+  switch (calibration_data->input.configuration.content.pg)
+  {
+    case Configuration::PGA_40mV:
+      V_shunt_max = 0.04f;
+      break;
+    case Configuration::PGA_80mV:
+      V_shunt_max = 0.08f;
+      break;
+    case Configuration::PGA_160mV:
+      V_shunt_max = 0.16f;
+      break;
+    case Configuration::PGA_320mV:
+      V_shunt_max = 0.32f;
+      break;
+  }
+
+  const float max_current_before_overflow = ( (current_lsb_uA * 0.032767f >= V_shunt_max) / (calibration_data->input.shunt_resistance_O) )?
+                                            V_shunt_max / calibration_data->input.shunt_resistance_O :
+                                            current_lsb_uA * 0.032767f;
+
+  const float max_shuntvoltage_before_overflow = ( (max_current_before_overflow * calibration_data->input.shunt_resistance_O) >= V_shunt_max )?
+                                                 V_shunt_max :
+                                                 max_current_before_overflow * calibration_data->input.shunt_resistance_O;
+
+  calibration_data->output.max_current_before_overflow_A = max_current_before_overflow;
+  calibration_data->output.max_shuntvoltage_before_overflow_V = max_shuntvoltage_before_overflow;
+  calibration_data->output.current_lsb_uA = current_lsb_uA;
+  calibration_data->output.calibration_value = calibration_value;
+
+  return SUCCESS;
+}
+#endif
+
+#ifndef AMIRO_NSELFTEST
+msg_t
+Driver::selftest()
+{
+  struct RegisterContent {
+    Configuration::Register configuration;
+    uint16_t shunt_voltage = 0;
+    BusVoltage bus_voltage;
+    uint16_t power = 0;
+    uint16_t current = 0;
+    uint16_t calibration = 0;
+  };
+
+  // backup the current status
+  RegisterContent backup;
+  msg_t res = this->readRegister(REG_CONFIGURATION, backup.configuration.value);
+  res |= this->readRegister(REG_SHUNT_VOLTAGE, backup.shunt_voltage);
+  res |= this->readRegister(REG_BUS_VOLTAGE, backup.bus_voltage.value);
+  res |= this->readRegister(REG_POWER, backup.power);
+  res |= this->readRegister(REG_CURRENT, backup.current);
+  res |= this->readRegister(REG_CALIBRATION, backup.calibration);
+  if (res ||
+      !(backup.configuration.value ||
+        backup.shunt_voltage ||
+        backup.bus_voltage.value ||
+        backup.power ||
+        backup.current ||
+        backup.calibration))
+  {
+    return ST_FAIL_BACKUP;
+  }
+
+  // reset the INA219
+  if (this->reset())
+  {
+    return ST_FAIL_RESET;
+  }
+
+  // read the configuration
+  Configuration::Register config = this->readConfiguration();
+
+  // check for the default configuration
+  Configuration::Register config_default;
+  config_default.content.brng = Configuration::BRNG_DEFAULT;
+  config_default.content.pg = Configuration::PGA_DEFAULT;
+  config_default.content.badc = Configuration::ADC_DEFAULT;
+  config_default.content.sadc = Configuration::ADC_DEFAULT;
+  config_default.content.mode = Configuration::MODE_DEFAULT;
+  if (config.value != config_default.value)
+  {
+    return ST_FAIL_IS_DEFAULT;
+  }
+
+  // revert to the previous configuration
+  if (this->writeRegister(REG_CONFIGURATION, backup.configuration.value))
+  {
+    return ST_FAIL_WRITE_CONFIG;
+  }
+
+  // revert to the previous calibration
+  if (this->writeRegister(REG_CALIBRATION, backup.calibration))
+  {
+    return ST_FAIL_WRITE_CALIB;
+  }
+
+  // read the current configuration
+  if (this->readConfiguration().value != backup.configuration.value)
+  {
+    return ST_FAIL_CHECK_CONFIG;
+  }
+
+  // read the current calibration
+  if (this->readCalibration() != backup.calibration)
+  {
+    return ST_FAIL_CHECK_CALIB;
+  }
+
+  // read and print the current status
+  INA219::BusVoltage bus_voltage = this->readBusVoltage();
+  uint16_t power;
+  this->readRegister(REG_POWER, power);
+  // wait until the bus voltage and the power register are valid
+  while (!bus_voltage.conversion_ready || power == 0)
+  {
+    BaseThread::sleep(MS2ST(10));
+    bus_voltage = this->readBusVoltage();
+    this->readRegister(REG_POWER, power);
+  }
+  chprintf((BaseSequentialStream*) &SD1, "shunt voltage : %fV\n", this->readShuntVoltage_uV() / 1000000.f);
+  chprintf((BaseSequentialStream*) &SD1, "bus voltage   : %fV\n", bus_voltage.voltage_uV / 1000000.f);
+  chprintf((BaseSequentialStream*) &SD1, "power         : %fW\n", this->readPower_uW() / 1000000.f);
+  chprintf((BaseSequentialStream*) &SD1, "current       : %fA\n", this->readCurrent_uA() / 1000000.f);
+
+  return ST_OK;
+}
+#endif
+
+Configuration::Register
+Driver::readConfiguration()
+{
+  Configuration::Register reg;
+  this->readRegister(REG_CONFIGURATION, reg.value);
+  return reg;
+}
+
+int32_t
+Driver::readShuntVoltage_uV()
+{
+  uint16_t val = 0;
+  this->readRegister(REG_SHUNT_VOLTAGE, val);
+  /*
+   * Depending on the configuration either one, two, three or four most significant bits are used for coding the sign (no two's complement!)
+   * -> Masking bits in order to separate sign bits and value bits.
+   * Furthermore, the least significant bit represents 10uV.
+   * -> Multiplication by 10 or -10 respectively.
+   */
+  switch (this->config.content.pg)
+  {
+    case Configuration::PGA_320mV:
+      return ( uint32_t(val & 0x7FFFu) * ((val & 0x8000u)? -10 : 10) );
+    case Configuration::PGA_160mV:
+      return ( uint32_t(val & 0x3FFFu) * ((val & 0xC000u)? -10 : 10) );
+    case Configuration::PGA_80mV:
+      return ( uint32_t(val & 0x1FFFu) * ((val & 0xE000u)? -10 : 10) );
+    case Configuration::PGA_40mV:
+      return ( uint32_t(val & 0x0FFFu) * ((val & 0xF000u)? -10 : 10) );
+  }
+}
+
+INA219::BusVoltage
+Driver::readBusVoltage()
+{
+  Driver::BusVoltage reg;
+  this->readRegister(REG_BUS_VOLTAGE, reg.value);
+  return this->busVoltageReg2uV(reg);
+}
+
+uint32_t
+Driver::readPower_uW()
+{
+  uint16_t val = 0;
+  this->readRegister(REG_POWER, val);
+  return this->powerReg2uW(val);
+}
+
+int32_t
+Driver::readCurrent_uA()
+{
+  uint16_t val = 0;
+  this->readRegister(REG_CURRENT, val);
+
+  /*
+   * Reinterpret register value as a signed integer (two's complement).
+   * Multiply with the value of the least significant bit.
+   */
+  return int32_t(*reinterpret_cast<int16_t*>(&val)) * this->current_lsb_uA;
+}
+
+uint16_t
+Driver::readCalibration()
+{
+  uint16_t val = 0;
+  this->readRegister(REG_CALIBRATION, val);
+  return (val & MASK_CALIBRATION);
+}
+
+uint8_t
+Driver::reset()
+{
+  const msg_t res = this->writeRegister(REG_CONFIGURATION, MASK_RESET);
+  return (res? ERROR : SUCCESS);
+}
+
+msg_t
+Driver::main(void)
+{
+  while (!this->shouldTerminate())
+  {
+    this->update();
+    //this->eventSource.broadcastFlags(0);
+    this->sleep(MS2ST(1000));
+    //this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(1000));
+  }
+
+  return RDY_OK;
+}
+
+msg_t
+Driver::readRegister(const RegisterAddress reg, uint16_t& dst)
+{
+  uint8_t buffer[2];
+  this->tx_params.txbuf = reinterpret_cast<const uint8_t*>(&reg);
+  this->tx_params.txbytes = 1;
+  this->tx_params.rxbuf = buffer;
+  this->tx_params.rxbytes = 2;
+
+  this->i2c_driver->acquireBus();
+  const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params);
+  this->i2c_driver->releaseBus();
+
+  if (!res) {
+    dst = (buffer[0] << 8) | buffer[1];
+    if (reg == REG_CALIBRATION)
+      dst &= MASK_CALIBRATION;
+#ifndef NDEBUG
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors());
+#endif
+  }
+
+  return res;
+}
+
+msg_t
+Driver::writeRegister(const RegisterAddress reg, const uint16_t& val)
+{
+  const uint8_t buffer[3] = {reg,
+                             static_cast<uint8_t>((val & 0xFF00u) >> 8),
+                             static_cast<uint8_t>(val & 0x00FFu)};
+  this->tx_params.txbuf = buffer;
+  this->tx_params.txbytes = 3;
+  this->tx_params.rxbytes = 0;
+
+  this->i2c_driver->acquireBus();
+  const msg_t res = this->i2c_driver->masterTransmit(&this->tx_params);
+  this->i2c_driver->releaseBus();
+
+#ifndef NDEBUG
+  if (res) {
+    chprintf((BaseSequentialStream*) &SD1, "%s(%d): ERROR: i2c transmit failed (%d | 0x%08X)\n", __FILE__ , __LINE__ , res, this->i2c_driver->getErrors());
+  }
+#endif
+
+  return res;
+}
+
diff --git a/components/proximity/vcnl4020.cpp b/components/proximity/vcnl4020.cpp
new file mode 100644
index 0000000..23befa7
--- /dev/null
+++ b/components/proximity/vcnl4020.cpp
@@ -0,0 +1,247 @@
+#include <ch.hpp>
+#include <hal.h>
+#include <chdebug.h>
+#include <chprintf.h>
+
+#include <amiro/bus/i2c/I2CParams.hpp>
+#include <amiro/bus/i2c/I2CDriver.hpp>
+#include <amiro/proximity/vcnl4020.hpp>
+
+using namespace chibios_rt;
+
+namespace amiro {
+
+VCNL4020::
+~VCNL4020() {
+
+}
+
+VCNL4020::
+VCNL4020(I2CDriver *driver, const VCNL4020Config *config) :
+  BaseStaticThread<256>(),
+  driver(driver),
+  config(config),
+  ambient(0x0000u),
+  proximity(0x0000u),
+  proximityOffset(0x0000u) {
+
+  this->tx_params.addr = VCNL4020::SLA;
+
+}
+
+uint16_t
+VCNL4020::
+getAmbientLight() {
+
+  return this->ambient;
+}
+
+void
+VCNL4020::
+setProximityOffset(uint16_t offset) {
+
+  this->proximityOffset = offset;
+}
+
+uint16_t
+VCNL4020::
+getProximityOffset() {
+
+  return this->proximityOffset;
+}
+
+EvtSource*
+VCNL4020::
+getEventSource() {
+
+  return &this->eventSource;
+}
+
+uint16_t
+VCNL4020::
+getProximity() {
+
+  return this->proximity;
+}
+
+uint16_t
+VCNL4020::
+getProximityScaledWoOffset() {
+
+  if (this->proximity <= this->proximityOffset)
+    return 0;
+
+  // Scale factor for the offset-less proximity value, so that we can reach full-scale
+  const float scaleFactor = float(0xFFFFu) / float((0xFFFFu - this->proximityOffset));
+
+  return uint16_t(float(this->proximity - this->proximityOffset) * scaleFactor);
+}
+
+msg_t
+VCNL4020::
+main(void) {
+
+  I2CDriver *drv = this->driver;
+  msg_t res;
+
+  this->setName("Vcnl4020");
+
+  drv->acquireBus();
+
+  /* exit if writing configuration fails */
+  res = this->writeIRConf();
+
+  drv->releaseBus();
+
+  if (res)
+    return RDY_RESET;
+
+  //TODO Read calibration from eeprom
+
+  while (!this->shouldTerminate()) {
+
+    drv->acquireBus();
+
+    this->readIntensities();
+    drv->releaseBus();
+
+    this->eventSource.broadcastFlags(0);
+
+    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));
+  }
+
+  return RDY_OK;
+}
+
+msg_t
+VCNL4020::
+writeIRConf() {
+
+  msg_t res;
+  const VCNL4020Config *cfg = this->config;
+
+  uint8_t buffer[4];
+  this->tx_params.txbuf = buffer;
+  this->tx_params.txbytes = 4;
+  this->tx_params.rxbytes = 0;
+
+  buffer[0] = offsetof(VCNL4020::registers, proximity_rate);
+  buffer[1] = cfg->proximity_rate;
+  buffer[2] = cfg->ir_led_current_mA > 200 ? 20u : cfg->ir_led_current_mA / 10u;
+  buffer[3] = cfg->ambient_parameter;
+
+  res = this->driver->masterTransmit(&this->tx_params);
+
+  buffer[0] = offsetof(VCNL4020::registers, command);
+  buffer[1] = cfg->command;
+
+  this->tx_params.txbytes = 2;
+
+  if (!res)
+    res = this->driver->masterTransmit(&this->tx_params);
+
+  return res;
+}
+
+msg_t
+VCNL4020::
+readIntensities() {
+
+  msg_t res;
+  uint8_t buffer[4];
+  uint8_t reg = offsetof(VCNL4020::registers, ambient_result);
+  this->tx_params.txbuf = &reg;
+  this->tx_params.txbytes = 1;
+  this->tx_params.rxbuf = buffer;
+  this->tx_params.rxbytes = 4;
+
+  res = this->driver->masterTransmit(&this->tx_params);
+
+  if (!res) {
+
+    /* update internal values */
+    this->ambient = (buffer[0] << 8) | buffer[1];
+    this->proximity = (buffer[2] << 8) | buffer[3];
+
+  }
+
+  return res;
+
+}
+
+uint8_t
+VCNL4020::
+getCheck(void) {
+
+  int8_t resTx;
+  uint8_t rxBuffer[2]; /** This is a bug workaround: 1 byte is impossible to read on STM32F1*/
+  const uint8_t txBuffer = uint8_t(offsetof(VCNL4020::registers, revision));
+
+  // Use own datastructure, so that there is now faulty
+  // behaviour with the other threads using it
+  I2CTxParams tx_params = this->tx_params;
+  tx_params.txbuf = &txBuffer;
+  tx_params.txbytes = 1;
+  tx_params.rxbytes = 2; /** This is a bug workaround: 1 byte is impossible to read on STM32F1*/
+  tx_params.rxbuf = rxBuffer;
+
+  // Read the data
+  resTx = this->driver->masterTransmit(&tx_params);
+  // Failed to transmit
+  if (resTx != RDY_OK || VCNL4020::PRODUCT_ID_REVISION != rxBuffer[0]) {
+    return VCNL4020::CHECK_FAIL;
+  } else {
+    return VCNL4020::CHECK_OK;
+  }
+
+}
+
+uint8_t
+VCNL4020::
+calibrate() {
+
+  uint16_t proximityFloorMeanValue;
+
+  // Get the offset
+  msg_t res = calibrateOffset(proximityFloorMeanValue);
+  if (res == CALIB_OK) {
+    this->proximityOffset = proximityFloorMeanValue;
+    //TODO Write value to eeprom
+    return res;
+  } else {
+    return res;
+  }
+
+}
+
+uint8_t
+VCNL4020::
+calibrateOffset(uint16_t &proximityFloorMeanValue) {
+
+  uint32_t tmpProximityFloorMeanValue;
+
+  // Register an event listener, to receive updates
+  chibios_rt::EvtListener eventTimerEvtListener;
+  chibios_rt::EvtSource *vcnlEvtSource;
+  vcnlEvtSource = reinterpret_cast<EvtSource *>(this->getEventSource());
+  vcnlEvtSource->registerOne(&eventTimerEvtListener,0);
+
+  // Get the first values
+  tmpProximityFloorMeanValue = this->getProximity();
+
+  // Get the others with a floating mean
+  const uint32_t maxValues = 20;
+  for (uint32_t idxMean = 2; idxMean <= maxValues; ++idxMean) {
+    this->waitOneEvent(ALL_EVENTS); /* Wait for a new update */
+    tmpProximityFloorMeanValue = (((idxMean-1) * tmpProximityFloorMeanValue) + uint32_t(this->getProximity())) / idxMean;
+  }
+
+  vcnlEvtSource->unregister(&eventTimerEvtListener);
+
+  // Cast the calculated offset to the temporary member variable
+  proximityFloorMeanValue = uint16_t(tmpProximityFloorMeanValue);
+
+  return CALIB_OK;
+}
+
+} /* amiro */
diff --git a/components/radio/a2500r24a.cpp b/components/radio/a2500r24a.cpp
new file mode 100644
index 0000000..5167b77
--- /dev/null
+++ b/components/radio/a2500r24a.cpp
@@ -0,0 +1,66 @@
+#include <string.h>
+
+#include <amiro/util/util.h>
+#include <amiro/bus/spi/HWSPIDriver.hpp>
+#include <amiro/radio/a2500r24a.hpp>
+
+namespace amiro {
+
+A2500R24A::A2500R24A(HWSPIDriver *driver)
+    : driver(driver) {
+
+}
+
+A2500R24A::~A2500R24A() {
+
+}
+
+chibios_rt::EvtSource*
+A2500R24A::getEventSource() {
+  return &this->eventSource;
+}
+
+msg_t A2500R24A::main() {
+
+  while (!this->shouldTerminate()) {
+
+    updateSensorData();
+
+    this->eventSource.broadcastFlags(0);
+
+    this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200));
+
+  }
+  return RDY_OK;
+}
+
+void A2500R24A::updateSensorData() {
+
+}
+
+msg_t A2500R24A::configure(A2500R24AConfig *config) {
+
+  return RDY_OK;
+
+}
+
+uint8_t A2500R24A::getCheck() {
+
+  const size_t buffer_size = 1 /* addressing */
+                              + 1; /* who am i */
+  uint8_t buffer[buffer_size];
+
+  // Exchange the data with the A2500R24A radio chip
+  // Specify the adress and the mode
+  buffer[0] =  offsetof(A2500R24A::registerStatus, PARTNUM) | A2500R24A::SPI_READ | A2500R24A::SPI_STATUS_ACCESS;
+  this->driver->exchange(buffer, buffer, buffer_size);
+  // Check
+  if (buffer[1] == A2500R24A::A2500R24A_PARTNUM) {
+          return A2500R24A::CHECK_OK;
+  } else {
+          return A2500R24A::CHECK_FAIL;
+  }
+
+}
+
+} /* amiro */
diff --git a/components/serial_reset/iwrap_can_mux.cpp b/components/serial_reset/iwrap_can_mux.cpp
new file mode 100644
index 0000000..f8d7318
--- /dev/null
+++ b/components/serial_reset/iwrap_can_mux.cpp
@@ -0,0 +1,278 @@
+#include <ch.h>
+#include <hal.h>
+#include <string.h>  // memcpy
+#include <chprintf.h>
+#include <amiro/Constants.h>
+
+#include <amiro/serial_reset/iwrap_can_mux.hpp>
+
+using namespace amiro;
+using namespace chibios_rt;
+
+IwrapCanMux::IwrapCanMux(SerialDriver *_sd_, CANDriver *can, const uint8_t boardId)
+  : BaseSequentialStreamInterface(),
+    sd_(_sd_),
+    canDriver(can)
+{
+    myID = boardId;
+    queryShellID = boardId;
+    replyShellID = boardId;
+
+    resetStatus = 0;
+    resetAddBytes = 0;
+
+    txmsg.IDE = CAN_IDE_STD;
+    txmsg.RTR = CAN_RTR_DATA;
+    txmsg.DLC = 0;
+
+    bluetoothStatus = BLUETOOTH_STATUS_START;
+}
+
+IwrapCanMux::~IwrapCanMux() {
+
+};
+
+
+/* Check bluetooth status
+void IwrapCanMux::checkBluetoothStatus(void) {
+
+      BluetoothUartReceiveByte(&byteData);
+      switch (bluetoothStatus) {
+
+        case BLUETOOTH_STATUS_START:
+          if (byteData == BLUETOOTH_START_FRAME) {
+            bluetoothStatus++;
+          }
+          break;
+
+        case BLUETOOTH_STATUS_LINK:
+            bluetoothLinkID = byteData;
+            bluetoothStatus++;
+          break;
+
+        case BLUETOOTH_STATUS_FLAGS:
+          if (byteData == BLUETOOTH_FLAGS) {
+            bluetoothStatus++;
+          } else {
+            bluetoothStatus = BLUETOOTH_STATUS_START;
+          }
+          break;
+
+        case BLUETOOTH_STATUS_LENGTH:
+          bluetoothStatus++;
+          bluetoothDataLength = byteData;
+          break;
+
+        case BLUETOOTH_STATUS_DATA:
+          if (--bluetoothDataLength == 0) {
+            bluetoothStatus++;
+          }
+          if (bluetoothLinkID == 0xFF) {
+          } else {
+            // read data
+          }
+          break;
+
+        default:
+          bluetoothStatus = BLUETOOTH_STATUS_START;
+          if (byteData == ~bluetoothLinkID) {
+            // iWRAP check
+          }
+          break;
+      }
+}
+*/
+
+
+/*
+ * resets system for entering in bootloader.
+ */
+void IwrapCanMux::restartSystem(void) {
+  //TODO Set reset SYS_PD_N
+  NVIC_SystemReset();
+}
+
+/*
+ * checks bytes for reset command sequence.
+ * if the last byte and the following identification number of the device have been sent,
+ * the microcontroller will be reset
+ */
+bool IwrapCanMux::checkByteForBLReset(uint8_t *inputs, size_t size) {
+  /* Run throug all given bytes */
+  for (size_t idx=0; idx < size; idx++) {
+    uint8_t input = *(inputs+idx);
+
+    /*
+     * if the last byte of reset command sequence has not been sent yet,
+     * check new byte and handle reset status
+     */
+    if (resetStatus < INPUT_BL_RESET_LENGTH) {
+      if (input == inputBLReset[resetStatus]) {
+        resetStatus++;
+        if (resetStatus == INPUT_BL_RESET_LENGTH) {
+          chprintf((BaseSequentialStream*)sd_, "\n\nDo reset ...\n\n");
+          uint8_t ledCount;
+          for (ledCount=0; ledCount<20; ledCount++) {
+//            boardWriteLed(1);
+            chThdSleepMilliseconds(100);
+//            boardWriteLed(0);
+            chThdSleepMilliseconds(100);
+          }
+          restartSystem();
+        }
+      } else {
+        resetStatus = 0;
+        resetAddBytes = 0;
+      }
+
+    /*
+     * if the last byte of reset command sequence has been sent,
+     * save new byte as identification number of the device which shall be flashed and
+     * reset microcontroller
+     */
+    } else {
+      inputAddBytes[resetAddBytes] = input;
+      resetAddBytes++;
+      if (resetAddBytes >= INPUT_BL_RESET_ADDITIONAL_BYTES) {
+        chprintf((BaseSequentialStream*)sd_, "\n\nAt device %x: Do reset ...\n\n", input);
+        chThdSleepMilliseconds(OFFSET_TIME_RESET_MS);
+        restartSystem();
+      }
+    }
+  }
+
+  return (resetStatus > 0);
+}
+
+msg_t IwrapCanMux::sendViaCan(uint32_t id, uint8_t *inputs, size_t size) {
+  msg_t status;
+  txmsg.SID = (id << CAN::DEVICE_ID_SHIFT) | myID;
+  for (size_t i=0; i<size; i+=8) {
+    txmsg.DLC = (size < (i + 8)) ? size-i : 8;
+    memcpy(txmsg.data8, &inputs[i], txmsg.DLC);
+    status = canTransmit(canDriver, CAN_TX_MAILBOXES, &txmsg, TIME_INFINITE);
+    if (status!= RDY_OK)
+      return status;
+  }
+
+  return RDY_OK;
+}
+
+/*
+ * Interface implementation.
+ */
+
+size_t IwrapCanMux::write(const uint8_t *bp, size_t n) {
+  size_t size;
+
+  if (myID != replyShellID) {
+     msg_t status = sendViaCan(CAN::SHELL_REPLY_ID(replyShellID), (uint8_t *)bp, n);
+     BaseThread::sleep(US2ST(500));
+     size = (status == RDY_OK) ? n : 0;
+  } else {
+    size = sdWrite(sd_, bp, n);
+  }
+
+  return size;
+}
+
+size_t IwrapCanMux::read(uint8_t *bp, size_t n) {
+  size_t size = sdRead(sd_, inputChar, n);
+
+  checkByteForBLReset(inputChar, size);
+
+  if (myID != queryShellID)
+    sendViaCan(CAN::SHELL_QUERY_ID(queryShellID),inputChar, size);
+  else
+    memcpy(bp, inputChar, size);
+
+  return size;
+}
+
+msg_t IwrapCanMux::put(uint8_t b) {
+  msg_t byte;
+
+  if (myID != replyShellID) {
+    byte = sendViaCan(CAN::SHELL_REPLY_ID(replyShellID), &b, 1);
+    BaseThread::sleep(US2ST(200));
+  } else {
+    byte = sdPut(sd_, b);
+  }
+
+  return byte;
+}
+
+msg_t IwrapCanMux::get(void) {
+  msg_t byte = sdGet(sd_);
+  uint8_t b = (uint8_t) byte;
+
+  checkByteForBLReset(&b, 1);
+
+  if (myID != queryShellID)
+    sendViaCan(CAN::SHELL_QUERY_ID(queryShellID), &b, 1);
+
+  return byte;
+}
+
+//----------------------------------------------------------------
+
+/*
+ * Converse data from CAN bus into input queue of serial driver.
+ */
+
+void IwrapCanMux::convCan2Serial(uint8_t *data_can, size_t n) {
+  chSysLock();
+  for (size_t i=0; i < n; i++)
+     sdIncomingDataI(sd_, data_can[i]);
+  chSysUnlock();
+}
+
+void IwrapCanMux::sendSwitchCmd(uint8_t setid) {
+
+  if (myID == setid)
+    chprintf((BaseSequentialStream*)sd_, "Stay at the same shell\n");
+  else if (replyShellID == setid)
+    chprintf((BaseSequentialStream*)sd_, "Switch to the own shell\n");
+  else
+    chprintf((BaseSequentialStream*)sd_, "Switch to the shell on board %d\n", setid);
+
+  if (myID != setid) {
+
+    if (replyShellID != setid)  {
+      // To set the new remote shell via CAN bus.
+      txmsg.SID = ((CAN::SHELL_QUERY_ID(setid)) << CAN::DEVICE_ID_SHIFT) | replyShellID;
+      txmsg.DLC = 0;
+      canTransmit(canDriver, CAN_ANY_MAILBOX, &txmsg, US2ST(545));
+    }
+
+    if (replyShellID == myID) {
+      queryShellID = setid;
+    } else {
+      if (replyShellID == setid)
+        // To return the own shell via CAN bus.
+        txmsg.SID = ((CAN::SHELL_QUERY_ID(replyShellID)) << CAN::DEVICE_ID_SHIFT) | replyShellID;
+      else
+        // To set the serial board to the new remote shell.
+        txmsg.SID = ((CAN::SHELL_QUERY_ID(replyShellID)) << CAN::DEVICE_ID_SHIFT) | setid;
+
+      txmsg.DLC = 0;
+      canTransmit(canDriver, CAN_ANY_MAILBOX, &txmsg, US2ST(545));
+
+      replyShellID = myID;
+    }
+  }
+}
+
+void IwrapCanMux::rcvSwitchCmd(uint8_t setid) {
+
+  if (myID == setid) {
+    chprintf((BaseSequentialStream*)sd_, "\nReturn own shell\n\n");
+    queryShellID = setid;
+  } else if (myID != queryShellID) {
+    chprintf((BaseSequentialStream*)sd_, "\nSwitch to the shell on board %d\n", setid);
+    queryShellID = setid;
+  } else {
+    chprintf((BaseSequentialStream*)sd_, "\nShell shown in board %d\n\n", setid);
+    replyShellID = setid;
+  }
+}
diff --git a/components/serial_reset/serial_can_mux.cpp b/components/serial_reset/serial_can_mux.cpp
new file mode 100644
index 0000000..52425f0
--- /dev/null
+++ b/components/serial_reset/serial_can_mux.cpp
@@ -0,0 +1,228 @@
+#include <ch.h>
+#include <hal.h>
+#include <string.h>  // memcpy
+#include <chprintf.h>
+#include <amiro/Constants.h>
+
+#include <amiro/serial_reset/serial_can_mux.hpp>
+
+using namespace amiro;
+using namespace chibios_rt;
+
+SerialCanMux::SerialCanMux(SerialDriver *_sd_, CANDriver *can, const uint8_t boardId)
+  : BaseSequentialStreamInterface(),
+    sd_(_sd_),
+    canDriver(can)
+{
+//    oqueue(ob, 500, NULL, NULL);
+    myID = boardId;
+    queryShellID = boardId;
+    replyShellID = boardId;
+
+    resetStatus = 0;
+    resetAddBytes = 0;
+
+    txmsg.IDE = CAN_IDE_STD;
+    txmsg.RTR = CAN_RTR_DATA;
+    txmsg.DLC = 0;
+}
+
+SerialCanMux::~SerialCanMux() {
+
+};
+
+/*
+ * resets system for entering in bootloader.
+ */
+void SerialCanMux::restartSystem(void) {
+  //TODO Set reset SYS_PD_N
+  NVIC_SystemReset();
+}
+
+/*
+ * checks bytes for reset command sequence.
+ * if the last byte and the following identification number of the device have been sent,
+ * the microcontroller will be reset
+ */
+bool SerialCanMux::checkByteForBLReset(uint8_t *inputs, size_t size) {
+  /* Run throug all given bytes */
+  for (size_t idx=0; idx < size; idx++) {
+    uint8_t input = *(inputs+idx);
+
+    /*
+     * if the last byte of reset command sequence has not been sent yet,
+     * check new byte and handle reset status
+     */
+    if (resetStatus < INPUT_BL_RESET_LENGTH) {
+      if (input == inputBLReset[resetStatus]) {
+        resetStatus++;
+        if (resetStatus == INPUT_BL_RESET_LENGTH) {
+          chprintf((BaseSequentialStream*)sd_, "\n\nDo reset ...\n\n");
+          uint8_t ledCount;
+          for (ledCount=0; ledCount<20; ledCount++) {
+//            boardWriteLed(1);
+            chThdSleepMilliseconds(100);
+//            boardWriteLed(0);
+            chThdSleepMilliseconds(100);
+          }
+          restartSystem();
+        }
+      } else {
+        resetStatus = 0;
+        resetAddBytes = 0;
+      }
+
+    /*
+     * if the last byte of reset command sequence has been sent,
+     * save new byte as identification number of the device which shall be flashed and
+     * reset microcontroller
+     */
+    } else {
+      inputAddBytes[resetAddBytes] = input;
+      resetAddBytes++;
+      if (resetAddBytes >= INPUT_BL_RESET_ADDITIONAL_BYTES) {
+        chprintf((BaseSequentialStream*)sd_, "\n\nAt device %x: Do reset ...\n\n", input);
+        chThdSleepMilliseconds(OFFSET_TIME_RESET_MS);
+        restartSystem();
+      }
+    }
+  }
+
+  return (resetStatus > 0);
+}
+
+
+msg_t SerialCanMux::sendViaCan(uint32_t id, uint8_t *inputs, size_t size) {
+  msg_t status;
+  txmsg.SID = (id << CAN::DEVICE_ID_SHIFT) | myID;
+  for (size_t i=0; i<size; i+=8) {
+    txmsg.DLC = (size < (i + 8)) ? size-i : 8;
+    memcpy(txmsg.data8, &inputs[i], txmsg.DLC);
+    status = canTransmit(canDriver, CAN_TX_MAILBOXES, &txmsg, TIME_INFINITE);
+    if (status!= RDY_OK)
+      return status;
+  }
+
+  return RDY_OK;
+}
+
+//----------------------------------------------------------------
+
+/*
+ * Interface implementation.
+ */
+
+size_t SerialCanMux::write(const uint8_t *bp, size_t n) {
+  size_t size;
+
+  if (myID != replyShellID) {
+     msg_t status = sendViaCan(CAN::SHELL_REPLY_ID(replyShellID), (uint8_t *)bp, n);
+     BaseThread::sleep(US2ST(500));
+     size = (status == RDY_OK) ? n : 0;
+  } else {
+    size = sdWrite(sd_, bp, n);
+  }
+
+  return size;
+}
+
+size_t SerialCanMux::read(uint8_t *bp, size_t n) {
+  size_t size = sdRead(sd_, inputChar, n);
+
+  checkByteForBLReset(inputChar, size);
+
+  if (myID != queryShellID)
+    sendViaCan(CAN::SHELL_QUERY_ID(queryShellID),inputChar, size);
+  else
+    memcpy(bp, inputChar, size);
+
+  return size;
+}
+
+msg_t SerialCanMux::put(uint8_t b) {
+  msg_t byte;
+
+  if (myID != replyShellID) {
+    byte = sendViaCan(CAN::SHELL_REPLY_ID(replyShellID), &b, 1);
+    BaseThread::sleep(US2ST(200));
+  } else {
+    byte = sdPut(sd_, b);
+  }
+
+  return byte;
+}
+
+msg_t SerialCanMux::get(void) {
+  msg_t byte = sdGet(sd_);
+  uint8_t b = (uint8_t) byte;
+
+  checkByteForBLReset(&b, 1);
+
+  if (myID != queryShellID)
+    sendViaCan(CAN::SHELL_QUERY_ID(queryShellID), &b, 1);
+
+  return byte;
+}
+
+//----------------------------------------------------------------
+
+/*
+ * Converse data from CAN bus into input queue of serial driver.
+ */
+
+void SerialCanMux::convCan2Serial(uint8_t *data_can, size_t n) {
+  chSysLock();
+  for (size_t i=0; i < n; i++)
+     sdIncomingDataI(sd_, data_can[i]);
+  chSysUnlock();
+}
+
+void SerialCanMux::sendSwitchCmd(uint8_t setid) {
+
+  if (myID == setid)
+    chprintf((BaseSequentialStream*)sd_, "Stay at the same shell\n");
+  else if (replyShellID == setid)
+    chprintf((BaseSequentialStream*)sd_, "Switch to the own shell\n");
+  else
+    chprintf((BaseSequentialStream*)sd_, "Switch to the shell on board %d\n", setid);
+
+  if (myID != setid) {
+
+    if (replyShellID != setid)  {
+      // To set the new remote shell via CAN bus.
+      txmsg.SID = ((CAN::SHELL_QUERY_ID(setid)) << CAN::DEVICE_ID_SHIFT) | replyShellID;
+      txmsg.DLC = 0;
+      canTransmit(canDriver, CAN_TX_MAILBOXES, &txmsg, US2ST(545));
+    }
+
+    if (replyShellID == myID) {
+      queryShellID = setid;
+    } else {
+      if (replyShellID == setid)
+        // To return the own shell via CAN bus.
+        txmsg.SID = ((CAN::SHELL_QUERY_ID(replyShellID)) << CAN::DEVICE_ID_SHIFT) | replyShellID;
+      else
+        // To set the serial board to the new remote shell.
+        txmsg.SID = ((CAN::SHELL_QUERY_ID(replyShellID)) << CAN::DEVICE_ID_SHIFT) | setid;
+
+      txmsg.DLC = 0;
+      canTransmit(canDriver, CAN_TX_MAILBOXES, &txmsg, US2ST(545));
+
+      replyShellID = myID;
+    }
+  }
+}
+
+void SerialCanMux::rcvSwitchCmd(uint8_t setid) {
+
+  if (myID == setid) {
+    chprintf((BaseSequentialStream*)sd_, "\nReturn own shell\n\n");
+    queryShellID = setid;
+  } else if (myID != queryShellID) {
+    chprintf((BaseSequentialStream*)sd_, "\nSwitch to the shell on board %d\n", setid);
+    queryShellID = setid;
+  } else {
+    chprintf((BaseSequentialStream*)sd_, "\nShell shown in board %d\n\n", setid);
+    replyShellID = setid;
+  }
+}
diff --git a/devices/DiWheelDrive/DiWheelDrive.cpp b/devices/DiWheelDrive/DiWheelDrive.cpp
new file mode 100644
index 0000000..ff925ea
--- /dev/null
+++ b/devices/DiWheelDrive/DiWheelDrive.cpp
@@ -0,0 +1,252 @@
+#include "ch.hpp"
+#include "hal.h"
+#include "qei.h"
+#include "DiWheelDrive.h"
+
+#include <global.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+using namespace types;
+
+extern volatile uint32_t shutdown_now;
+extern Global global;
+
+DiWheelDrive::DiWheelDrive(CANDriver *can)
+    : ControllerAreaNetworkTx(can, CAN::DI_WHEEL_DRIVE_ID),
+      ControllerAreaNetworkRx(can, CAN::DI_WHEEL_DRIVE_ID),
+      bcCounter(0)
+{
+}
+
+msg_t DiWheelDrive::receiveMessage(CANRxFrame *frame) {
+  int deviceId = this->decodeDeviceId(frame);
+
+  switch (deviceId) {
+
+    case CAN::SHELL_REPLY_ID(CAN::DI_WHEEL_DRIVE_ID):
+      if (frame->DLC > 0) {
+        sdWrite(&SD1, frame->data8, frame->DLC);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::SHELL_QUERY_ID(CAN::DI_WHEEL_DRIVE_ID):
+      if (frame->DLC != 0) {
+        global.sercanmux1.convCan2Serial(frame->data8, frame->DLC);
+        return RDY_OK;
+      } else {
+        global.sercanmux1.rcvSwitchCmd(this->decodeBoardId(frame));
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::TARGET_SPEED_ID:
+      if (frame->DLC == 8) {
+        global.distcontrol.deactivateController();
+        kinematic targetVelocity;
+        targetVelocity.x = frame->data32[0];
+        targetVelocity.w_z = frame->data32[1];
+        global.motorcontrol.setTargetSpeed(targetVelocity);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::TARGET_RPM_ID:
+      if (frame->DLC == 8) {
+        global.distcontrol.deactivateController();
+        global.motorcontrol.setTargetRPM(frame->data32[0], frame->data32[1]);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::SET_ODOMETRY_ID:
+      if (frame->DLC == 8) {
+        int32_t robotPositionX = (frame->data8[0] << 8 | frame->data8[1] << 16 | frame->data8[2] << 24);
+        int32_t robotPositionY = (frame->data8[3] << 8 | frame->data8[4] << 16 | frame->data8[5] << 24);
+        int32_t robotPositionF_Z = (frame->data8[6] << 8 | frame->data8[7] << 16);
+        global.odometry.setPosition(float(robotPositionX)*1e-6,float(robotPositionY)*1e-6,float(robotPositionF_Z)*1e-6);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::BROADCAST_SHUTDOWN:
+      if (frame->DLC == 2 && frame->data16[0] == CAN::SHUTDOWN_MAGIC) {
+        shutdown_now = 0x4;
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::CALIBRATE_PROXIMITY_FLOOR:
+      // Dont care about the payload but start the calibration
+      // TODO Care about the payload. Differ between:
+      // 1: Do fresh calibration (Save values to memory and to temporary values)
+      // 2: Remove temporary Calibration and get uncalibrated values
+      // 3: Load calibration from memory
+      this->calibrate();
+      break;
+
+    case CAN::TARGET_POSITION_ID:
+      if (frame->DLC == 8) {
+        // Robot target position [x] = µm, [f_z] = µrad, [t] = ms
+        int32_t robotPositionX = (frame->data8[0] << 8 | frame->data8[1] << 16 | frame->data8[2] << 24);
+        int32_t robotPositionF_Z = (frame->data8[3] << 8 | frame->data8[4] << 16 | frame->data8[5] << 24);
+        uint16_t targetTimeMilliSeconds = (frame->data8[6] | frame->data8[7] << 8);
+        //chprintf((BaseSequentialStream*) &SD1, "\nx=%d\nf_z=%d\nt=%d", robotPositionX, robotPositionF_Z, targetTimeMilliSeconds);
+        global.distcontrol.setTargetPosition(robotPositionX, robotPositionF_Z, targetTimeMilliSeconds);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::SET_KINEMATIC_CONST_ID:
+      if (frame->DLC == 8) {
+/*        // Set (but do not store) Ed
+        global.motorcontrol.setWheelDiameterCorrectionFactor(static_cast<float>(frame->data32[0]), false);
+        // Set (but do not store) Eb
+        global.motorcontrol.setActualWheelBaseDistance(static_cast<float>(frame->data32[1]), false);
+        return RDY_OK;*/
+        // Set (but do not store) Ed
+        uint32_t ed_int = static_cast<uint32_t>(frame->data32[0]);
+        float ed_float = static_cast<float>(ed_int)/1000000.0;
+        global.motorcontrol.setWheelDiameterCorrectionFactor(ed_float, false);
+        // Set (but do not store) Eb
+        uint32_t eb_int = static_cast<uint32_t>(frame->data32[1]);
+        float eb_float = static_cast<float>(eb_int)/1000000.0;
+        global.motorcontrol.setActualWheelBaseDistance(eb_float, false);
+        //chprintf((BaseSequentialStream*) &SD1, "Edi=%i, Edf=%f, Ebi=%i, Ebf=%f\n", ed_int, ed_float, eb_int, eb_float);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::POWER_STATUS_ID:
+      if (frame->DLC == 6) {
+        // The power status is evaluated by inherited ControllerAreaNetworkRx object, but depending on the flags the power path controller needs to enabled or disabled.
+        types::power_status::ChargingState charging_flags;
+        charging_flags.value = frame->data8[0];
+        global.ltc4412.enable(charging_flags.content.diwheeldrive_enable_power_path);
+        // Do not return with RDY_OK, or the inherited ControllerAreaNetworkRx object would not evaluate the rest of this message.
+      }
+    break;
+
+    default:
+      break;
+  }
+  return -1;
+}
+
+msg_t DiWheelDrive::updateSensorVal() {
+
+  // Update robot velocity values
+  kinematic currentVelocity = global.motorcontrol.getCurrentVelocity();
+  this->actualSpeed[0] = currentVelocity.x;
+  this->actualSpeed[1] = currentVelocity.w_z;
+
+  // Update odometry values
+  this->robotPosition = global.odometry.getPosition();
+
+  // Update proximity values
+  for (int idx = 0; idx < 4; ++idx)
+    this->proximityFloorValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset();
+
+  return 0;
+}
+
+void DiWheelDrive::periodicBroadcast() {
+  CANTxFrame frame;
+  frame.SID = 0;
+
+  // Send the velocites µm/s of the x axis and µrad/s around z axis: start
+  this->encodeDeviceId(&frame, CAN::ACTUAL_SPEED_ID);
+  frame.data32[0] = this->actualSpeed[0];
+  frame.data32[1] = this->actualSpeed[1];
+  frame.DLC = 8;
+  this->transmitMessage(&frame);
+
+  // Send the valocites µm/s of the x axis and µrad/s around z axis: end
+  // Send the odometry: start
+  //       BaseThread::sleep(MS2ST(10));  // Sleep, otherwise the cognition-board wont receive all messages
+  // Set the frame id
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::ODOMETRY_ID);
+  // Cut of the first byte, which precission is not needed
+  int32_t x_mm = (this->robotPosition.x >> 8);
+  int32_t y_mm = (this->robotPosition.y >> 8);
+  int16_t f_z_mrad = int16_t(this->robotPosition.f_z >> 8 );
+  // Copy the data structure
+  memcpy((uint8_t *)&(frame.data8[0]), (uint8_t *)&x_mm, 3);
+  memcpy((uint8_t *)&(frame.data8[3]), (uint8_t *)&y_mm, 3);
+  memcpy((uint8_t *)&(frame.data8[6]), (uint8_t *)&f_z_mrad, 2);
+  frame.DLC = 8;
+  this->transmitMessage(&frame);
+
+  // Send the odometry: end
+  // Send the proximity values of the floor: start
+  //       BaseThread::sleep(MS2ST(10));  // Sleep, otherwise the cognition-board wont receive all messages
+  // Set the frame id
+  frame.SID = 0;
+  this->encodeDeviceId(&frame, CAN::PROXIMITY_FLOOR_ID);
+  frame.data16[0] = this->proximityFloorValue[0];
+  frame.data16[1] = this->proximityFloorValue[1];
+  frame.data16[2] = this->proximityFloorValue[2];
+  frame.data16[3] = this->proximityFloorValue[3];
+  frame.DLC = 8;
+  this->transmitMessage(&frame);
+
+  // Send the board ID (board ID of DiWheelDrive = Robot ID)
+  if (this->bcCounter % 10 == 0) {
+    frame.SID = 0;
+    this->encodeDeviceId(&frame, CAN::ROBOT_ID);
+    frame.data8[0] = this->robotId;
+    frame.DLC = 1;
+    this->transmitMessage(&frame);
+  }
+
+  ++this->bcCounter;
+}
+
+void DiWheelDrive::calibrate() {
+  // Stop sending and receiving of values to indicate the calibration phase
+//   eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
+//   rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
+
+  this->calibrateProximityFloorValues();
+
+  // Start sending and receving of values
+//   eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
+//   rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
+
+}
+
+void DiWheelDrive::calibrateProximityFloorValues() {
+
+  uint16_t buffer;
+  for (uint8_t idx = 0; idx < 4; ++idx) {
+    global.vcnl4020[idx].calibrate();
+    buffer = global.vcnl4020[idx].getProximityOffset();
+    global.memory.setVcnl4020Offset(buffer,idx);
+  }
+
+}
+
+ThreadReference DiWheelDrive::start(tprio_t PRIO) {
+  // set the robot ID as the board ID, which is read from the memory
+  if (global.memory.getBoardId(&this->robotId) != fileSystemIo::FileSystemIoBase::OK) {
+    this->robotId = 0;
+  }
+
+  this->ControllerAreaNetworkRx::start(PRIO + 1);
+  this->ControllerAreaNetworkTx::start(PRIO);
+  return NULL;
+}
+
+msg_t
+DiWheelDrive::terminate(void) {
+  msg_t ret = RDY_OK;
+
+  this->ControllerAreaNetworkTx::requestTerminate();
+  ret |= this->ControllerAreaNetworkTx::wait();
+  this->ControllerAreaNetworkRx::requestTerminate();
+  ret |= this->ControllerAreaNetworkRx::wait();
+
+  return ret;
+}
diff --git a/devices/DiWheelDrive/DiWheelDrive.h b/devices/DiWheelDrive/DiWheelDrive.h
new file mode 100644
index 0000000..0ee13ed
--- /dev/null
+++ b/devices/DiWheelDrive/DiWheelDrive.h
@@ -0,0 +1,51 @@
+#ifndef AMIRO_DI_WHEEL_DRIVE_H_
+#define AMIRO_DI_WHEEL_DRIVE_H_
+
+#include <amiro/ControllerAreaNetworkRx.h>
+#include <amiro/ControllerAreaNetworkTx.h>
+#include <amiro/MotorIncrements.h>
+#include <amiro/MotorControl.h>
+#include <amiro/Odometry.h>
+#include <amiro/proximity/vcnl4020.hpp>
+#include <Types.h> // kinematic
+#include <amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp>
+#include <amiro/power/ltc4412.hpp>
+#include <string.h>  // memcpy
+
+namespace amiro {
+
+  class DiWheelDrive : public ControllerAreaNetworkTx, public ControllerAreaNetworkRx {
+  public:
+    DiWheelDrive(CANDriver *can);//, MotorControl *motorcontrol, Odometry *odometry, VCNL4020 (*vcnl4020)[4], fileSystemIo::FSIODiWheelDrive *memory, BaseLTC4412wEN *ltc4412);
+    virtual void  calibrate();
+
+    /** \brief Handle the termination of ControllerAreaNetworkTx and ControllerAreaNetworkRx threads
+     *         This includes waiting until the threads have terminated, thus this function might block a relatively long time.
+     */
+    msg_t terminate(void);
+
+    /** \brief Handle the start of ControllerAreaNetworkTx and ControllerAreaNetworkRx threads */
+    ThreadReference start(tprio_t PRIO);
+
+  protected:
+    virtual msg_t receiveMessage(CANRxFrame *frame);
+    virtual msg_t updateSensorVal();
+    virtual void periodicBroadcast();
+
+    void  calibrateProximityFloorValues();
+
+  private:
+    uint32_t bcCounter;
+//    MotorControl *motorcontrol;
+//    Odometry *odometry;
+//    VCNL4020 (*vcnl4020)[4];
+//    fileSystemIo::FSIODiWheelDrive *memory;
+//    BaseLTC4412wEN *ltc4412;
+    int actualSpeed[2];
+    types::position robotPosition;
+
+  };
+
+}
+
+#endif /* AMIRO_DI_WHEEL_DRIVE_H_ */
diff --git a/devices/DiWheelDrive/Makefile b/devices/DiWheelDrive/Makefile
new file mode 100644
index 0000000..4b12897
--- /dev/null
+++ b/devices/DiWheelDrive/Makefile
@@ -0,0 +1,259 @@
+##############################################################################
+# Build global options
+# NOTE: Can be overridden externally.
+#
+
+# Compiler options here.
+ifeq ($(USE_OPT),)
+  USE_OPT = -O2 -fomit-frame-pointer -falign-functions=16 -DCHPRINTF_USE_FLOAT=1 -fstack-usage -DSERIAL_BUFFERS_SIZE=64
+endif
+
+# C specific options here (added to USE_OPT).
+ifeq ($(USE_COPT),)
+  USE_COPT =
+endif
+
+# C++ specific options here (added to USE_OPT).
+ifeq ($(USE_CPPOPT),)
+  USE_CPPOPT = -fno-rtti -fno-exceptions -std=c++11 -U__STRICT_ANSI__
+endif
+
+# Enable this if you want the linker to remove unused code and data
+ifeq ($(USE_LINK_GC),)
+  USE_LINK_GC = yes
+endif
+
+# Linker extra options here.
+ifeq ($(USE_LDOPT),)
+  USE_LDOPT =
+endif
+
+# Enable this if you want link time optimizations (LTO)
+ifeq ($(USE_LTO),)
+  USE_LTO = no
+endif
+
+# If enabled, this option allows to compile the application in THUMB mode.
+ifeq ($(USE_THUMB),)
+  USE_THUMB = yes
+endif
+
+# Enable this if you want to see the full log while compiling.
+ifeq ($(USE_VERBOSE_COMPILE),)
+  USE_VERBOSE_COMPILE = no
+endif
+
+#
+# Build global options
+##############################################################################
+
+##############################################################################
+# Architecture or project specific options
+#
+
+# Enable this if you really want to use the STM FWLib.
+ifeq ($(USE_FWLIB),)
+  USE_FWLIB = no
+endif
+
+#
+# Architecture or project specific options
+##############################################################################
+
+##############################################################################
+# Project, sources and paths
+#
+
+# Define project name here
+PROJECT = DiWheelDrive
+
+# Imported source files and paths
+CHIBIOS = ../../../ChibiOS
+AMIRO = ../..
+include $(AMIRO)/boards/DiWheelDrive/board.mk
+include $(CHIBIOS)/os/hal/platforms/STM32F1xx/platform.mk
+include $(CHIBIOS)/os/hal/hal.mk
+include $(CHIBIOS)/os/ports/GCC/ARMCMx/STM32F1xx/port.mk
+include $(CHIBIOS)/os/kernel/kernel.mk
+include $(CHIBIOS)/os/various/cpp_wrappers/kernel.mk
+include $(AMIRO)/hal/platforms/STM32/platform.mk
+include $(AMIRO)/hal/hal.mk
+
+# Define linker script file here
+LDSCRIPT= $(BOARDLD)/STM32F103xE.ld
+
+# C sources that can be compiled in ARM or THUMB mode depending on the global
+# setting.
+CSRC = $(PORTSRC) \
+       $(KERNSRC) \
+       $(HALSRC) \
+       $(PLATFORMSRC) \
+       $(BOARDSRC) \
+       $(CHIBIOS)/os/various/evtimer.c \
+       $(CHIBIOS)/os/various/syscalls.c \
+       $(CHIBIOS)/os/various/chprintf.c \
+       $(CHIBIOS)/os/various/shell.c \
+       $(CHIBIOS)/os/various/memstreams.c \
+       $(AMIRO)/stubs.c \
+       $(AMIRO)/components/Debug.c \
+
+# C++ sources that can be compiled in ARM or THUMB mode depending on the global
+# setting.
+CPPSRC = $(CHCPPSRC) \
+         $(AMIRO)/components/bus/spi/HWSPIDriver.cpp \
+         $(AMIRO)/components/bus/i2c/HWI2CDriver.cpp \
+         $(AMIRO)/components/bus/i2c/I2CMultiplexer.cpp \
+         $(AMIRO)/components/bus/i2c/VI2CDriver.cpp \
+         $(AMIRO)/components/proximity/vcnl4020.cpp \
+         $(AMIRO)/components/magneto/hmc5883l.cpp \
+         $(AMIRO)/components/accel/lis331dlh.cpp \
+         $(AMIRO)/components/gyro/l3g4200d.cpp \
+         $(AMIRO)/components/power/ina219.cpp \
+         $(AMIRO)/components/eeprom/eeprom.cpp \
+         $(AMIRO)/components/eeprom/at24.cpp \
+         $(AMIRO)/components/FileSystemInputOutput/FileSystemInputOutputBase.cpp \
+         $(AMIRO)/components/FileSystemInputOutput/FSIODiWheelDrive.cpp \
+         $(AMIRO)/components/MotorIncrements.cpp \
+         $(AMIRO)/components/MotorControl.cpp \
+         $(AMIRO)/components/DistControl.cpp \
+         $(AMIRO)/components/Odometry.cpp \
+         $(AMIRO)/components/ControllerAreaNetworkRx.cpp \
+         $(AMIRO)/components/ControllerAreaNetworkTx.cpp \
+         $(AMIRO)/components/Color.cpp \
+         $(AMIRO)/components/serial_reset/serial_can_mux.cpp \
+         DiWheelDrive.cpp \
+         userthread.cpp \
+         main.cpp \
+         exti.cpp
+
+# C sources to be compiled in ARM mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+ACSRC =
+
+# C++ sources to be compiled in ARM mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+ACPPSRC =
+
+# C sources to be compiled in THUMB mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+TCSRC =
+
+# C sources to be compiled in THUMB mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+TCPPSRC =
+
+# List ASM source files here
+ASMSRC = $(PORTASM)
+
+INCDIR = $(PORTINC) $(KERNINC) $(TESTINC) \
+         $(HALINC) $(PLATFORMINC) $(BOARDINC) \
+         $(CHCPPINC) \
+         $(CHIBIOS)/os/various \
+         $(AMIRO) \
+         $(AMIRO)/include
+
+#
+# Project, sources and paths
+##############################################################################
+
+##############################################################################
+# Compiler settings
+#
+
+MCU  = cortex-m3
+
+#TRGT = arm-elf-
+TRGT = arm-none-eabi-
+CC   = $(TRGT)gcc
+CPPC = $(TRGT)g++
+# Enable loading with g++ only if you need C++ runtime support.
+# NOTE: You can use C++ even without C++ support if you are careful. C++
+#       runtime support makes code size explode.
+#LD   = $(TRGT)gcc
+LD   = $(TRGT)g++
+CP   = $(TRGT)objcopy
+AS   = $(TRGT)gcc -x assembler-with-cpp
+OD   = $(TRGT)objdump
+SZ   = $(TRGT)size
+HEX  = $(CP) -O ihex
+BIN  = $(CP) -O binary
+SREC = $(CP) -O srec --srec-len=248
+
+# ARM-specific options here
+AOPT =
+
+# THUMB-specific options here
+TOPT = -mthumb -DTHUMB
+
+# Define C warning options here
+CWARN = -Wall -Wextra -Wstrict-prototypes
+
+# Define C++ warning options here
+CPPWARN = -Wall -Wextra
+
+#
+# Compiler settings
+##############################################################################
+
+##############################################################################
+# Start of default section
+#
+
+# List all default C defines here, like -D_DEBUG=1
+DDEFS =
+
+# List all default ASM defines here, like -D_DEBUG=1
+DADEFS =
+
+# List all default directories to look for include files here
+DINCDIR =
+
+# List the default directory to look for the libraries here
+DLIBDIR =
+
+# List all default libraries here
+DLIBS =
+
+#
+# End of default section
+##############################################################################
+
+##############################################################################
+# Start of user section
+#
+
+# List all user C define here, like -D_DEBUG=1
+UDEFS =
+
+# Define ASM defines here
+UADEFS =
+
+# List all user directories here
+UINCDIR =
+
+# List the user directory to look for the libraries here
+ULIBDIR =
+
+# List all user libraries here
+ULIBS =
+
+#
+# End of user defines
+##############################################################################
+
+ifeq ($(USE_FWLIB),yes)
+  include $(CHIBIOS)/ext/stm32lib/stm32lib.mk
+  CSRC += $(STM32SRC)
+  INCDIR += $(STM32INC)
+  USE_OPT += -DUSE_STDPERIPH_DRIVER
+endif
+
+RULESPATH = $(CHIBIOS)/os/ports/GCC/ARMCMx
+include $(RULESPATH)/rules.mk
+
+include $(AMIRO)/ports/rules.mk
+include $(AMIRO)/devices/flash.mk
diff --git a/devices/DiWheelDrive/amiroosconf.h b/devices/DiWheelDrive/amiroosconf.h
new file mode 100644
index 0000000..4bb1b27
--- /dev/null
+++ b/devices/DiWheelDrive/amiroosconf.h
@@ -0,0 +1,19 @@
+#ifndef AMIRO_AMIROOSCONF_H_
+#define AMIRO_AMIROOSCONF_H_
+
+/**
+ * @brief Stack size of the user thread object
+ */
+#if !defined(USER_THREAD_STACK_SIZE) || defined(__DOXYGEN__)
+#define USER_THREAD_STACK_SIZE         1024
+#endif
+
+/**
+ * @brief Flag to enable/disable als amiroDbgXXX macros
+ */
+#if !defined(AMIRO_DBG) || defined(__DOXYGEN__)
+#define AMIRO_DBG                      TRUE
+#endif
+
+#endif // AMIRO_AMIROOSCONF_H_
+
diff --git a/devices/DiWheelDrive/chconf.h b/devices/DiWheelDrive/chconf.h
new file mode 100644
index 0000000..75c019e
--- /dev/null
+++ b/devices/DiWheelDrive/chconf.h
@@ -0,0 +1,507 @@
+#ifndef _CHCONF_H_
+#define _CHCONF_H_
+
+/*===========================================================================*/
+/**
+ * @name Kernel parameters and options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   System tick frequency.
+ * @details Frequency of the system timer that drives the system ticks. This
+ *          setting also defines the system tick time unit.
+ */
+#if !defined(CH_FREQUENCY) || defined(__DOXYGEN__)
+#define CH_FREQUENCY                    1000
+#endif
+
+/**
+ * @brief   Round robin interval.
+ * @details This constant is the number of system ticks allowed for the
+ *          threads before preemption occurs. Setting this value to zero
+ *          disables the preemption for threads with equal priority and the
+ *          round robin becomes cooperative. Note that higher priority
+ *          threads can still preempt, the kernel is always preemptive.
+ *
+ * @note    Disabling the round robin preemption makes the kernel more compact
+ *          and generally faster.
+ */
+#if !defined(CH_TIME_QUANTUM) || defined(__DOXYGEN__)
+#define CH_TIME_QUANTUM                 20
+#endif
+
+/**
+ * @brief   Managed RAM size.
+ * @details Size of the RAM area to be managed by the OS. If set to zero
+ *          then the whole available RAM is used. The core memory is made
+ *          available to the heap allocator and/or can be used directly through
+ *          the simplified core memory allocator.
+ *
+ * @note    In order to let the OS manage the whole RAM the linker script must
+ *          provide the @p __heap_base__ and @p __heap_end__ symbols.
+ * @note    Requires @p CH_USE_MEMCORE.
+ */
+#if !defined(CH_MEMCORE_SIZE) || defined(__DOXYGEN__)
+#define CH_MEMCORE_SIZE                 0
+#endif
+
+/**
+ * @brief   Idle thread automatic spawn suppression.
+ * @details When this option is activated the function @p chSysInit()
+ *          does not spawn the idle thread automatically. The application has
+ *          then the responsibility to do one of the following:
+ *          - Spawn a custom idle thread at priority @p IDLEPRIO.
+ *          - Change the main() thread priority to @p IDLEPRIO then enter
+ *            an endless loop. In this scenario the @p main() thread acts as
+ *            the idle thread.
+ *          .
+ * @note    Unless an idle thread is spawned the @p main() thread must not
+ *          enter a sleep state.
+ */
+#if !defined(CH_NO_IDLE_THREAD) || defined(__DOXYGEN__)
+#define CH_NO_IDLE_THREAD               FALSE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Performance options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   OS optimization.
+ * @details If enabled then time efficient rather than space efficient code
+ *          is used when two possible implementations exist.
+ *
+ * @note    This is not related to the compiler optimization options.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_OPTIMIZE_SPEED) || defined(__DOXYGEN__)
+#define CH_OPTIMIZE_SPEED               TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Subsystem options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Threads registry APIs.
+ * @details If enabled then the registry APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_REGISTRY) || defined(__DOXYGEN__)
+#define CH_USE_REGISTRY                 TRUE
+#endif
+
+/**
+ * @brief   Threads synchronization APIs.
+ * @details If enabled then the @p chThdWait() function is included in
+ *          the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_WAITEXIT) || defined(__DOXYGEN__)
+#define CH_USE_WAITEXIT                 TRUE
+#endif
+
+/**
+ * @brief   Semaphores APIs.
+ * @details If enabled then the Semaphores APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_SEMAPHORES) || defined(__DOXYGEN__)
+#define CH_USE_SEMAPHORES               TRUE
+#endif
+
+/**
+ * @brief   Semaphores queuing mode.
+ * @details If enabled then the threads are enqueued on semaphores by
+ *          priority rather than in FIFO order.
+ *
+ * @note    The default is @p FALSE. Enable this if you have special requirements.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_SEMAPHORES_PRIORITY) || defined(__DOXYGEN__)
+#define CH_USE_SEMAPHORES_PRIORITY      FALSE
+#endif
+
+/**
+ * @brief   Atomic semaphore API.
+ * @details If enabled then the semaphores the @p chSemSignalWait() API
+ *          is included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_SEMSW) || defined(__DOXYGEN__)
+#define CH_USE_SEMSW                    TRUE
+#endif
+
+/**
+ * @brief   Mutexes APIs.
+ * @details If enabled then the mutexes APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MUTEXES) || defined(__DOXYGEN__)
+#define CH_USE_MUTEXES                  TRUE
+#endif
+
+/**
+ * @brief   Conditional Variables APIs.
+ * @details If enabled then the conditional variables APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_MUTEXES.
+ */
+#if !defined(CH_USE_CONDVARS) || defined(__DOXYGEN__)
+#define CH_USE_CONDVARS                 TRUE
+#endif
+
+/**
+ * @brief   Conditional Variables APIs with timeout.
+ * @details If enabled then the conditional variables APIs with timeout
+ *          specification are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_CONDVARS.
+ */
+#if !defined(CH_USE_CONDVARS_TIMEOUT) || defined(__DOXYGEN__)
+#define CH_USE_CONDVARS_TIMEOUT         TRUE
+#endif
+
+/**
+ * @brief   Events Flags APIs.
+ * @details If enabled then the event flags APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_EVENTS) || defined(__DOXYGEN__)
+#define CH_USE_EVENTS                   TRUE
+#endif
+
+/**
+ * @brief   Events Flags APIs with timeout.
+ * @details If enabled then the events APIs with timeout specification
+ *          are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_EVENTS.
+ */
+#if !defined(CH_USE_EVENTS_TIMEOUT) || defined(__DOXYGEN__)
+#define CH_USE_EVENTS_TIMEOUT           TRUE
+#endif
+
+/**
+ * @brief   Synchronous Messages APIs.
+ * @details If enabled then the synchronous messages APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MESSAGES) || defined(__DOXYGEN__)
+#define CH_USE_MESSAGES                 TRUE
+#endif
+
+/**
+ * @brief   Synchronous Messages queuing mode.
+ * @details If enabled then messages are served by priority rather than in
+ *          FIFO order.
+ *
+ * @note    The default is @p FALSE. Enable this if you have special requirements.
+ * @note    Requires @p CH_USE_MESSAGES.
+ */
+#if !defined(CH_USE_MESSAGES_PRIORITY) || defined(__DOXYGEN__)
+#define CH_USE_MESSAGES_PRIORITY        FALSE
+#endif
+
+/**
+ * @brief   Mailboxes APIs.
+ * @details If enabled then the asynchronous messages (mailboxes) APIs are
+ *          included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_MAILBOXES) || defined(__DOXYGEN__)
+#define CH_USE_MAILBOXES                TRUE
+#endif
+
+/**
+ * @brief   I/O Queues APIs.
+ * @details If enabled then the I/O queues APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_QUEUES) || defined(__DOXYGEN__)
+#define CH_USE_QUEUES                   TRUE
+#endif
+
+/**
+ * @brief   Core Memory Manager APIs.
+ * @details If enabled then the core memory manager APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MEMCORE) || defined(__DOXYGEN__)
+#define CH_USE_MEMCORE                  TRUE
+#endif
+
+/**
+ * @brief   Heap Allocator APIs.
+ * @details If enabled then the memory heap allocator APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_MEMCORE and either @p CH_USE_MUTEXES or
+ *          @p CH_USE_SEMAPHORES.
+ * @note    Mutexes are recommended.
+ */
+#if !defined(CH_USE_HEAP) || defined(__DOXYGEN__)
+#define CH_USE_HEAP                     TRUE
+#endif
+
+/**
+ * @brief   C-runtime allocator.
+ * @details If enabled the the heap allocator APIs just wrap the C-runtime
+ *          @p malloc() and @p free() functions.
+ *
+ * @note    The default is @p FALSE.
+ * @note    Requires @p CH_USE_HEAP.
+ * @note    The C-runtime may or may not require @p CH_USE_MEMCORE, see the
+ *          appropriate documentation.
+ */
+#if !defined(CH_USE_MALLOC_HEAP) || defined(__DOXYGEN__)
+#define CH_USE_MALLOC_HEAP              FALSE
+#endif
+
+/**
+ * @brief   Memory Pools Allocator APIs.
+ * @details If enabled then the memory pools allocator APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MEMPOOLS) || defined(__DOXYGEN__)
+#define CH_USE_MEMPOOLS                 TRUE
+#endif
+
+/**
+ * @brief   Dynamic Threads APIs.
+ * @details If enabled then the dynamic threads creation APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_WAITEXIT.
+ * @note    Requires @p CH_USE_HEAP and/or @p CH_USE_MEMPOOLS.
+ */
+#if !defined(CH_USE_DYNAMIC) || defined(__DOXYGEN__)
+#define CH_USE_DYNAMIC                  TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Debug options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Debug option, system state check.
+ * @details If enabled the correct call protocol for system APIs is checked
+ *          at runtime.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_SYSTEM_STATE_CHECK) || defined(__DOXYGEN__)
+#define CH_DBG_SYSTEM_STATE_CHECK       TRUE
+#endif
+
+/**
+ * @brief   Debug option, parameters checks.
+ * @details If enabled then the checks on the API functions input
+ *          parameters are activated.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_CHECKS) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_CHECKS            TRUE
+#endif
+
+/**
+ * @brief   Debug option, consistency checks.
+ * @details If enabled then all the assertions in the kernel code are
+ *          activated. This includes consistency checks inside the kernel,
+ *          runtime anomalies and port-defined checks.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_ASSERTS) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_ASSERTS           TRUE
+#endif
+
+/**
+ * @brief   Debug option, trace buffer.
+ * @details If enabled then the context switch circular trace buffer is
+ *          activated.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_TRACE) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_TRACE             TRUE
+#endif
+
+/**
+ * @brief   Debug option, stack checks.
+ * @details If enabled then a runtime stack check is performed.
+ *
+ * @note    The default is @p FALSE.
+ * @note    The stack check is performed in a architecture/port dependent way.
+ *          It may not be implemented or some ports.
+ * @note    The default failure mode is to halt the system with the global
+ *          @p panic_msg variable set to @p NULL.
+ */
+#if !defined(CH_DBG_ENABLE_STACK_CHECK) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_STACK_CHECK       TRUE
+#endif
+
+/**
+ * @brief   Debug option, stacks initialization.
+ * @details If enabled then the threads working area is filled with a byte
+ *          value when a thread is created. This can be useful for the
+ *          runtime measurement of the used stack.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_FILL_THREADS) || defined(__DOXYGEN__)
+#define CH_DBG_FILL_THREADS             FALSE
+#endif
+
+/**
+ * @brief   Debug option, threads profiling.
+ * @details If enabled then a field is added to the @p Thread structure that
+ *          counts the system ticks occurred while executing the thread.
+ *
+ * @note    The default is @p TRUE.
+ * @note    This debug option is defaulted to TRUE because it is required by
+ *          some test cases into the test suite.
+ */
+#if !defined(CH_DBG_THREADS_PROFILING) || defined(__DOXYGEN__)
+#define CH_DBG_THREADS_PROFILING        TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Kernel hooks
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Threads descriptor structure extension.
+ * @details User fields added to the end of the @p Thread structure.
+ */
+#if !defined(THREAD_EXT_FIELDS) || defined(__DOXYGEN__)
+#define THREAD_EXT_FIELDS                                                   \
+  /* Add threads custom fields here.*/
+#endif
+
+/**
+ * @brief   Threads initialization hook.
+ * @details User initialization code added to the @p chThdInit() API.
+ *
+ * @note    It is invoked from within @p chThdInit() and implicitly from all
+ *          the threads creation APIs.
+ */
+#if !defined(THREAD_EXT_INIT_HOOK) || defined(__DOXYGEN__)
+#define THREAD_EXT_INIT_HOOK(tp) {                                          \
+  /* Add threads initialization code here.*/                                \
+}
+#endif
+
+/**
+ * @brief   Threads finalization hook.
+ * @details User finalization code added to the @p chThdExit() API.
+ *
+ * @note    It is inserted into lock zone.
+ * @note    It is also invoked when the threads simply return in order to
+ *          terminate.
+ */
+#if !defined(THREAD_EXT_EXIT_HOOK) || defined(__DOXYGEN__)
+#define THREAD_EXT_EXIT_HOOK(tp) {                                          \
+  /* Add threads finalization code here.*/                                  \
+}
+#endif
+
+/**
+ * @brief   Context switch hook.
+ * @details This hook is invoked just before switching between threads.
+ */
+#if !defined(THREAD_CONTEXT_SWITCH_HOOK) || defined(__DOXYGEN__)
+#define THREAD_CONTEXT_SWITCH_HOOK(ntp, otp) {                              \
+  /* Context switch code here.*/                                            \
+}
+#endif
+
+/**
+ * @brief   Idle Loop hook.
+ * @details This hook is continuously invoked by the idle thread loop.
+ */
+#if !defined(IDLE_LOOP_HOOK) || defined(__DOXYGEN__)
+#define IDLE_LOOP_HOOK() {                                                  \
+  /* Idle loop code here.*/                                                 \
+}
+#endif
+
+/**
+ * @brief   System tick event hook.
+ * @details This hook is invoked in the system tick handler immediately
+ *          after processing the virtual timers queue.
+ */
+#if !defined(SYSTEM_TICK_EVENT_HOOK) || defined(__DOXYGEN__)
+#define SYSTEM_TICK_EVENT_HOOK() {                                          \
+  /* System tick event code here.*/                                         \
+}
+#endif
+
+/**
+ * @brief   System halt hook.
+ * @details This hook is invoked in case to a system halting error before
+ *          the system is halted.
+ */
+#if !defined(SYSTEM_HALT_HOOK) || defined(__DOXYGEN__)
+void haltErrorCode(void);
+#define SYSTEM_HALT_HOOK() {                                                \
+  /* System halt code here.*/                                               \
+  haltErrorCode();                                                          \
+}
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/* Port-specific settings (override port settings defaulted in chcore.h).    */
+/*===========================================================================*/
+
+/* NVIC VTOR initialization (only offset!) */
+#define CORTEX_VTOR_INIT 0x00006000
+
+#endif  /* _CHCONF_H_ */
diff --git a/devices/DiWheelDrive/exti.cpp b/devices/DiWheelDrive/exti.cpp
new file mode 100644
index 0000000..2a95029
--- /dev/null
+++ b/devices/DiWheelDrive/exti.cpp
@@ -0,0 +1,119 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <board.h>
+
+#include "exti.hpp"
+
+volatile uint32_t shutdown_now = 0x00000000u; // = BL_SHUTDOWN_NONE in main.cpp
+
+EXTConfig extcfg = {
+
+  {
+    /* channel 0 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 1 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 2 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 3 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 4 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 5 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 6 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 7 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 8 */
+    {
+      /* mode */ EXT_MODE_GPIOC | EXT_CH_MODE_AUTOSTART | EXT_CH_MODE_FALLING_EDGE,
+      /* cb   */ power_down_cb,
+    },
+    /* channel 9 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 10 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 11 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 12 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 13 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 14 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 15 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 16 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 17 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 18 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+  }
+};
+
+void power_down_cb(EXTDriver *extp, expchannel_t channel) {
+
+  (void) extp;
+  (void) channel;
+  // make sure we don't accidentally power down when somebody tries to wake us
+  if (!palReadPad(GPIOC, GPIOC_SYS_PD_N))
+    palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW); // indicate that the module needs some time to shut down
+    shutdown_now = 5; // = SHUTDOWN_HANDLE_REQUEST in main.cpp
+}
diff --git a/devices/DiWheelDrive/exti.hpp b/devices/DiWheelDrive/exti.hpp
new file mode 100644
index 0000000..97b8683
--- /dev/null
+++ b/devices/DiWheelDrive/exti.hpp
@@ -0,0 +1,10 @@
+#ifndef EXT_HPP_
+#define EXT_HPP_
+
+extern volatile uint32_t shutdown_now;
+
+extern EXTConfig extcfg;
+
+void power_down_cb(EXTDriver *extp, expchannel_t channel);
+
+#endif /* EXT_HPP_ */
diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
new file mode 100644
index 0000000..3165f18
--- /dev/null
+++ b/devices/DiWheelDrive/global.hpp
@@ -0,0 +1,207 @@
+#ifndef AMIRO_GLOBAL_HPP_
+#define AMIRO_GLOBAL_HPP_
+
+#include <hal.h>
+#include <qei.h>
+
+#include <stdlib.h>
+#include <array>
+
+#include <board.h>
+#include <amiro/proximity/vcnl4020.hpp>
+#include <amiro/bus/i2c/HWI2CDriver.hpp>
+#include <amiro/bus/i2c/mux/pca9544.hpp>
+#include <amiro/bus/i2c/VI2CDriver.hpp>
+#include <amiro/magneto/hmc5883l.hpp>
+#include <amiro/bus/spi/HWSPIDriver.hpp>
+#include <amiro/accel/lis331dlh.hpp>
+#include <amiro/gyro/l3g4200d.hpp>
+#include <amiro/power/ina219.hpp>
+#include <amiro/power/ltc4412.hpp>
+#include <amiro/eeprom/at24.hpp>
+#include <amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp>
+#include <amiro/serial_reset/serial_can_mux.hpp>
+#include <amiro/MotorIncrements.h>
+#include <amiro/MotorControl.h>
+#include <amiro/DistControl.h>
+#include <amiro/Odometry.h>
+#include <DiWheelDrive.h>
+#include <userthread.hpp>
+
+using namespace amiro;
+
+class Global final
+{
+public:
+  SerialConfig sd1_config{
+    /* speed        */ 115200,
+    /* CR1 register */ 0,
+    /* CR2 register */ 0,
+    /* CR3 register */ 0
+  };
+
+  I2CConfig i2c1_config{
+    /* I2C mode                 */ OPMODE_I2C,
+    /* frequency                */ 400000,
+    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2
+  };
+  I2CConfig i2c2_config{
+    /* I2C mode                 */ OPMODE_I2C,
+    /* frequency                */ 400000,
+    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2
+  };
+
+  VCNL4020::VCNL4020Config vcnl4020_config{
+    /* command             */ VCNL4020::ALS_EN | VCNL4020::PROX_EN | VCNL4020::SELFTIMED_EN,
+    /* ambient parameter   */ VCNL4020::AMBIENT_RATE_2 | VCNL4020::AMBIENT_AUTO_OFFSET | VCNL4020::AMBIENT_AVG_32,
+    /* IR LED current [mA] */ 200u,
+    /* proximity rate      */ VCNL4020::PROX_RATE_7_8125
+  };
+
+  /**
+   * @brief I2C Bus 1
+   * Conected devices:
+   *   HMC5883L
+   */
+  HWI2CDriver HW_I2C1;
+  /**
+   * @brief I2C Bus 2
+   * Connected devices:
+   *   PCA9544
+   *   VCNL4020
+   *   INA219
+   *   AT24Cxx
+   */
+  HWI2CDriver HW_I2C2;
+
+  PCA9544<true> HW_PCA9544;
+
+  std::array<VI2CDriver, 4> V_I2C2;
+
+  std::array<VCNL4020, 4> vcnl4020;
+
+  HMC5883L::HMC5883LConfig hmc5883l_config{
+    /* ctrlA */ HMC5883L::DO_20_HZ | HMC5883L::MS_NORMAL | HMC5883L::MA_AVG8,
+    /* ctrlB */ HMC5883L::GN_5_GA,
+    /* mode  */ HMC5883L::MD_CONTCV | HMC5883L::HS_DISABLE
+  };
+  HMC5883L hmc5883l;
+
+  SPIConfig accel_spi1_config{
+    /* callback function pointer   */ NULL,
+    /* chip select line port       */ GPIOC,
+    /* chip select line pad number */ GPIOC_ACCEL_SS_N,
+    /* initialzation data          */ SPI_CR1_BR_0
+  };
+
+  SPIConfig gyro_spi1_config{
+    /* callback function pointer   */ NULL,
+    /* chip select line port       */ GPIOC,
+    /* chip select line pad number */ GPIOC_GYRO_SS_N,
+    /* initialzation data          */ SPI_CR1_BR_0
+  };
+
+  HWSPIDriver HW_SPI1_ACCEL;
+  HWSPIDriver HW_SPI1_GYRO;
+
+  LIS331DLH::LIS331DLHConfig accel_run_config{
+    /* ctrl1                     */ LIS331DLH::PM_ODR | LIS331DLH::DR_50HZ_37LP | LIS331DLH::ZEN | LIS331DLH::YEN | LIS331DLH::XEN,
+    /* ctrl2                     */ 0x00u,
+    /* ctrl3                     */ LIS331DLH::INT_LOW | LIS331DLH::I1_CFG_DRY,
+    /* ctrl4                     */ LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE | LIS331DLH::FS_4G | LIS331DLH::SIM_4WI,
+    /* ctrl5                     */ LIS331DLH::SLEEP_TO_WAKE_OFF,
+    /* interrupt 1 configuration */ {/* configuration */ LIS331DLH::AOI_OR_INT,
+                                     /* ths           */ 0x00u,
+                                     /* duration      */ 0x00u},
+    /* interrupt 2 configuration */ {/* configuration */ LIS331DLH::AOI_OR_INT,
+                                     /* ths           */ 0x00u,
+                                     /* duration      */ 0x00u}
+  };
+  LIS331DLH::LIS331DLHConfig accel_sleep_config{
+    /* ctrl1                     */ LIS331DLH::PM_0_5_HZ | LIS331DLH::DR_50HZ_37LP | LIS331DLH::XEN,
+    /* ctrl2                     */ 0x00u,
+    /* ctrl3                     */ LIS331DLH::INT_LOW | LIS331DLH::I1_CFG_I1,
+    /* ctrl4                     */ LIS331DLH::BDU_CONT | LIS331DLH::BLE_LE | LIS331DLH::FS_4G | LIS331DLH::SIM_4WI,
+    /* ctrl5                     */ LIS331DLH::SLEEP_TO_WAKE_ON,
+    /* interrupt 1 configuration */ {/* configuration */ LIS331DLH::AOI_OR_INT | LIS331DLH::XHIE,
+                                     /* ths           */ 0x10u,
+                                     /* duration      */ 0x02u},
+    /* interrupt 2 configuration */ {/* configuration */ LIS331DLH::AOI_OR_INT,
+                                     /* ths           */ 0x00u,
+                                     /* duration      */ 0x00u}
+  };
+  LIS331DLH lis331dlh;
+
+  L3G4200D::L3G4200DConfig gyro_run_config{
+    /* ctrl reg 1 */ L3G4200D::DR_100_HZ | L3G4200D::BW_25 | L3G4200D::PD | L3G4200D::ZEN | L3G4200D::YEN | L3G4200D::XEN,
+    /* ctrl reg 2 */ 0,
+    /* ctrl reg 3 */ L3G4200D::I2_DRDY,
+    /* ctrl reg 4 */ L3G4200D::FS_500_DPS | L3G4200D::SIM_4W,
+    /* ctrl reg 5 */ 0
+  };
+  L3G4200D::L3G4200DConfig gyro_sleep_config{
+    /* ctrl reg 1 */ L3G4200D::DR_100_HZ | L3G4200D::BW_25 | L3G4200D::PD,
+    /* ctrl reg 2 */ gyro_run_config.ctrl2,
+    /* ctrl reg 3 */ gyro_run_config.ctrl3,
+    /* ctrl reg 4 */ gyro_run_config.ctrl4,
+    /* ctrl reg 5 */ gyro_run_config.ctrl5
+  };
+  L3G4200D l3g4200d;
+
+  INA219::Driver ina219;
+
+  LTC4412wEN<(uintptr_t)GPIOC, GPIOC_PATH_DCSTAT, (uintptr_t)GPIOC, GPIOC_PATH_DCEN> ltc4412;
+
+  AT24 at24c01;
+  fileSystemIo::FSIODiWheelDrive memory;
+
+  MotorIncrements increments;
+  MotorControl motorcontrol;
+  DistControl distcontrol;
+  Odometry odometry;
+  SerialCanMux sercanmux1;
+
+  DiWheelDrive robot;
+
+  UserThread userThread;
+
+public:
+  Global() :
+    HW_I2C1(&I2CD1), HW_I2C2(&I2CD2),
+    HW_PCA9544(&HW_I2C2, 0x07u),
+    V_I2C2{{VI2CDriver(&HW_PCA9544, 0),
+            VI2CDriver(&HW_PCA9544, 1),
+            VI2CDriver(&HW_PCA9544, 2),
+            VI2CDriver(&HW_PCA9544, 3)}
+          },
+    vcnl4020{{/* front left  */ VCNL4020(&V_I2C2[0], &vcnl4020_config),
+              /* left wheel  */ VCNL4020(&V_I2C2[1], &vcnl4020_config),
+              /* right wheel */ VCNL4020(&V_I2C2[2], &vcnl4020_config),
+              /* front right */ VCNL4020(&V_I2C2[3], &vcnl4020_config)}
+            },
+    hmc5883l(&HW_I2C1, &hmc5883l_config),
+    HW_SPI1_ACCEL(&SPID1, &accel_spi1_config), HW_SPI1_GYRO(&SPID1, &gyro_spi1_config),
+    lis331dlh(&HW_SPI1_ACCEL),
+    l3g4200d(&HW_SPI1_GYRO),
+    ina219(HW_I2C2, 0x40u),
+    ltc4412(),
+    at24c01(0x400u / 0x08u, 0x08u, 500u, &HW_I2C2),
+    memory(at24c01, /*BMSV*/ 1, /*bmsv*/ 3, /*HMV*/ 1, /*hmv*/ 0), // bmsv changed von 2 auf 3
+    increments(&QEID3, &QEID4),
+    motorcontrol(&PWMD2, &increments, GPIOB, GPIOB_POWER_EN, &memory),
+    distcontrol(&motorcontrol, &increments),
+    odometry(&increments),
+    sercanmux1(&SD1, &CAND1, CAN::DI_WHEEL_DRIVE_ID),
+    robot(&CAND1),
+    userThread()
+  {
+    return;
+  }
+
+  ~Global()
+  {
+    return;
+  }
+};
+
+#endif /* AMIRO_GLOBAL_HPP_ */
diff --git a/devices/DiWheelDrive/halconf.h b/devices/DiWheelDrive/halconf.h
new file mode 100644
index 0000000..a210f0d
--- /dev/null
+++ b/devices/DiWheelDrive/halconf.h
@@ -0,0 +1,290 @@
+#ifndef _HALCONF_H_
+#define _HALCONF_H_
+
+#include "mcuconf.h"
+
+/**
+ * @brief   Enables the TM subsystem.
+ */
+#if !defined(HAL_USE_TM) || defined(__DOXYGEN__)
+#define HAL_USE_TM                  TRUE
+#endif
+
+/**
+ * @brief   Enables the PAL subsystem.
+ */
+#if !defined(HAL_USE_PAL) || defined(__DOXYGEN__)
+#define HAL_USE_PAL                 TRUE
+#endif
+
+/**
+ * @brief   Enables the ADC subsystem.
+ */
+#if !defined(HAL_USE_ADC) || defined(__DOXYGEN__)
+#define HAL_USE_ADC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the CAN subsystem.
+ */
+#if !defined(HAL_USE_CAN) || defined(__DOXYGEN__)
+#define HAL_USE_CAN                 TRUE
+#endif
+
+/**
+ * @brief   Enables the EXT subsystem.
+ */
+#if !defined(HAL_USE_EXT) || defined(__DOXYGEN__)
+#define HAL_USE_EXT                 TRUE
+#endif
+
+/**
+ * @brief   Enables the GPT subsystem.
+ */
+#if !defined(HAL_USE_GPT) || defined(__DOXYGEN__)
+#define HAL_USE_GPT                 FALSE
+#endif
+
+/**
+ * @brief   Enables the I2C subsystem.
+ */
+#if !defined(HAL_USE_I2C) || defined(__DOXYGEN__)
+#define HAL_USE_I2C                 TRUE
+#endif
+
+/**
+ * @brief   Enables the ICU subsystem.
+ */
+#if !defined(HAL_USE_ICU) || defined(__DOXYGEN__)
+#define HAL_USE_ICU                 FALSE
+#endif
+
+/**
+ * @brief   Enables the MAC subsystem.
+ */
+#if !defined(HAL_USE_MAC) || defined(__DOXYGEN__)
+#define HAL_USE_MAC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the MMC_SPI subsystem.
+ */
+#if !defined(HAL_USE_MMC_SPI) || defined(__DOXYGEN__)
+#define HAL_USE_MMC_SPI             FALSE
+#endif
+
+/**
+ * @brief   Enables the PWM subsystem.
+ */
+#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__)
+#define HAL_USE_PWM                 TRUE
+#endif
+
+/**
+ * @brief   Enables the RTC subsystem.
+ */
+#if !defined(HAL_USE_RTC) || defined(__DOXYGEN__)
+#define HAL_USE_RTC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the SDC subsystem.
+ */
+#if !defined(HAL_USE_SDC) || defined(__DOXYGEN__)
+#define HAL_USE_SDC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the SERIAL subsystem.
+ */
+#if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__)
+#define HAL_USE_SERIAL              TRUE
+#endif
+
+/**
+ * @brief   Enables the SERIAL over USB subsystem.
+ */
+#if !defined(HAL_USE_SERIAL_USB) || defined(__DOXYGEN__)
+#define HAL_USE_SERIAL_USB          FALSE
+#endif
+
+/**
+ * @brief   Enables the SPI subsystem.
+ */
+#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__)
+#define HAL_USE_SPI                 TRUE
+#endif
+
+/**
+ * @brief   Enables the UART subsystem.
+ */
+#if !defined(HAL_USE_UART) || defined(__DOXYGEN__)
+#define HAL_USE_UART                FALSE
+#endif
+
+/**
+ * @brief   Enables the USB subsystem.
+ */
+#if !defined(HAL_USE_USB) || defined(__DOXYGEN__)
+#define HAL_USE_USB                 FALSE
+#endif
+
+/**
+ * @brief   Enables the QEI subsystem.
+ */
+#if !defined(HAL_USE_QEI) || defined(__DOXYGEN__)
+#define HAL_USE_QEI                 TRUE
+#endif
+
+/*===========================================================================*/
+/* ADC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables synchronous APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(ADC_USE_WAIT) || defined(__DOXYGEN__)
+#define ADC_USE_WAIT                TRUE
+#endif
+
+/**
+ * @brief   Enables the @p adcAcquireBus() and @p adcReleaseBus() APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(ADC_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define ADC_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+/*===========================================================================*/
+/* CAN driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Sleep mode related APIs inclusion switch.
+ */
+#if !defined(CAN_USE_SLEEP_MODE) || defined(__DOXYGEN__)
+#define CAN_USE_SLEEP_MODE          TRUE
+#endif
+
+/*===========================================================================*/
+/* I2C driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables the mutual exclusion APIs on the I2C bus.
+ */
+#if !defined(I2C_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define I2C_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+/*===========================================================================*/
+/* MAC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables an event sources for incoming packets.
+ */
+#if !defined(MAC_USE_ZERO_COPY) || defined(__DOXYGEN__)
+#define MAC_USE_ZERO_COPY           FALSE
+#endif
+
+/**
+ * @brief   Enables an event sources for incoming packets.
+ */
+#if !defined(MAC_USE_EVENTS) || defined(__DOXYGEN__)
+#define MAC_USE_EVENTS              TRUE
+#endif
+
+/*===========================================================================*/
+/* MMC_SPI driver related settings.                                          */
+/*===========================================================================*/
+
+/**
+ * @brief   Delays insertions.
+ * @details If enabled this options inserts delays into the MMC waiting
+ *          routines releasing some extra CPU time for the threads with
+ *          lower priority, this may slow down the driver a bit however.
+ *          This option is recommended also if the SPI driver does not
+ *          use a DMA channel and heavily loads the CPU.
+ */
+#if !defined(MMC_NICE_WAITING) || defined(__DOXYGEN__)
+#define MMC_NICE_WAITING            TRUE
+#endif
+
+/*===========================================================================*/
+/* SDC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Number of initialization attempts before rejecting the card.
+ * @note    Attempts are performed at 10mS intervals.
+ */
+#if !defined(SDC_INIT_RETRY) || defined(__DOXYGEN__)
+#define SDC_INIT_RETRY              100
+#endif
+
+/**
+ * @brief   Include support for MMC cards.
+ * @note    MMC support is not yet implemented so this option must be kept
+ *          at @p FALSE.
+ */
+#if !defined(SDC_MMC_SUPPORT) || defined(__DOXYGEN__)
+#define SDC_MMC_SUPPORT             FALSE
+#endif
+
+/**
+ * @brief   Delays insertions.
+ * @details If enabled this options inserts delays into the MMC waiting
+ *          routines releasing some extra CPU time for the threads with
+ *          lower priority, this may slow down the driver a bit however.
+ */
+#if !defined(SDC_NICE_WAITING) || defined(__DOXYGEN__)
+#define SDC_NICE_WAITING            TRUE
+#endif
+
+/*===========================================================================*/
+/* SERIAL driver related settings.                                           */
+/*===========================================================================*/
+
+/**
+ * @brief   Default bit rate.
+ * @details Configuration parameter, this is the baud rate selected for the
+ *          default configuration.
+ */
+#if !defined(SERIAL_DEFAULT_BITRATE) || defined(__DOXYGEN__)
+#define SERIAL_DEFAULT_BITRATE      38400
+#endif
+
+/**
+ * @brief   Serial buffers size.
+ * @details Configuration parameter, you can change the depth of the queue
+ *          buffers depending on the requirements of your application.
+ * @note    The default is 64 bytes for both the transmission and receive
+ *          buffers.
+ */
+#if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__)
+#define SERIAL_BUFFERS_SIZE         16
+#endif
+
+/*===========================================================================*/
+/* SPI driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables synchronous APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(SPI_USE_WAIT) || defined(__DOXYGEN__)
+#define SPI_USE_WAIT                TRUE
+#endif
+
+/**
+ * @brief   Enables the @p spiAcquireBus() and @p spiReleaseBus() APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(SPI_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define SPI_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+#endif /* _HALCONF_H_ */
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
new file mode 100644
index 0000000..ea94967
--- /dev/null
+++ b/devices/DiWheelDrive/main.cpp
@@ -0,0 +1,961 @@
+#define BL_CALLBACK_TABLE_ADDR  (0x08000000 + 0x01C0)
+#define BL_MAGIC_NUMBER         ((uint32_t)0xFF669900u)
+
+#define SHUTDOWN_NONE             0
+#define SHUTDOWN_TRANSPORTATION   1
+#define SHUTDOWN_DEEPSLEEP        2
+#define SHUTDOWN_HIBERNATE        3
+#define SHUTDOWN_RESTART          4
+#define SHUTDOWN_HANDLE_REQUEST   5
+
+#include <ch.hpp>
+
+#include <amiro/util/util.h>
+#include <global.hpp>
+#include <exti.hpp>
+
+#include <chprintf.h>
+#include <shell.h>
+
+using namespace chibios_rt;
+
+Global global;
+
+void systemShutdown() {
+  types::kinematic k;
+  uint8_t i;
+
+//  // make sure we assert SYS_PD_N to delay shutdown until we're done.
+//  boardRequestShutdown();
+
+    // stop the user thread
+  global.userThread.requestTerminate();
+  global.userThread.wait();
+
+  k.x = 0x00u;
+  k.w_z = 0x00u;
+
+  // stop wheels
+  global.robot.setTargetSpeed(k);
+  global.robot.terminate();
+
+  for (i = 0x00; i < global.vcnl4020.size(); i++) {
+    global.vcnl4020[i].requestTerminate();
+    global.vcnl4020[i].wait();
+  }
+
+  global.ina219.requestTerminate();
+  global.ina219.wait();
+  global.hmc5883l.requestTerminate();
+  global.hmc5883l.wait();
+  global.l3g4200d.requestTerminate();
+  global.l3g4200d.wait();
+
+  global.motorcontrol.requestTerminate();
+  global.motorcontrol.wait();
+  global.odometry.requestTerminate();
+  global.odometry.wait();
+
+  // stop I²C
+  for (i = 0; i < global.V_I2C2.size(); ++i)
+    global.V_I2C2[i].stop();
+  global.HW_I2C2.stop();
+
+  global.lis331dlh.requestTerminate();
+  global.lis331dlh.wait();
+
+  global.lis331dlh.configure(&global.accel_sleep_config);
+//  global.lis331dlh.start(NORMALPRIO +4);
+
+//  boardWriteIoPower(0);
+//  boardStandby();
+
+  return;
+}
+
+
+//void (*shellcmd_t)(BaseSequentialStream *chp, int argc, char *argv[]);
+
+void shellRequestShutdown(BaseSequentialStream *chp, int argc, char *argv[]) {
+
+  chprintf(chp, "shellRequestShutdown\n");
+
+  /* if nor argument was given, print some help text */
+  if (argc == 0 || strcmp(argv[0], "help") == 0) {
+    chprintf(chp, "\tUSAGE:\n");
+    chprintf(chp, "> shutdown <type>\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\ttype\n");
+    chprintf(chp, "The type of shutdown to perform.\n");
+    chprintf(chp, "Choose one of the following types:\n");
+    chprintf(chp, "  transportation - Ultra low-power mode with all wakeups disabled.\n");
+    chprintf(chp, "                   The robot can not be charged.\n");
+    chprintf(chp, "  deepsleep      - Ultra low-power mode with several wakeups enabled.\n");
+    chprintf(chp, "                   The robot can only be charged via the power plug.\n");
+    chprintf(chp, "  hibernate      - Medium low-power mode, but with full charging capabilities.\n");
+    chprintf(chp, "  restart        - Performs a system restart.\n");
+    chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
+    chprintf(chp, "\n");
+    return;
+  }
+
+  if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) {
+    shutdown_now = SHUTDOWN_TRANSPORTATION;
+    chprintf(chp, "shutdown to transportation mode initialized\n");
+  } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) {
+    shutdown_now = SHUTDOWN_DEEPSLEEP;
+    chprintf(chp, "shutdown to deepsleep mode initialized\n");
+  } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) {
+    shutdown_now = SHUTDOWN_HIBERNATE;
+    chprintf(chp, "shutdown to hibernate mode initialized\n");
+  } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) {
+    chprintf(chp, "restart initialized\n");
+    shutdown_now = SHUTDOWN_RESTART;
+  } else {
+    chprintf(chp, "ERROR: unknown argument!\n");
+    shutdown_now = SHUTDOWN_NONE;
+  }
+
+  return;
+}
+
+void shellRequestWakeup(BaseSequentialStream *chp, int argc, char *argv[]) {
+  int i;
+  chprintf(chp, "shellRequestWakeup\n");
+
+  for (i = 0x00u; i < argc; i++)
+    chprintf(chp, "%s\n", argv[i]);
+
+  boardWakeup();
+}
+
+void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) {
+  enum Type {HEX, U8, U16, U32, S8, S16, S32};
+
+  chprintf(chp, "shellRequestReadData\n");
+
+  if (argc < 2 || strcmp(argv[0],"help") == 0)
+  {
+    chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]");
+    chprintf(chp, "\n");
+    chprintf(chp, "\ttype\n");
+    chprintf(chp, "The data type as which to interpret the data.\n");
+    chprintf(chp, "Choose one of the following types:\n");
+    chprintf(chp, "  hex - one byte as hexadecimal value\n");
+    chprintf(chp, "  u8  - unsigned integer (8 bit)\n");
+    chprintf(chp, "  u16 - unsigned integer (16 bit)\n");
+    chprintf(chp, "  u32 - unsigned integer (32 bit)\n");
+    chprintf(chp, "  s8  - signed integer (8 bit)\n");
+    chprintf(chp, "  s16 - signed integer (16 bit)\n");
+    chprintf(chp, "  s32 - signed integer (32 bit)\n");
+    chprintf(chp, "\tstart\n");
+    chprintf(chp, "The first byte to read from the memory.\n");
+    chprintf(chp, "\tcount [default = 1]\n");
+    chprintf(chp, "The number of elements to read.\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\tNOTE\n");
+    chprintf(chp, "Type conversions of this function might fail.\n");
+    chprintf(chp, "If so, use type=hex and convert by hand.\n");
+    chprintf(chp, "\n");
+    return;
+  }
+
+  uint8_t type_size = 0;
+  Type type = HEX;
+  if (strcmp(argv[0],"hex") == 0) {
+    type_size = sizeof(unsigned char);
+    type = HEX;
+  } else if(strcmp(argv[0],"u8") == 0) {
+    type_size = sizeof(uint8_t);
+    type = U8;
+  } else if(strcmp(argv[0],"u16") == 0) {
+    type_size = sizeof(uint16_t);
+    type = U16;
+  } else if(strcmp(argv[0],"u32") == 0) {
+    type_size = sizeof(uint32_t);
+    type = U32;
+  } else if(strcmp(argv[0],"s8") == 0) {
+    type_size = sizeof(int8_t);
+    type = S8;
+  } else if(strcmp(argv[0],"s16") == 0) {
+    type_size = sizeof(int16_t);
+    type = S16;
+  } else if(strcmp(argv[0],"s32") == 0) {
+    type_size = sizeof(int32_t);
+    type = S32;
+  } else {
+    chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
+    return;
+  }
+
+  unsigned int start_byte = atoi(argv[1]);
+
+  unsigned int num_elements = 1;
+  if (argc >= 3)
+    num_elements = atoi(argv[2]);
+
+  const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
+  uint8_t buffer[eeprom_size];
+  if (start_byte + (type_size * num_elements) > eeprom_size) {
+    num_elements = (eeprom_size - start_byte) / type_size;
+    chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
+  }
+
+  chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte);
+
+  // Work around, because stm32f1 cannot read a single byte
+  if (type_size*num_elements < 2)
+    type_size = 2;
+
+  uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements);
+
+  if (bytes_read != type_size*num_elements)
+    chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);
+
+  for (unsigned int i = 0; i < num_elements; ++i) {
+    switch (type) {
+      case HEX:
+        chprintf(chp, "%02X ", buffer[i]);
+        break;
+      case U8:
+        chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
+        break;
+      case U16:
+        chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
+        break;
+      case U32:
+        chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
+        break;
+      case S8:
+        chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
+        break;
+      case S16:
+        chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
+        break;
+      case S32:
+        chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
+        break;
+      default:
+        break;
+    }
+  }
+  chprintf(chp, "\n");
+
+  return;
+}
+
+void shellRequestSetLights(BaseSequentialStream *chp, int argc, char *argv[]) {
+
+  if (argc < 2 || argc == 3 ||strcmp(argv[0],"help") == 0) {
+    chprintf(chp, "\tUSAGE:\n");
+    chprintf(chp, "> set_lights <led mask> <white/red> [<green> <blue>]\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\tled mask\n");
+    chprintf(chp, "The LEDs to be set.\n");
+    chprintf(chp, "You can set multiple LEDs at once by adding the following values:\n");
+    chprintf(chp, "  0x01 - rear left LED (SSW)\n");
+    chprintf(chp, "  0x02 - left rear LED (WSW)\n");
+    chprintf(chp, "  0x04 - left front LED (WNW)\n");
+    chprintf(chp, "  0x08 - front left LED (NNW)\n");
+    chprintf(chp, "  0x10 - front right LED (NNE)\n");
+    chprintf(chp, "  0x20 - right front LED (ENE)\n");
+    chprintf(chp, "  0x40 - right rear LED (ESE)\n");
+    chprintf(chp, "  0x80 - rear right LED (SSE)\n");
+    chprintf(chp, "\twhite/red\n");
+    chprintf(chp, "If no optional argument is given, this arguments sets the white value of the selected LEDs.\n");
+    chprintf(chp, "Otherwise this arguments sets the red color channel value.\n");
+    chprintf(chp, "\tgreen\n");
+    chprintf(chp, "Sets the green color channel value.\n");
+    chprintf(chp, "\tblue\n");
+    chprintf(chp, "Sets the blue color channel value.\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\tExample:\n");
+    chprintf(chp, "This line will set the two most left and two most right LEDs to bright cyan.\n");
+    chprintf(chp, "> set_lights 0x66 0 255 255\n");
+    chprintf(chp, "\n");
+    return;
+  }
+
+  int arg_mask = strtol(argv[0], NULL, 0);
+  int red = strtol(argv[1], NULL, 0);
+  int green = red;
+  int blue = red;
+  if (argc >= 4) {
+    green = strtol(argv[2], NULL, 0);
+    blue = strtol(argv[3], NULL, 0);
+  }
+  Color color(red, green, blue);
+
+  if (arg_mask & 0x01) {
+    global.robot.setLightColor(constants::LightRing::LED_SSW, color);
+  }
+  if (arg_mask & 0x02) {
+    global.robot.setLightColor(constants::LightRing::LED_WSW, color);
+  }
+  if (arg_mask & 0x04) {
+    global.robot.setLightColor(constants::LightRing::LED_WNW, color);
+  }
+  if (arg_mask & 0x08) {
+    global.robot.setLightColor(constants::LightRing::LED_NNW, color);
+  }
+  if (arg_mask & 0x10) {
+    global.robot.setLightColor(constants::LightRing::LED_NNE, color);
+  }
+  if (arg_mask & 0x20) {
+    global.robot.setLightColor(constants::LightRing::LED_ENE, color);
+  }
+  if (arg_mask & 0x40) {
+    global.robot.setLightColor(constants::LightRing::LED_ESE, color);
+  }
+  if (arg_mask & 0x80) {
+    global.robot.setLightColor(constants::LightRing::LED_SSE, color);
+  }
+
+  return;
+}
+
+void boardPeripheryCheck(BaseSequentialStream *chp) {
+  msg_t result;
+  chprintf(chp, "\nCHECK: START\n");
+  // Check the accelerometer
+  result = global.lis331dlh.getCheck();
+  if (result == global.lis331dlh.CHECK_OK)
+    chprintf(chp, "LIS331DLH: OK\n");
+  else
+    chprintf(chp, "LIS331DLH: FAIL\n");
+
+  // Self-test accelerometer
+//  lis331dlh.printSelfTest(NULL);
+
+  // Check the eeprom
+  result = global.memory.getCheck();
+  if ( result != global.memory.OK)
+    chprintf(chp, "Memory Structure: FAIL\n");
+  else
+    chprintf(chp, "Memory Structure: OK\n");
+
+  // Check the gyroscope
+  result = global.l3g4200d.getCheck();
+  if (result == global.l3g4200d.CHECK_OK)
+    chprintf(chp, "L3G4200D: OK\n");
+  else
+    chprintf(chp, "L3G4200D: FAIL\n");
+
+  // Check the magnetometer
+  result = global.hmc5883l.getCheck();
+  if (result == global.hmc5883l.CHECK_OK)
+    chprintf(chp, "HMC5883L: OK\n");
+  else
+    chprintf(chp, "HMC5883L: FAIL\n");
+
+  // Check the MUX
+  result = global.HW_PCA9544.getCheck();
+  if (result == global.HW_PCA9544.CHECK_OK)
+    chprintf(chp, "PCA9544: OK\n");
+  else
+    chprintf(chp, "PCA9544: FAIL\n");
+
+  // Check the power monitor
+  chprintf(chp, "INA219:\tVDD (3.3V):\n");
+  result = global.ina219.selftest();
+  if (result == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "->\tnot implemented\n");
+  else if (result != INA219::Driver::ST_OK)
+    chprintf(chp, "->\tFAIL (error code 0x%02X)\n", result);
+  else
+    chprintf(chp, "->\tOK\n");
+
+  // Check the proximitysensors
+  for (uint8_t i = 0x00; i < global.vcnl4020.size(); i++) {
+    result = global.vcnl4020[i].getCheck();
+    if (result == global.vcnl4020[i].CHECK_OK)
+      chprintf(chp, "VCNL4020: %d OK\n", i);
+    else
+      chprintf(chp, "VCNL4020: %d FAIL\n", i);
+  }
+  chprintf(chp, "CHECK: FINISH\n");
+}
+
+void shellRequestCheck(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestCheck\n");
+  boardPeripheryCheck(chp);
+}
+
+void shellRequestResetMemory(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestInitMemory\n");
+
+  msg_t res = global.memory.resetMemory();
+  if ( res != global.memory.OK)
+    chprintf(chp, "Memory Init: FAIL\n");
+  else
+    chprintf(chp, "Memory Init: OK\n");
+}
+
+void shellRequestGetBoardId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestGetBoardId\n");
+  uint8_t id = 0xFFu;
+
+  msg_t res = global.memory.getBoardId(&id);
+
+  if (res != global.memory.OK)
+    chprintf(chp, "Get Board ID: FAIL\n");
+  else
+    chprintf(chp, "Get Board ID: %u\n", id);
+}
+
+void shellRequestSetBoardId(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestSetBoardId\n");
+
+  if (argc == 0) {
+    chprintf(chp, "Usage: %s\n","set_board_id <idx>");
+  } else {
+    msg_t res = global.memory.setBoardId(atoi(argv[0]));
+    if (res != global.memory.OK)
+      chprintf(chp, "Set Board ID: FAIL\n");
+    else
+      chprintf(chp, "Set Board ID: OK\n");
+  }
+}
+
+void shellRequestResetCalibrationConstants(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestResetCalibrationConstants\n");
+  chprintf(chp, "Setting Ed=1.0f, Eb=1.0f\n");
+  msg_t res;
+
+  res = global.memory.setEd(1.0f);
+  if (res != global.memory.OK)
+    chprintf(chp, "Set Ed: FAIL\n");
+  else
+    chprintf(chp, "Set Ed: OK\n");
+
+  res = global.memory.setEb(1.0f);
+  if (res != global.memory.OK)
+    chprintf(chp, "Set Eb: FAIL\n");
+  else
+    chprintf(chp, "Set Eb: OK\n");
+}
+
+void shellRequestGetCalibrationConstants(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestGetCalibrationConstants\n");
+  msg_t res;
+  float Ed, Eb;
+
+  res = global.memory.getEd(&Ed);
+  if (res != global.memory.OK)
+    chprintf(chp, "Get Ed: FAIL\n");
+  else
+    chprintf(chp, "Get Ed: OK \t Ed=%f\n", Ed);
+
+  res = global.memory.getEb(&Eb);
+  if (res != global.memory.OK)
+    chprintf(chp, "Get Eb: FAIL\n");
+  else
+    chprintf(chp, "Get Eb: OK \t Eb=%f\n", Eb);
+}
+
+void shellRequestSetCalibrationConstants(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestSetCalibrationConstants\n");
+  msg_t res;
+
+  if (argc != 3) {
+    chprintf(chp, "Usage: %s\n","set_Ed_Eb <Ed> <Eb> <Write To Eeprom ? 1 : 0>");
+    chprintf(chp, "(Call with floating point values for Ed and Eb values and write condition):\n");
+    return;
+  }
+  // Get the write condition
+  const float Ed = atof(argv[0]);
+  const float Eb = atof(argv[1]);
+  bool_t writeToMemory = atoi(argv[2]) == 1 ? true : false;
+
+  res = global.motorcontrol.setWheelDiameterCorrectionFactor(Ed, writeToMemory);
+  if (res != global.memory.OK)
+    chprintf(chp, "Set Ed: FAIL\n");
+  else
+    chprintf(chp, "Set Ed: OK \t Ed=%f\n", Ed);
+
+  res = global.motorcontrol.setActualWheelBaseDistance(Eb, writeToMemory);
+  if (res != global.memory.OK)
+    chprintf(chp, "Set Eb: FAIL\n");
+  else
+    chprintf(chp, "Set Eb: OK \t Ed=%f\n", Eb);
+}
+
+void shellRequestGetVcnl(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestGetVcnl\n");
+  // Print the sensor information
+  if (argc != 1) {
+    chprintf(chp, "Usage: %s\n","get_vcnl <rep>");
+    return;
+  }
+  for (int32_t rep = 0x00; rep < atoi(argv[0]); ++rep) {
+    for (uint8_t idx = 0x00; idx < global.vcnl4020.size(); idx++) {
+     chprintf(chp, "%d: Ambi %d\tProx raw %d\tProx scaled %d\n", idx, global.vcnl4020[idx].getAmbientLight(), global.vcnl4020[idx].getProximity(), global.vcnl4020[idx].getProximityScaledWoOffset());
+    }
+    chprintf(chp, "\n\n");
+    BaseThread::sleep(MS2ST(250));
+  }
+}
+
+void shellRequestSetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestSetVcnlOffset\n");
+  if (argc != 2) {
+    chprintf(chp, "Usage: %s\n","set_vcnl <idx> <offset>");
+    return;
+  }
+
+  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
+  uint16_t vcnlOffset = static_cast<uint16_t>(atoi(argv[1]));
+
+  if (vcnlIdx >= global.vcnl4020.size()) {
+    chprintf(chp, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
+    return;
+  }
+
+  msg_t res = global.memory.setVcnl4020Offset(vcnlOffset, vcnlIdx);
+  if (res != global.memory.OK) {
+    chprintf(chp, "Set Offset: FAIL\n");
+  } else {
+    chprintf(chp, "Set Offset: OK\n");
+    global.vcnl4020[vcnlIdx].setProximityOffset(vcnlOffset);
+  }
+}
+
+void shellRequestResetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
+  msg_t res = global.memory.OK;
+  for (uint8_t idx = 0; idx < 4; ++idx) {
+    msg_t r = global.memory.setVcnl4020Offset(0, idx);
+    if (r == global.memory.OK) {
+      global.vcnl4020[idx].setProximityOffset(0);
+    } else {
+      chprintf(chp, "Reset Offset %u: FAIL\n", idx);
+      res = r;
+    }
+  }
+
+  if (res == global.memory.OK) {
+    chprintf(chp, "Reset Offset: DONE\n");
+  }
+
+  return;
+}
+
+void shellRequestGetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestGetVcnlOffset\n");
+  if (argc != 1) {
+    chprintf(chp, "Call with decimal numbers: get_vcnl <idx>\n");
+    return;
+  }
+
+  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
+
+  if (vcnlIdx >= global.vcnl4020.size()) {
+    chprintf(chp, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
+    return;
+  }
+
+  uint16_t vcnlOffset;
+  msg_t res = global.memory.getVcnl4020Offset(&vcnlOffset, vcnlIdx);
+  if (res != global.memory.OK) {
+    chprintf(chp, "Get Offset: FAIL\n");
+  } else {
+    chprintf(chp, "Get Offset: OK \t Offset=%d\n", vcnlOffset);
+  }
+}
+
+void shellRequestCalib(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestCalib\n");
+  global.robot.calibrate();
+}
+
+void shellRequestGetRobotId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestGetRobotId\n");
+  chprintf(chp, "Robot ID: %u\n", global.robot.getRobotID());
+  if (global.robot.getRobotID() == 0)
+    chprintf(chp, "Warning: The board ID seems to be uninitialized.\n");
+}
+
+void shellRequestGetSystemLoad(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestGetSystemLoad\n");
+  uint8_t seconds = 1;
+  if (argc >= 1) {
+    seconds = atoi(argv[0]);
+  }
+  chprintf(chp, "measuring CPU load for %u %s...\n", seconds, (seconds>1)? "seconds" : "second");
+
+  const systime_t before = chThdGetTicks(chSysGetIdleThread());
+  BaseThread::sleep(S2ST(seconds));
+  const systime_t after = chThdGetTicks(chSysGetIdleThread());
+  const float usage = 1.0f - (float(after - before) / float(seconds * CH_FREQUENCY));
+
+  chprintf(chp, "CPU load: %3.2f%%\n", usage * 100);
+  const uint32_t memory_total = 0x10000;
+  const uint32_t memory_load = memory_total - chCoreStatus();
+  chprintf(chp, "RAM load: %3.2f%% (%u / %u Byte)\n", float(memory_load)/float(memory_total) * 100, memory_load, memory_total);
+}
+
+void shellSwitchBoardCmd(BaseSequentialStream *chp, int argc, char *argv[]) {
+  if (argc != 1) {
+    chprintf(chp, "Call with decimal numbers: shell_board <idx>\n");
+    return;
+  }
+  uint8_t boardIdx = static_cast<uint8_t>(atoi(argv[0]));
+
+  chprintf(chp, "shellSwitchBoardCmd\n");
+  global.sercanmux1.sendSwitchCmd(boardIdx);
+}
+
+void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *argv[]) {
+  // check the magic number
+  if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) {
+    chprintf(chp, "Bootloader version:  %u.%u.%u\n",
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (3*4))));
+    chprintf(chp, "Callback functions:\n");
+    if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2) {
+      chprintf(chp, "\thibernate:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (4*4))) ? "available" : "unsupported");
+      chprintf(chp, "\tdeepsleep:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (5*4))) ? "available" : "unsupported");
+      chprintf(chp, "\ttransportation: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (6*4))) ? "available" : "unsupported");
+      chprintf(chp, "\trestart:        %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (10*4))) ? "available" : "unsupported");
+    } else if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3) {
+      chprintf(chp, "\thibernate:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (4*4))) ? "available" : "unsupported");
+      chprintf(chp, "\tdeepsleep:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (5*4))) ? "available" : "unsupported");
+      chprintf(chp, "\ttransportation: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (6*4))) ? "available" : "unsupported");
+      chprintf(chp, "\trestart:        %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (7*4))) ? "available" : "unsupported");
+      chprintf(chp, "\thandle request: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (8*4))) ? "available" : "unsupported");
+    }
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader deprecated\n");
+  }
+
+  return;
+}
+
+void shellRequestMotorDrive(BaseSequentialStream *chp, int argc, char *argv[]) {
+  types::kinematic tmp;
+  tmp.w_z = 0;
+  tmp.x = 0;
+  if (argc == 1){
+    chprintf(chp, "Set speed to %i um/s \n", atoi(argv[0]));
+    tmp.x = atoi(argv[0]);
+  } else {
+    if(argc == 2){
+      chprintf(chp, "Set speed to %i \n um/s", atoi(argv[0]));
+      chprintf(chp, "Set angular speed to %i \n urad/s", atoi(argv[1]));
+      tmp.x = atoi(argv[0]);
+      tmp.w_z= atoi(argv[1]);
+    } else {
+      chprintf(chp, "Wrong number of parameters given (%i), stopping robot \n", argc);
+    }
+  }
+
+  global.motorcontrol.setTargetSpeed(tmp);
+  return;
+}
+
+void shellRequestMotorStop(BaseSequentialStream *chp, int argc, char *argv[]) {
+  types::kinematic tmp;
+  tmp.x = 0;
+  tmp.w_z = 0;
+
+  global.motorcontrol.setTargetSpeed(tmp);
+
+  chprintf(chp, "stop");
+return;
+}
+
+void shellRequestMotorCalibrate(BaseSequentialStream *chp, int argc, char *argv[]) {
+  global.motorcontrol.isCalibrating = true;
+
+  return;
+}
+
+void shellRequestMotorGetGains(BaseSequentialStream *chp, int argc, char *argv[]){
+  global.motorcontrol.printGains();
+
+  return;
+}
+
+static const ShellCommand commands[] = {
+  {"shutdown", shellRequestShutdown},
+  {"wakeup", shellRequestWakeup},
+  {"check", shellRequestCheck},
+  {"reset_memory", shellRequestResetMemory},
+  {"get_board_id", shellRequestGetBoardId},
+  {"set_board_id", shellRequestSetBoardId},
+  {"get_memory_data", shellRequestGetMemoryData},
+  {"get_vcnl", shellRequestGetVcnl},
+  {"calib_vcnl_offset", shellRequestCalib},
+  {"set_vcnl_offset", shellRequestSetVcnlOffset},
+  {"reset_vcnl_offset", shellRequestResetVcnlOffset},
+  {"get_vcnl_offset", shellRequestGetVcnlOffset},
+  {"reset_Ed_Eb", shellRequestResetCalibrationConstants},
+  {"get_Ed_Eb", shellRequestGetCalibrationConstants},
+  {"set_Ed_Eb", shellRequestSetCalibrationConstants},
+  {"get_robot_id", shellRequestGetRobotId},
+  {"get_system_load", shellRequestGetSystemLoad},
+  {"set_lights", shellRequestSetLights},
+  {"shell_board", shellSwitchBoardCmd},
+  {"get_bootloader_info", shellRequestGetBootloaderInfo},
+  {"motor_drive", shellRequestMotorDrive},
+  {"motor_stop", shellRequestMotorStop},
+  {"motor_calibrate", shellRequestMotorCalibrate},
+  {"motor_getGains", shellRequestMotorGetGains},
+  {NULL, NULL}
+};
+
+static const ShellConfig shell_cfg1 = {
+  (BaseSequentialStream *) &global.sercanmux1,
+  commands
+};
+
+void initPowermonitor(INA219::Driver &ina219, const float shuntResistance_O, const float maxExpectedCurrent_A, const uint16_t currentLsb_uA)
+{
+  INA219::CalibData calibData;
+  INA219::InitData initData;
+
+  calibData.input.configuration.content.brng = INA219::Configuration::BRNG_16V;
+  calibData.input.configuration.content.pg = INA219::Configuration::PGA_40mV;
+  calibData.input.configuration.content.badc = INA219::Configuration::ADC_68100us;
+  calibData.input.configuration.content.sadc = INA219::Configuration::ADC_68100us;
+  calibData.input.configuration.content.mode = INA219::Configuration::MODE_ShuntBus_Continuous;
+  calibData.input.shunt_resistance_O = shuntResistance_O;
+  calibData.input.max_expected_current_A = maxExpectedCurrent_A;
+  calibData.input.current_lsb_uA = currentLsb_uA;
+  if (ina219.calibration(&calibData) != BaseSensor<>::SUCCESS)
+  {
+    chprintf((BaseSequentialStream*)&SD1, "WARNING: calibration of INA219 failed.\n");
+  }
+
+  initData.configuration.value = calibData.input.configuration.value;
+  initData.calibration = calibData.output.calibration_value;
+  initData.current_lsb_uA = calibData.output.current_lsb_uA;
+  if (ina219.init(&initData) != BaseSensor<>::SUCCESS)
+  {
+    chprintf((BaseSequentialStream*)&SD1, "WARNING: initialization of INA219 failed.\n");
+  }
+
+  if (calibData.input.current_lsb_uA != initData.current_lsb_uA)
+  {
+    chprintf((BaseSequentialStream*)&SD1, "NOTE: LSB for current measurement was limited when initializing INA219 (%u -> %u)", calibData.input.current_lsb_uA, initData.current_lsb_uA);
+  }
+
+  return;
+}
+
+/*
+ * Application entry point.
+ */
+int main(void) {
+
+//  int16_t accel;
+  Thread *shelltp = NULL;
+
+  /*
+   * System initializations.
+   * - HAL initialization, this also initializes the configured device drivers
+   *   and performs the board-specific initializations.
+   * - Kernel initialization, the main() function becomes a thread and the
+   *   RTOS is active.
+   */
+  halInit();
+  qeiInit();
+  System::init();
+
+//  boardWakeup();
+//  boardWriteIoPower(1);
+
+  /*
+   * Activates the serial driver 2 using the driver default configuration.
+   */
+  sdStart(&SD1, &global.sd1_config);
+
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+  chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n");
+  if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (3*4))));
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader deprecated\n");
+  }
+  chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n");
+  // make sure that the info text is completetly printed
+  BaseThread::sleep(10);
+
+  extStart(&EXTD1, &extcfg);
+
+  /*
+   * Wait for a certain amount of time, so that the PowerBoard can activate
+   * the IO voltages for the I2C Bus
+   */
+  BaseThread::sleep(MS2ST(2000));
+
+  boardClearI2CBus(GPIOB_COMPASS_SCL);
+  boardClearI2CBus(GPIOB_IR_SCL);
+
+  global.HW_I2C1.start(&global.i2c1_config);
+  global.HW_I2C2.start(&global.i2c2_config);
+
+  global.memory.init();
+
+  uint8_t i = 0;
+  if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
+    chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
+  }
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+
+  initPowermonitor(global.ina219, 0.1f, 0.075f, 10);
+
+  for (i = 0x00u; i < global.vcnl4020.size(); i++) {
+    uint16_t buffer;
+    global.memory.getVcnl4020Offset(&buffer,i);
+    global.vcnl4020[i].setProximityOffset(buffer);
+    global.vcnl4020[i].start(NORMALPRIO);
+  }
+
+  global.ina219.start(NORMALPRIO);
+
+  global.hmc5883l.start(NORMALPRIO + 8);
+
+  global.increments.start();  // Start the qei driver
+
+  global.odometry.start(NORMALPRIO + 20);
+
+  global.robot.start(HIGHPRIO - 1);
+
+  global.motorcontrol.start(NORMALPRIO + 7);
+
+  global.distcontrol.start(NORMALPRIO + 9);
+
+  // Set target velocity
+  types::kinematic velocity;
+  velocity.x = 0; // E.g.  "100*1e3" equals "10 cm/s"
+  velocity.w_z = 0; // E.g. "2*1e6" equals "2 rad/s"
+  global.motorcontrol.setTargetSpeed(velocity);
+
+  // Start the three axes linear accelerometer
+  global.lis331dlh.configure(&global.accel_run_config);
+  global.lis331dlh.start(NORMALPRIO+4);
+
+  // Start the three axes gyroscope
+  global.l3g4200d.configure(&global.gyro_run_config);
+  global.l3g4200d.start(NORMALPRIO+5);
+
+  // Start the user thread
+  global.userThread.start(NORMALPRIO);
+
+  /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
+  palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_HIGH);
+
+  /* wait until all modules are done */
+  while (palReadPad(GPIOC, GPIOC_SYS_INT_N) == PAL_LOW) {
+    continue;
+  }
+
+  while (true) {
+
+    if (!shelltp)
+      shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
+    else if (chThdTerminated(shelltp)) {
+      chThdRelease(shelltp);    /* Recovers memory of the previous shell. */
+      shelltp = NULL;           /* Triggers spawning of a new shell.      */
+    }
+
+    // Let the LED just blink as an alive signal
+    boardWriteLed(1);
+    BaseThread::sleep(MS2ST(250));
+    boardWriteLed(0);
+    BaseThread::sleep(MS2ST(250));
+
+    if (shutdown_now != SHUTDOWN_NONE) {
+      if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER) {
+        chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
+        shutdown_now = SHUTDOWN_NONE;
+      } else {
+        uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR;
+        if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2) {
+          switch (shutdown_now) {
+            case SHUTDOWN_TRANSPORTATION:
+              blCallbackPtrAddr += 6 * 4;
+              break;
+            case SHUTDOWN_DEEPSLEEP:
+              blCallbackPtrAddr += 5 * 4;
+              break;
+            case SHUTDOWN_HIBERNATE:
+              blCallbackPtrAddr += 4 * 4;
+              break;
+            case SHUTDOWN_HANDLE_REQUEST:
+            case SHUTDOWN_RESTART:
+              blCallbackPtrAddr += 10 * 4;
+              break;
+            default:
+              blCallbackPtrAddr = 0;
+              break;
+          }
+        } else if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3) {
+          switch (shutdown_now) {
+            case SHUTDOWN_TRANSPORTATION:
+              blCallbackPtrAddr += 6 * 4;
+              break;
+            case SHUTDOWN_DEEPSLEEP:
+              blCallbackPtrAddr += 5 * 4;
+              break;
+            case SHUTDOWN_HIBERNATE:
+              blCallbackPtrAddr += 4 * 4;
+              break;
+            case SHUTDOWN_RESTART:
+              blCallbackPtrAddr += 7 * 4;
+              break;
+            case SHUTDOWN_HANDLE_REQUEST:
+              blCallbackPtrAddr += 8 * 4;
+              break;
+            default:
+              blCallbackPtrAddr = 0;
+              break;
+          }
+        }
+
+        void (*blCallback)(void) = NULL;
+        if (blCallbackPtrAddr) {
+          blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr));
+
+          if (!blCallback) {
+            chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
+            shutdown_now = SHUTDOWN_NONE;
+          } else {
+            chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
+            palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW);
+            palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW);
+
+            chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
+            systemShutdown();
+            chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
+            BaseThread::sleep(MS2ST(10)); // sleep to print everything
+
+            blCallback();
+          }
+
+        } else {
+          chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
+          shutdown_now = SHUTDOWN_NONE;
+        }
+      }
+
+//    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
+//        accel = lis331dlh.getAcceleration(i);
+//        chprintf((BaseSequentialStream*) &SD1, "%c%04X ", accel < 0 ? '-' : '+', accel < 0 ? -accel : accel);
+//    }
+//
+//    chprintf((BaseSequentialStream*) &SD1, "\n");
+//
+//    // Print out an alive signal
+//    chprintf((BaseSequentialStream*) &SD1, ".");
+    }
+  }
+
+  return 0;
+}
diff --git a/devices/DiWheelDrive/mcuconf.h b/devices/DiWheelDrive/mcuconf.h
new file mode 100644
index 0000000..19ec6e7
--- /dev/null
+++ b/devices/DiWheelDrive/mcuconf.h
@@ -0,0 +1,193 @@
+#define STM32F103_MCUCONF
+
+/*
+ * STM32F103 drivers configuration.
+ * The following settings override the default settings present in
+ * the various device driver implementation headers.
+ * Note that the settings for each driver only have effect if the whole
+ * driver is enabled in halconf.h.
+ *
+ * IRQ priorities:
+ * 15...0       Lowest...Highest.
+ *
+ * DMA priorities:
+ * 0...3        Lowest...Highest.
+ */
+
+/*
+ * HAL driver system settings.
+ */
+#define STM32_NO_INIT                       FALSE
+#define STM32_HSI_ENABLED                   TRUE
+#define STM32_LSI_ENABLED                   FALSE
+#define STM32_HSE_ENABLED                   TRUE
+#define STM32_LSE_ENABLED                   FALSE
+#define STM32_SW                            STM32_SW_PLL
+#define STM32_PLLSRC                        STM32_PLLSRC_HSE
+#define STM32_PLLXTPRE                      STM32_PLLXTPRE_DIV1
+#define STM32_PLLMUL_VALUE                  9
+#define STM32_HPRE                          STM32_HPRE_DIV1
+#define STM32_PPRE1                         STM32_PPRE1_DIV2
+#define STM32_PPRE2                         STM32_PPRE2_DIV2
+#define STM32_ADCPRE                        STM32_ADCPRE_DIV4
+#define STM32_USB_CLOCK_REQUIRED            TRUE
+#define STM32_USBPRE                        STM32_USBPRE_DIV1P5
+#define STM32_MCOSEL                        STM32_MCOSEL_NOCLOCK
+#define STM32_RTCSEL                        STM32_RTCSEL_HSEDIV
+#define STM32_PVD_ENABLE                    FALSE
+#define STM32_PLS                           STM32_PLS_LEV0
+
+/*
+ * ADC driver system settings.
+ */
+#define STM32_ADC_USE_ADC1                  FALSE
+#define STM32_ADC_ADC1_DMA_PRIORITY         2
+#define STM32_ADC_ADC1_IRQ_PRIORITY         6
+
+/*
+ * CAN driver system settings.
+ */
+#define STM32_CAN_USE_CAN1                  TRUE
+#define STM32_CAN_CAN1_IRQ_PRIORITY         11
+
+/*
+ * EXT driver system settings.
+ */
+#define STM32_EXT_EXTI0_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI1_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI2_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI3_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI4_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI5_9_IRQ_PRIORITY      6
+#define STM32_EXT_EXTI10_15_IRQ_PRIORITY    6
+#define STM32_EXT_EXTI16_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI17_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI18_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI19_IRQ_PRIORITY       6
+
+/*
+ * GPT driver system settings.
+ */
+#define STM32_GPT_USE_TIM1                  FALSE
+#define STM32_GPT_USE_TIM2                  FALSE
+#define STM32_GPT_USE_TIM3                  FALSE
+#define STM32_GPT_USE_TIM4                  FALSE
+#define STM32_GPT_USE_TIM5                  FALSE
+#define STM32_GPT_USE_TIM8                  FALSE
+#define STM32_GPT_TIM1_IRQ_PRIORITY         7
+#define STM32_GPT_TIM2_IRQ_PRIORITY         7
+#define STM32_GPT_TIM3_IRQ_PRIORITY         7
+#define STM32_GPT_TIM4_IRQ_PRIORITY         7
+#define STM32_GPT_TIM5_IRQ_PRIORITY         7
+#define STM32_GPT_TIM8_IRQ_PRIORITY         7
+
+/*
+ * I2C driver system settings.
+ */
+#define STM32_I2C_USE_I2C1                  TRUE
+#define STM32_I2C_USE_I2C2                  TRUE
+#define STM32_I2C_I2C1_IRQ_PRIORITY         5
+#define STM32_I2C_I2C2_IRQ_PRIORITY         5
+#define STM32_I2C_I2C1_DMA_PRIORITY         3
+#define STM32_I2C_I2C2_DMA_PRIORITY         3
+#define STM32_I2C_I2C1_DMA_ERROR_HOOK()     chSysHalt()
+#define STM32_I2C_I2C2_DMA_ERROR_HOOK()     chSysHalt()
+#define STM32_I2C_USE_DMA                   FALSE
+
+/*
+ * ICU driver system settings.
+ */
+#define STM32_ICU_USE_TIM1                  FALSE
+#define STM32_ICU_USE_TIM2                  FALSE
+#define STM32_ICU_USE_TIM3                  FALSE
+#define STM32_ICU_USE_TIM4                  FALSE
+#define STM32_ICU_USE_TIM5                  FALSE
+#define STM32_ICU_USE_TIM8                  FALSE
+#define STM32_ICU_TIM1_IRQ_PRIORITY         7
+#define STM32_ICU_TIM2_IRQ_PRIORITY         7
+#define STM32_ICU_TIM3_IRQ_PRIORITY         7
+#define STM32_ICU_TIM4_IRQ_PRIORITY         7
+#define STM32_ICU_TIM5_IRQ_PRIORITY         7
+#define STM32_ICU_TIM8_IRQ_PRIORITY         7
+
+/*
+ * PWM driver system settings.
+ */
+#define STM32_PWM_USE_ADVANCED              FALSE
+#define STM32_PWM_USE_TIM1                  FALSE
+#define STM32_PWM_USE_TIM2                  TRUE
+#define STM32_PWM_USE_TIM3                  FALSE
+#define STM32_PWM_USE_TIM4                  FALSE
+#define STM32_PWM_USE_TIM5                  FALSE
+#define STM32_PWM_USE_TIM8                  FALSE
+#define STM32_PWM_TIM1_IRQ_PRIORITY         7
+#define STM32_PWM_TIM2_IRQ_PRIORITY         7
+#define STM32_PWM_TIM3_IRQ_PRIORITY         7
+#define STM32_PWM_TIM4_IRQ_PRIORITY         7
+#define STM32_PWM_TIM5_IRQ_PRIORITY         7
+#define STM32_PWM_TIM8_IRQ_PRIORITY         7
+
+/*
+ * RTC driver system settings.
+ */
+#define STM32_RTC_IRQ_PRIORITY              15
+
+/*
+ * SERIAL driver system settings.
+ */
+#define STM32_SERIAL_USE_USART1             TRUE
+#define STM32_SERIAL_USE_USART2             FALSE
+#define STM32_SERIAL_USE_USART3             FALSE
+#define STM32_SERIAL_USE_UART4              FALSE
+#define STM32_SERIAL_USE_UART5              FALSE
+#define STM32_SERIAL_USART1_PRIORITY        12
+#define STM32_SERIAL_USART2_PRIORITY        12
+#define STM32_SERIAL_USART3_PRIORITY        12
+#define STM32_SERIAL_UART4_PRIORITY         12
+#define STM32_SERIAL_UART5_PRIORITY         12
+
+/*
+ * SPI driver system settings.
+ */
+#define STM32_SPI_USE_SPI1                  TRUE
+#define STM32_SPI_USE_SPI2                  TRUE
+#define STM32_SPI_USE_SPI3                  FALSE
+#define STM32_SPI_SPI1_DMA_PRIORITY         1
+#define STM32_SPI_SPI2_DMA_PRIORITY         1
+#define STM32_SPI_SPI3_DMA_PRIORITY         1
+#define STM32_SPI_SPI1_IRQ_PRIORITY         10
+#define STM32_SPI_SPI2_IRQ_PRIORITY         10
+#define STM32_SPI_SPI3_IRQ_PRIORITY         10
+#define STM32_SPI_DMA_ERROR_HOOK(spip)      chSysHalt()
+
+/*
+ * UART driver system settings.
+ */
+#define STM32_UART_USE_USART1               FALSE
+#define STM32_UART_USE_USART2               FALSE
+#define STM32_UART_USE_USART3               FALSE
+#define STM32_UART_USART1_IRQ_PRIORITY      12
+#define STM32_UART_USART2_IRQ_PRIORITY      12
+#define STM32_UART_USART3_IRQ_PRIORITY      12
+#define STM32_UART_USART1_DMA_PRIORITY      0
+#define STM32_UART_USART2_DMA_PRIORITY      0
+#define STM32_UART_USART3_DMA_PRIORITY      0
+#define STM32_UART_DMA_ERROR_HOOK(uartp)    chSysHalt()
+
+/*
+ * USB driver system settings.
+ */
+#define STM32_USB_USE_USB1                  FALSE
+#define STM32_USB_LOW_POWER_ON_SUSPEND      FALSE
+#define STM32_USB_USB1_HP_IRQ_PRIORITY      13
+#define STM32_USB_USB1_LP_IRQ_PRIORITY      14
+
+/*
+ * QEI driver system settings.
+ */
+#define STM32_QEI_USE_TIM1                  FALSE
+#define STM32_QEI_USE_TIM2                  FALSE
+#define STM32_QEI_USE_TIM3                  TRUE
+#define STM32_QEI_USE_TIM4                  TRUE
+#define STM32_QEI_USE_TIM5                  FALSE
+#define STM32_QEI_USE_TIM8                  FALSE
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
new file mode 100644
index 0000000..c24a2ac
--- /dev/null
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -0,0 +1,379 @@
+#include "userthread.hpp"
+
+#include "global.hpp"
+
+using namespace amiro;
+
+extern Global global;
+
+// State machine states
+enum states : uint8_t {
+	IDLE,
+	GO_RIGHT,
+	GO_STRAIGHT,
+	PARKING,
+	PARKING_RIGHT,
+	PARKING_LEFT,
+	GO_LEFT,
+	SPINNING_PARKING,
+	SPINNING
+};
+
+// Policy
+states policy[] = {
+  GO_STRAIGHT,
+  GO_RIGHT,
+  GO_RIGHT,
+  GO_STRAIGHT,
+  GO_RIGHT,
+  GO_STRAIGHT,
+  GO_RIGHT,
+  GO_STRAIGHT,
+  GO_STRAIGHT,
+  GO_RIGHT,
+  GO_STRAIGHT,
+  GO_RIGHT,
+  GO_STRAIGHT
+};
+
+// The different classes (or members) of color discrimination
+// BLACK is the line itselfe
+// GREY is the boarder between the line and the surface
+// WHITE is the common surface
+enum colorMember : uint8_t {
+	BLACK=0,
+	GREY=1,
+	WHITE=2
+};
+
+// a buffer for the z-value of the accelerometer
+int16_t accel_z;
+bool running;
+
+// Get some information about the policy
+const int sizeOfPolicy = sizeof(policy) / sizeof(states);
+int policyCounter = 0; // Do not change this, it points to the beginning of the policy
+
+// Different speed settings (all values in "rounds per minute")
+const int rpmForward[2] = {25,25};
+const int rpmSoftLeft[2] = {15,25};
+const int rpmHardLeft[2] = {10,25};
+const int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
+const int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
+const int rpmTurnLeft[2] = {-10, 10};
+const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
+const int rpmHalt[2] = {0, 0};
+
+// Definition of the fuzzyfication function
+//  | Membership
+// 1|_B__   G    __W__
+//  |    \  /\  /
+//  |     \/  \/
+//  |_____/\__/\______ Sensor values
+// SEE MATLAB SCRIPT "fuzzyRule.m" for adjusting the values
+// All values are "raw sensor values"
+/* Use these values for white ground surface (e.g. paper) */
+
+const int blackStartFalling = 0x1000; // Where the black curve starts falling
+const int blackOff = 0x1800; // Where no more black is detected
+const int whiteStartRising = 0x4000; // Where the white curve starts rising
+const int whiteOn = 0x8000; // Where the white curve has reached the maximum value
+const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
+const int greyStartRising = blackStartFalling; // Where grey starts rising
+const int greyOff = whiteOn; // Where grey is completely off again
+
+/* Use these values for gray ground surfaces */
+/*
+const int blackStartFalling = 0x1000; // Where the black curve starts falling
+const int blackOff = 0x2800; // Where no more black is detected
+const int whiteStartRising = 0x4000; // Where the white curve starts rising
+const int whiteOn = 0x5000; // Where the white curve starts rising
+const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
+const int greyStartRising = blackStartFalling; // Where grey starts rising
+const int greyOff = whiteOn; // Where grey is completely off again
+*/
+
+int vcnl4020AmbientLight[4] = {0};
+int vcnl4020Proximity[4] = {0};
+
+// Border for the discrimination between black and white
+const int discrBlackWhite = 16000; // border in "raw sensor values"
+// Discrimination between black and white (returns BLACK or WHITE)
+// The border was calculated by a MAP-decider
+colorMember discrimination(int value) {
+	if (value < discrBlackWhite)
+		return BLACK;
+	else
+		return WHITE;
+}
+
+// Copy the speed from the source to the target array
+void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
+	target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];
+	target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL];
+}
+
+// Fuzzyfication of the sensor values
+void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) {
+	if (sensorValue < blackStartFalling ) {
+		// Only black value
+		fuzziedValue[BLACK] = 1.0f;
+		fuzziedValue[GREY] = 0.0f;
+		fuzziedValue[WHITE] = 0.0f;
+	} else if (sensorValue > whiteOn ) {
+		// Only white value
+		fuzziedValue[BLACK] = 0.0f;
+		fuzziedValue[GREY] = 0.0f;
+		fuzziedValue[WHITE] = 1.0f;
+	} else if ( sensorValue < greyMax) {
+		// Some greyisch value between black and grey
+
+		// Black is going down
+		if ( sensorValue > blackOff) {
+			fuzziedValue[BLACK] = 0.0f;
+		} else {
+			fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff);
+		}
+
+		// Grey is going up
+		if ( sensorValue < greyStartRising) {
+			fuzziedValue[GREY] = 0.0f;
+		} else {
+			fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising);
+		}
+
+		// White is absent
+		fuzziedValue[WHITE] = 0.0f;
+
+	} else if ( sensorValue >= greyMax) {
+		// Some greyisch value between grey white
+
+		// Black is absent
+		fuzziedValue[BLACK] = 0.0f;
+
+		// Grey is going down
+		if ( sensorValue < greyOff) {
+			fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff);
+		} else {
+			fuzziedValue[GREY] = 0.0f;
+		}
+
+		// White is going up
+		if ( sensorValue < whiteStartRising) {
+			fuzziedValue[WHITE] = 0.0f;
+		} else {
+			fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising);
+		}
+	}
+}
+
+// Return the color, which has the highest fuzzy value
+colorMember getMember(float (&fuzzyValue)[3]) {
+	colorMember member;
+
+	if (fuzzyValue[BLACK] > fuzzyValue[GREY])
+		if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
+			member = BLACK;
+		else
+			member = WHITE;
+	else
+		if (fuzzyValue[GREY] > fuzzyValue[WHITE])
+			member = GREY;
+		else
+			member = WHITE;
+
+	return member;
+}
+
+// Get a crisp output for the steering commands
+void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
+
+	if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
+	     member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
+		// straight
+		copyRpmSpeed(rpmForward, rpmFuzzyCtrl);
+	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||
+	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
+		// soft correction
+		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY)
+			// soft right
+			copyRpmSpeed(rpmSoftRight, rpmFuzzyCtrl);
+		else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE)
+			// hard right
+			copyRpmSpeed(rpmHardRight, rpmFuzzyCtrl);
+		else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY)
+			// soft left
+			copyRpmSpeed(rpmSoftLeft, rpmFuzzyCtrl);
+		else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE)
+			// hard left
+			copyRpmSpeed(rpmHardLeft, rpmFuzzyCtrl);
+	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||
+	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
+		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE)
+			// turn right
+			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+		else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE)
+			// turn left
+			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
+		else
+			// go straight
+			copyRpmSpeed(rpmForward, rpmFuzzyCtrl);
+	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE &&
+	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
+		// go straight and check wheel sensors
+		if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] != WHITE)
+			// turn left
+			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
+		else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] != WHITE)
+			// turn right
+			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+		else
+            // line is lost -> stop
+            copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
+	}
+
+	return;
+}
+
+Color memberToLed(colorMember member) {
+	switch (member) {
+		case BLACK:
+			return Color(Color::GREEN);
+		case GREY:
+			return Color(Color::YELLOW);
+		case WHITE:
+			return Color(Color::RED);
+		default:
+			return Color(Color::WHITE);
+	}
+}
+
+// Line following by a fuzzy controler
+void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
+	// FUZZYFICATION
+	// First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
+	float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3];
+	fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues);
+	fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues);
+	fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues);
+	fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues);
+
+	// INFERENCE RULE DEFINITION
+	// Get the member for each sensor
+	colorMember member[4];
+	member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues);
+	member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues);
+	member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues);
+	member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues);
+
+	// visualize sensors via LEDs
+	global.robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT]));
+	global.robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT]));
+	global.robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
+	global.robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));
+
+//	chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftFuzzyMemberValues[BLACK], leftFuzzyMemberValues[GREY], leftFuzzyMemberValues[WHITE]);
+//	chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
+
+	// DEFUZZYFICATION
+	defuzzyfication(member, rpmFuzzyCtrl);
+}
+
+// Set the speed by the array
+void setRpmSpeed(const int (&rpmSpeed)[2]) {
+	global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
+}
+
+// Get the next policy rule
+states getNextPolicy() {
+	// If the policy is over, start again
+	if (policyCounter >= sizeOfPolicy)
+		policyCounter = 3;
+
+	return policy[policyCounter++];
+}
+
+
+
+UserThread::UserThread() :
+  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
+{
+}
+
+UserThread::~UserThread()
+{
+}
+
+msg_t
+UserThread::main()
+{
+	/*
+	 * SETUP
+	 */
+	int rpmFuzzyCtrl[2] = {0};
+    for (uint8_t led = 0; led < 8; ++led) {
+		global.robot.setLightColor(led, Color(Color::BLACK));
+    }
+    running = false;
+
+	/*
+	 * LOOP
+	 */
+	while (!this->shouldTerminate())
+	{
+        /*
+         * read accelerometer z-value
+         */
+        accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);
+
+        /*
+         * evaluate the accelerometer
+         */
+        if (accel_z < -900 /*-0.9g*/) {
+            if (running) {
+                // stop the robot
+                running = false;
+                global.motorcontrol.setTargetRPM(0, 0);
+            } else {
+                // start the robot
+                running = true;
+            }
+            // set the front LEDs to blue for one second
+            global.robot.setLightColor(constants::LightRing::LED_SSW, Color(Color::BLACK));
+            global.robot.setLightColor(constants::LightRing::LED_WSW, Color(Color::BLACK));
+            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::WHITE));
+            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::WHITE));
+            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::WHITE));
+            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::WHITE));
+            global.robot.setLightColor(constants::LightRing::LED_ESE, Color(Color::BLACK));
+            global.robot.setLightColor(constants::LightRing::LED_SSE, Color(Color::BLACK));
+            this->sleep(MS2ST(1000));
+            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::BLACK));
+            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::BLACK));
+            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::BLACK));
+            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::BLACK));
+        }
+
+        if (running) {
+            // Read the proximity values
+            for (int i = 0; i < 4; i++) {
+                vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
+                vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
+            }
+
+//            chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
+//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
+//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
+//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
+//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);
+
+            lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl);
+            setRpmSpeed(rpmFuzzyCtrl);
+        }
+
+		this->sleep(MS2ST(100));
+	}
+
+  return RDY_OK;
+}
+
diff --git a/devices/DiWheelDrive/userthread.hpp b/devices/DiWheelDrive/userthread.hpp
new file mode 100644
index 0000000..ac056b1
--- /dev/null
+++ b/devices/DiWheelDrive/userthread.hpp
@@ -0,0 +1,21 @@
+#ifndef AMIRO_USERTHREAD_H_
+#define AMIRO_USERTHREAD_H_
+
+#include <ch.hpp>
+#include <amiroosconf.h>
+
+namespace amiro {
+
+class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
+{
+public:
+  explicit UserThread();
+
+  virtual ~UserThread();
+
+  virtual msg_t main();
+};
+
+} // end of namespace amiro
+
+#endif // AMIRO_USERTHREAD_H_
diff --git a/devices/LightRing/LightRing.cpp b/devices/LightRing/LightRing.cpp
new file mode 100644
index 0000000..98158fa
--- /dev/null
+++ b/devices/LightRing/LightRing.cpp
@@ -0,0 +1,120 @@
+#include "ch.hpp"
+#include "hal.h"
+
+#include "LightRing.h"
+
+#include <global.hpp>
+
+using namespace chibios_rt;
+using namespace amiro;
+using namespace types;
+
+extern volatile uint32_t shutdown_now;
+extern Global global;
+
+
+LightRing::LightRing(CANDriver *can, TLC5947 *tlc5947, fileSystemIo::FSIOLightRing *memory)
+    : ControllerAreaNetworkTx(can, CAN::LIGHT_RING_ID),
+      ControllerAreaNetworkRx(can, CAN::LIGHT_RING_ID),
+      tlc5947(tlc5947),
+      memory(memory) {
+  chDbgCheck(tlc5947 != NULL, "LightRing");
+}
+
+void LightRing::setLightBrightness(int brightness) {
+  this->tlc5947->setBrightness(brightness);
+  this->tlc5947->update();
+}
+
+void LightRing::setLightColor(int index, Color color) {
+  this->tlc5947->setColor(index, color);
+  this->tlc5947->update();
+}
+
+msg_t LightRing::receiveMessage(CANRxFrame *frame) {
+  int deviceId = this->decodeDeviceId(frame);
+
+  switch (deviceId) {
+
+    case CAN::SHELL_REPLY_ID(CAN::LIGHT_RING_ID):
+      if (frame->DLC > 0) {
+        sdWrite(&SD1, frame->data8, frame->DLC);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::SHELL_QUERY_ID(CAN::LIGHT_RING_ID):
+      if (frame->DLC != 0) {
+        global.sercanmux1.convCan2Serial(frame->data8, frame->DLC);
+        return RDY_OK;
+      } else {
+        global.sercanmux1.rcvSwitchCmd(this->decodeBoardId(frame));
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::COLOR_ID(0):
+    case CAN::COLOR_ID(1):
+    case CAN::COLOR_ID(2):
+    case CAN::COLOR_ID(3):
+    case CAN::COLOR_ID(4):
+    case CAN::COLOR_ID(5):
+    case CAN::COLOR_ID(6):
+    case CAN::COLOR_ID(7):
+      if (frame->DLC == 3) {
+        int index = deviceId & 0x7;
+        Color color(frame->data8[0], frame->data8[1], frame->data8[2]);
+        this->setLightColor(index, color);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::BRIGHTNESS_ID:
+      if (frame->DLC == 1) {
+        int brightness = frame->data8[0];
+        this->setLightBrightness(brightness);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::BROADCAST_SHUTDOWN:
+      if (frame->DLC == 2 && frame->data16[0] == CAN::SHUTDOWN_MAGIC) {
+        shutdown_now = 0x4;
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::ROBOT_ID:
+      if (frame->DLC == 1) {
+        this->robotId = frame->data8[0];
+        return RDY_OK;
+      }
+      break;
+
+    default:
+      break;
+  }
+  return -1;
+}
+
+ThreadReference LightRing::start(tprio_t PRIO) {
+  this->ControllerAreaNetworkRx::start(PRIO + 1);
+  this->ControllerAreaNetworkTx::start(PRIO);
+  return NULL;
+}
+
+msg_t
+LightRing::terminate(void) {
+  msg_t ret = RDY_OK;
+
+  this->ControllerAreaNetworkTx::requestTerminate();
+  ret |= this->ControllerAreaNetworkTx::wait();
+  this->ControllerAreaNetworkRx::requestTerminate();
+  ret |= this->ControllerAreaNetworkRx::wait();
+
+  return ret;
+}
+
+void LightRing::periodicBroadcast() {
+
+}
diff --git a/devices/LightRing/LightRing.h b/devices/LightRing/LightRing.h
new file mode 100644
index 0000000..0001c8a
--- /dev/null
+++ b/devices/LightRing/LightRing.h
@@ -0,0 +1,42 @@
+#ifndef _AMIRO_LIGTH_RING_H_
+#define _AMIRO_LIGTH_RING_H_
+
+#include <amiro/Constants.h>
+
+#include <amiro/ControllerAreaNetworkRx.h>
+#include <amiro/ControllerAreaNetworkTx.h>
+#include <amiro/leds/tlc5947.hpp>
+#include <amiro/FileSystemInputOutput/FSIOLightRing.hpp>
+
+using namespace chibios_rt;
+
+namespace amiro {
+
+  class LightRing : public ControllerAreaNetworkTx, public ControllerAreaNetworkRx {
+  public:
+    LightRing(CANDriver *can, TLC5947 *tlc5947, fileSystemIo::FSIOLightRing *memory);
+    void setLightBrightness(int brightness);
+    void setLightColor(int index, Color color);
+
+    /** \brief Handle the termination of ControllerAreaNetworkTx and ControllerAreaNetworkRx threads
+     *         This includes waiting until the threads have terminated, thus this function might block a relatively long time.
+     */
+    msg_t terminate(void);
+
+    /** \brief Handle the start of ControllerAreaNetworkTx and ControllerAreaNetworkRx threads */
+    ThreadReference start(tprio_t PRIO);
+
+  protected:
+    virtual msg_t receiveMessage(CANRxFrame *frame);
+    virtual void periodicBroadcast();
+
+  private:
+    TLC5947 *tlc5947;
+    fileSystemIo::FSIOLightRing *memory;
+  };
+
+}
+
+#endif /* _AMIRO_LIGTH_RING_H_ */
+
+/** @} */
diff --git a/devices/LightRing/Makefile b/devices/LightRing/Makefile
new file mode 100644
index 0000000..c01caf6
--- /dev/null
+++ b/devices/LightRing/Makefile
@@ -0,0 +1,249 @@
+##############################################################################
+# Build global options
+# NOTE: Can be overridden externally.
+#
+
+# Compiler options here.
+ifeq ($(USE_OPT),)
+  USE_OPT = -O2 -fomit-frame-pointer -falign-functions=16 -DCHPRINTF_USE_FLOAT=1 -fstack-usage -DSERIAL_BUFFERS_SIZE=64
+endif
+
+# C specific options here (added to USE_OPT).
+ifeq ($(USE_COPT),)
+  USE_COPT =
+endif
+
+# C++ specific options here (added to USE_OPT).
+ifeq ($(USE_CPPOPT),)
+  USE_CPPOPT = -fno-rtti -fno-exceptions -std=c++11 -U__STRICT_ANSI__
+endif
+
+# Enable this if you want the linker to remove unused code and data
+ifeq ($(USE_LINK_GC),)
+  USE_LINK_GC = yes
+endif
+
+# Linker extra options here.
+ifeq ($(USE_LDOPT),)
+  USE_LDOPT =
+endif
+
+# Enable this if you want link time optimizations (LTO)
+ifeq ($(USE_LTO),)
+  USE_LTO = no
+endif
+
+# If enabled, this option allows to compile the application in THUMB mode.
+ifeq ($(USE_THUMB),)
+  USE_THUMB = yes
+endif
+
+# Enable this if you want to see the full log while compiling.
+ifeq ($(USE_VERBOSE_COMPILE),)
+  USE_VERBOSE_COMPILE = no
+endif
+
+#
+# Build global options
+##############################################################################
+
+##############################################################################
+# Architecture or project specific options
+#
+
+# Enable this if you really want to use the STM FWLib.
+ifeq ($(USE_FWLIB),)
+  USE_FWLIB = no
+endif
+
+#
+# Architecture or project specific options
+##############################################################################
+
+##############################################################################
+# Project, sources and paths
+#
+
+# Define project name here
+PROJECT = LightRing
+
+# Imported source files and paths
+CHIBIOS = ../../../ChibiOS
+AMIRO = ../..
+include $(AMIRO)/boards/LightRing/board.mk
+include $(CHIBIOS)/os/hal/platforms/STM32F1xx/platform.mk
+include $(CHIBIOS)/os/hal/hal.mk
+include $(CHIBIOS)/os/ports/GCC/ARMCMx/STM32F1xx/port.mk
+include $(CHIBIOS)/os/kernel/kernel.mk
+include $(CHIBIOS)/os/various/cpp_wrappers/kernel.mk
+
+# Define linker script file here
+LDSCRIPT= $(BOARDLD)/STM32F103xE.ld
+
+# C sources that can be compiled in ARM or THUMB mode depending on the global
+# setting.
+CSRC = $(PORTSRC) \
+       $(KERNSRC) \
+       $(HALSRC) \
+       $(PLATFORMSRC) \
+       $(BOARDSRC) \
+       $(CHIBIOS)/os/various/evtimer.c \
+       $(CHIBIOS)/os/various/syscalls.c \
+       $(CHIBIOS)/os/various/chprintf.c \
+       $(CHIBIOS)/os/various/shell.c \
+       $(CHIBIOS)/os/various/memstreams.c \
+       $(AMIRO)/stubs.c \
+       $(AMIRO)/components/Debug.c
+
+# C++ sources that can be compiled in ARM or THUMB mode depending on the global
+# setting.
+CPPSRC = $(CHCPPSRC) \
+         $(AMIRO)/components/Color.cpp \
+         $(AMIRO)/components/bus/spi/HWSPIDriver.cpp \
+         $(AMIRO)/components/bus/i2c/HWI2CDriver.cpp \
+         $(AMIRO)/components/leds/tlc5947.cpp \
+         $(AMIRO)/components/radio/a2500r24a.cpp \
+         $(AMIRO)/components/eeprom/eeprom.cpp \
+         $(AMIRO)/components/eeprom/at24.cpp \
+         $(AMIRO)/components/FileSystemInputOutput/FileSystemInputOutputBase.cpp \
+         $(AMIRO)/components/FileSystemInputOutput/FSIOLightRing.cpp \
+         $(AMIRO)/components/ControllerAreaNetworkRx.cpp \
+         $(AMIRO)/components/ControllerAreaNetworkTx.cpp \
+         $(AMIRO)/components/Lidar.cpp \
+         $(AMIRO)/components/serial_reset/serial_can_mux.cpp \
+         LightRing.cpp \
+         userthread.cpp \
+         main.cpp \
+         exti.cpp
+
+# C sources to be compiled in ARM mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+ACSRC =
+
+# C++ sources to be compiled in ARM mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+ACPPSRC =
+
+# C sources to be compiled in THUMB mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+TCSRC =
+
+# C sources to be compiled in THUMB mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+TCPPSRC =
+
+# List ASM source files here
+ASMSRC = $(PORTASM)
+
+INCDIR = $(PORTINC) $(KERNINC) $(TESTINC) \
+         $(HALINC) $(PLATFORMINC) $(BOARDINC) \
+         $(CHCPPINC) \
+         $(CHIBIOS)/os/various \
+         $(AMIRO) \
+         $(AMIRO)/include
+
+#
+# Project, sources and paths
+##############################################################################
+
+##############################################################################
+# Compiler settings
+#
+
+MCU  = cortex-m3
+
+#TRGT = arm-elf-
+TRGT = arm-none-eabi-
+CC   = $(TRGT)gcc
+CPPC = $(TRGT)g++
+# Enable loading with g++ only if you need C++ runtime support.
+# NOTE: You can use C++ even without C++ support if you are careful. C++
+#       runtime support makes code size explode.
+#LD   = $(TRGT)gcc
+LD   = $(TRGT)g++
+CP   = $(TRGT)objcopy
+AS   = $(TRGT)gcc -x assembler-with-cpp
+OD   = $(TRGT)objdump
+SZ   = $(TRGT)size
+HEX  = $(CP) -O ihex
+BIN  = $(CP) -O binary
+SREC = $(CP) -O srec --srec-len=248
+
+# ARM-specific options here
+AOPT =
+
+# THUMB-specific options here
+TOPT = -mthumb -DTHUMB
+
+# Define C warning options here
+CWARN = -Wall -Wextra -Wstrict-prototypes
+
+# Define C++ warning options here
+CPPWARN = -Wall -Wextra
+
+#
+# Compiler settings
+##############################################################################
+
+##############################################################################
+# Start of default section
+#
+
+# List all default C defines here, like -D_DEBUG=1
+DDEFS =
+
+# List all default ASM defines here, like -D_DEBUG=1
+DADEFS =
+
+# List all default directories to look for include files here
+DINCDIR =
+
+# List the default directory to look for the libraries here
+DLIBDIR =
+
+# List all default libraries here
+DLIBS =
+
+#
+# End of default section
+##############################################################################
+
+##############################################################################
+# Start of user section
+#
+
+# List all user C define here, like -D_DEBUG=1
+UDEFS =
+
+# Define ASM defines here
+UADEFS =
+
+# List all user directories here
+UINCDIR =
+
+# List the user directory to look for the libraries here
+ULIBDIR =
+
+# List all user libraries here
+ULIBS =
+
+#
+# End of user defines
+##############################################################################
+
+ifeq ($(USE_FWLIB),yes)
+  include $(CHIBIOS)/ext/stm32lib/stm32lib.mk
+  CSRC += $(STM32SRC)
+  INCDIR += $(STM32INC)
+  USE_OPT += -DUSE_STDPERIPH_DRIVER
+endif
+
+RULESPATH = $(CHIBIOS)/os/ports/GCC/ARMCMx
+include $(RULESPATH)/rules.mk
+
+include $(AMIRO)/ports/rules.mk
+include $(AMIRO)/devices/flash.mk
diff --git a/devices/LightRing/amiroosconf.h b/devices/LightRing/amiroosconf.h
new file mode 100644
index 0000000..4bb1b27
--- /dev/null
+++ b/devices/LightRing/amiroosconf.h
@@ -0,0 +1,19 @@
+#ifndef AMIRO_AMIROOSCONF_H_
+#define AMIRO_AMIROOSCONF_H_
+
+/**
+ * @brief Stack size of the user thread object
+ */
+#if !defined(USER_THREAD_STACK_SIZE) || defined(__DOXYGEN__)
+#define USER_THREAD_STACK_SIZE         1024
+#endif
+
+/**
+ * @brief Flag to enable/disable als amiroDbgXXX macros
+ */
+#if !defined(AMIRO_DBG) || defined(__DOXYGEN__)
+#define AMIRO_DBG                      TRUE
+#endif
+
+#endif // AMIRO_AMIROOSCONF_H_
+
diff --git a/devices/LightRing/chconf.h b/devices/LightRing/chconf.h
new file mode 100644
index 0000000..75c019e
--- /dev/null
+++ b/devices/LightRing/chconf.h
@@ -0,0 +1,507 @@
+#ifndef _CHCONF_H_
+#define _CHCONF_H_
+
+/*===========================================================================*/
+/**
+ * @name Kernel parameters and options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   System tick frequency.
+ * @details Frequency of the system timer that drives the system ticks. This
+ *          setting also defines the system tick time unit.
+ */
+#if !defined(CH_FREQUENCY) || defined(__DOXYGEN__)
+#define CH_FREQUENCY                    1000
+#endif
+
+/**
+ * @brief   Round robin interval.
+ * @details This constant is the number of system ticks allowed for the
+ *          threads before preemption occurs. Setting this value to zero
+ *          disables the preemption for threads with equal priority and the
+ *          round robin becomes cooperative. Note that higher priority
+ *          threads can still preempt, the kernel is always preemptive.
+ *
+ * @note    Disabling the round robin preemption makes the kernel more compact
+ *          and generally faster.
+ */
+#if !defined(CH_TIME_QUANTUM) || defined(__DOXYGEN__)
+#define CH_TIME_QUANTUM                 20
+#endif
+
+/**
+ * @brief   Managed RAM size.
+ * @details Size of the RAM area to be managed by the OS. If set to zero
+ *          then the whole available RAM is used. The core memory is made
+ *          available to the heap allocator and/or can be used directly through
+ *          the simplified core memory allocator.
+ *
+ * @note    In order to let the OS manage the whole RAM the linker script must
+ *          provide the @p __heap_base__ and @p __heap_end__ symbols.
+ * @note    Requires @p CH_USE_MEMCORE.
+ */
+#if !defined(CH_MEMCORE_SIZE) || defined(__DOXYGEN__)
+#define CH_MEMCORE_SIZE                 0
+#endif
+
+/**
+ * @brief   Idle thread automatic spawn suppression.
+ * @details When this option is activated the function @p chSysInit()
+ *          does not spawn the idle thread automatically. The application has
+ *          then the responsibility to do one of the following:
+ *          - Spawn a custom idle thread at priority @p IDLEPRIO.
+ *          - Change the main() thread priority to @p IDLEPRIO then enter
+ *            an endless loop. In this scenario the @p main() thread acts as
+ *            the idle thread.
+ *          .
+ * @note    Unless an idle thread is spawned the @p main() thread must not
+ *          enter a sleep state.
+ */
+#if !defined(CH_NO_IDLE_THREAD) || defined(__DOXYGEN__)
+#define CH_NO_IDLE_THREAD               FALSE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Performance options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   OS optimization.
+ * @details If enabled then time efficient rather than space efficient code
+ *          is used when two possible implementations exist.
+ *
+ * @note    This is not related to the compiler optimization options.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_OPTIMIZE_SPEED) || defined(__DOXYGEN__)
+#define CH_OPTIMIZE_SPEED               TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Subsystem options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Threads registry APIs.
+ * @details If enabled then the registry APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_REGISTRY) || defined(__DOXYGEN__)
+#define CH_USE_REGISTRY                 TRUE
+#endif
+
+/**
+ * @brief   Threads synchronization APIs.
+ * @details If enabled then the @p chThdWait() function is included in
+ *          the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_WAITEXIT) || defined(__DOXYGEN__)
+#define CH_USE_WAITEXIT                 TRUE
+#endif
+
+/**
+ * @brief   Semaphores APIs.
+ * @details If enabled then the Semaphores APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_SEMAPHORES) || defined(__DOXYGEN__)
+#define CH_USE_SEMAPHORES               TRUE
+#endif
+
+/**
+ * @brief   Semaphores queuing mode.
+ * @details If enabled then the threads are enqueued on semaphores by
+ *          priority rather than in FIFO order.
+ *
+ * @note    The default is @p FALSE. Enable this if you have special requirements.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_SEMAPHORES_PRIORITY) || defined(__DOXYGEN__)
+#define CH_USE_SEMAPHORES_PRIORITY      FALSE
+#endif
+
+/**
+ * @brief   Atomic semaphore API.
+ * @details If enabled then the semaphores the @p chSemSignalWait() API
+ *          is included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_SEMSW) || defined(__DOXYGEN__)
+#define CH_USE_SEMSW                    TRUE
+#endif
+
+/**
+ * @brief   Mutexes APIs.
+ * @details If enabled then the mutexes APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MUTEXES) || defined(__DOXYGEN__)
+#define CH_USE_MUTEXES                  TRUE
+#endif
+
+/**
+ * @brief   Conditional Variables APIs.
+ * @details If enabled then the conditional variables APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_MUTEXES.
+ */
+#if !defined(CH_USE_CONDVARS) || defined(__DOXYGEN__)
+#define CH_USE_CONDVARS                 TRUE
+#endif
+
+/**
+ * @brief   Conditional Variables APIs with timeout.
+ * @details If enabled then the conditional variables APIs with timeout
+ *          specification are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_CONDVARS.
+ */
+#if !defined(CH_USE_CONDVARS_TIMEOUT) || defined(__DOXYGEN__)
+#define CH_USE_CONDVARS_TIMEOUT         TRUE
+#endif
+
+/**
+ * @brief   Events Flags APIs.
+ * @details If enabled then the event flags APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_EVENTS) || defined(__DOXYGEN__)
+#define CH_USE_EVENTS                   TRUE
+#endif
+
+/**
+ * @brief   Events Flags APIs with timeout.
+ * @details If enabled then the events APIs with timeout specification
+ *          are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_EVENTS.
+ */
+#if !defined(CH_USE_EVENTS_TIMEOUT) || defined(__DOXYGEN__)
+#define CH_USE_EVENTS_TIMEOUT           TRUE
+#endif
+
+/**
+ * @brief   Synchronous Messages APIs.
+ * @details If enabled then the synchronous messages APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MESSAGES) || defined(__DOXYGEN__)
+#define CH_USE_MESSAGES                 TRUE
+#endif
+
+/**
+ * @brief   Synchronous Messages queuing mode.
+ * @details If enabled then messages are served by priority rather than in
+ *          FIFO order.
+ *
+ * @note    The default is @p FALSE. Enable this if you have special requirements.
+ * @note    Requires @p CH_USE_MESSAGES.
+ */
+#if !defined(CH_USE_MESSAGES_PRIORITY) || defined(__DOXYGEN__)
+#define CH_USE_MESSAGES_PRIORITY        FALSE
+#endif
+
+/**
+ * @brief   Mailboxes APIs.
+ * @details If enabled then the asynchronous messages (mailboxes) APIs are
+ *          included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_MAILBOXES) || defined(__DOXYGEN__)
+#define CH_USE_MAILBOXES                TRUE
+#endif
+
+/**
+ * @brief   I/O Queues APIs.
+ * @details If enabled then the I/O queues APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_QUEUES) || defined(__DOXYGEN__)
+#define CH_USE_QUEUES                   TRUE
+#endif
+
+/**
+ * @brief   Core Memory Manager APIs.
+ * @details If enabled then the core memory manager APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MEMCORE) || defined(__DOXYGEN__)
+#define CH_USE_MEMCORE                  TRUE
+#endif
+
+/**
+ * @brief   Heap Allocator APIs.
+ * @details If enabled then the memory heap allocator APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_MEMCORE and either @p CH_USE_MUTEXES or
+ *          @p CH_USE_SEMAPHORES.
+ * @note    Mutexes are recommended.
+ */
+#if !defined(CH_USE_HEAP) || defined(__DOXYGEN__)
+#define CH_USE_HEAP                     TRUE
+#endif
+
+/**
+ * @brief   C-runtime allocator.
+ * @details If enabled the the heap allocator APIs just wrap the C-runtime
+ *          @p malloc() and @p free() functions.
+ *
+ * @note    The default is @p FALSE.
+ * @note    Requires @p CH_USE_HEAP.
+ * @note    The C-runtime may or may not require @p CH_USE_MEMCORE, see the
+ *          appropriate documentation.
+ */
+#if !defined(CH_USE_MALLOC_HEAP) || defined(__DOXYGEN__)
+#define CH_USE_MALLOC_HEAP              FALSE
+#endif
+
+/**
+ * @brief   Memory Pools Allocator APIs.
+ * @details If enabled then the memory pools allocator APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MEMPOOLS) || defined(__DOXYGEN__)
+#define CH_USE_MEMPOOLS                 TRUE
+#endif
+
+/**
+ * @brief   Dynamic Threads APIs.
+ * @details If enabled then the dynamic threads creation APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_WAITEXIT.
+ * @note    Requires @p CH_USE_HEAP and/or @p CH_USE_MEMPOOLS.
+ */
+#if !defined(CH_USE_DYNAMIC) || defined(__DOXYGEN__)
+#define CH_USE_DYNAMIC                  TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Debug options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Debug option, system state check.
+ * @details If enabled the correct call protocol for system APIs is checked
+ *          at runtime.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_SYSTEM_STATE_CHECK) || defined(__DOXYGEN__)
+#define CH_DBG_SYSTEM_STATE_CHECK       TRUE
+#endif
+
+/**
+ * @brief   Debug option, parameters checks.
+ * @details If enabled then the checks on the API functions input
+ *          parameters are activated.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_CHECKS) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_CHECKS            TRUE
+#endif
+
+/**
+ * @brief   Debug option, consistency checks.
+ * @details If enabled then all the assertions in the kernel code are
+ *          activated. This includes consistency checks inside the kernel,
+ *          runtime anomalies and port-defined checks.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_ASSERTS) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_ASSERTS           TRUE
+#endif
+
+/**
+ * @brief   Debug option, trace buffer.
+ * @details If enabled then the context switch circular trace buffer is
+ *          activated.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_TRACE) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_TRACE             TRUE
+#endif
+
+/**
+ * @brief   Debug option, stack checks.
+ * @details If enabled then a runtime stack check is performed.
+ *
+ * @note    The default is @p FALSE.
+ * @note    The stack check is performed in a architecture/port dependent way.
+ *          It may not be implemented or some ports.
+ * @note    The default failure mode is to halt the system with the global
+ *          @p panic_msg variable set to @p NULL.
+ */
+#if !defined(CH_DBG_ENABLE_STACK_CHECK) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_STACK_CHECK       TRUE
+#endif
+
+/**
+ * @brief   Debug option, stacks initialization.
+ * @details If enabled then the threads working area is filled with a byte
+ *          value when a thread is created. This can be useful for the
+ *          runtime measurement of the used stack.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_FILL_THREADS) || defined(__DOXYGEN__)
+#define CH_DBG_FILL_THREADS             FALSE
+#endif
+
+/**
+ * @brief   Debug option, threads profiling.
+ * @details If enabled then a field is added to the @p Thread structure that
+ *          counts the system ticks occurred while executing the thread.
+ *
+ * @note    The default is @p TRUE.
+ * @note    This debug option is defaulted to TRUE because it is required by
+ *          some test cases into the test suite.
+ */
+#if !defined(CH_DBG_THREADS_PROFILING) || defined(__DOXYGEN__)
+#define CH_DBG_THREADS_PROFILING        TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Kernel hooks
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Threads descriptor structure extension.
+ * @details User fields added to the end of the @p Thread structure.
+ */
+#if !defined(THREAD_EXT_FIELDS) || defined(__DOXYGEN__)
+#define THREAD_EXT_FIELDS                                                   \
+  /* Add threads custom fields here.*/
+#endif
+
+/**
+ * @brief   Threads initialization hook.
+ * @details User initialization code added to the @p chThdInit() API.
+ *
+ * @note    It is invoked from within @p chThdInit() and implicitly from all
+ *          the threads creation APIs.
+ */
+#if !defined(THREAD_EXT_INIT_HOOK) || defined(__DOXYGEN__)
+#define THREAD_EXT_INIT_HOOK(tp) {                                          \
+  /* Add threads initialization code here.*/                                \
+}
+#endif
+
+/**
+ * @brief   Threads finalization hook.
+ * @details User finalization code added to the @p chThdExit() API.
+ *
+ * @note    It is inserted into lock zone.
+ * @note    It is also invoked when the threads simply return in order to
+ *          terminate.
+ */
+#if !defined(THREAD_EXT_EXIT_HOOK) || defined(__DOXYGEN__)
+#define THREAD_EXT_EXIT_HOOK(tp) {                                          \
+  /* Add threads finalization code here.*/                                  \
+}
+#endif
+
+/**
+ * @brief   Context switch hook.
+ * @details This hook is invoked just before switching between threads.
+ */
+#if !defined(THREAD_CONTEXT_SWITCH_HOOK) || defined(__DOXYGEN__)
+#define THREAD_CONTEXT_SWITCH_HOOK(ntp, otp) {                              \
+  /* Context switch code here.*/                                            \
+}
+#endif
+
+/**
+ * @brief   Idle Loop hook.
+ * @details This hook is continuously invoked by the idle thread loop.
+ */
+#if !defined(IDLE_LOOP_HOOK) || defined(__DOXYGEN__)
+#define IDLE_LOOP_HOOK() {                                                  \
+  /* Idle loop code here.*/                                                 \
+}
+#endif
+
+/**
+ * @brief   System tick event hook.
+ * @details This hook is invoked in the system tick handler immediately
+ *          after processing the virtual timers queue.
+ */
+#if !defined(SYSTEM_TICK_EVENT_HOOK) || defined(__DOXYGEN__)
+#define SYSTEM_TICK_EVENT_HOOK() {                                          \
+  /* System tick event code here.*/                                         \
+}
+#endif
+
+/**
+ * @brief   System halt hook.
+ * @details This hook is invoked in case to a system halting error before
+ *          the system is halted.
+ */
+#if !defined(SYSTEM_HALT_HOOK) || defined(__DOXYGEN__)
+void haltErrorCode(void);
+#define SYSTEM_HALT_HOOK() {                                                \
+  /* System halt code here.*/                                               \
+  haltErrorCode();                                                          \
+}
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/* Port-specific settings (override port settings defaulted in chcore.h).    */
+/*===========================================================================*/
+
+/* NVIC VTOR initialization (only offset!) */
+#define CORTEX_VTOR_INIT 0x00006000
+
+#endif  /* _CHCONF_H_ */
diff --git a/devices/LightRing/exti.cpp b/devices/LightRing/exti.cpp
new file mode 100644
index 0000000..97132fb
--- /dev/null
+++ b/devices/LightRing/exti.cpp
@@ -0,0 +1,119 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <board.h>
+
+#include "exti.hpp"
+
+volatile uint32_t shutdown_now = 0x00000000u; // = BL_SHUTDOWN_NONE in main.cpp
+
+EXTConfig extcfg = {
+
+  {
+    /* channel 0 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 1 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 2 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 3 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 4 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 5 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 6 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 7 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 8 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 9 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 10 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 11 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 12 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 13 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 14 */
+    {
+      /* mode */ EXT_MODE_GPIOC | EXT_CH_MODE_AUTOSTART | EXT_CH_MODE_FALLING_EDGE,
+      /* cb   */ power_down_cb,
+    },
+    /* channel 15 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 16 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 17 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 18 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+  }
+};
+
+void power_down_cb(EXTDriver *extp, expchannel_t channel) {
+
+  (void) extp;
+  (void) channel;
+  // make sure we don't accidentally power down when somebody tries to wake us
+  if (!palReadPad(GPIOC, GPIOC_SYS_PD_N))
+    palWritePad(GPIOD, GPIOD_SYS_INT_N, PAL_LOW); // indicate that the module needs some time to shut down
+    shutdown_now = 5; // = SHUTDOWN_HANDLE_REQUEST in main.cpp
+}
diff --git a/devices/LightRing/exti.hpp b/devices/LightRing/exti.hpp
new file mode 100644
index 0000000..97b8683
--- /dev/null
+++ b/devices/LightRing/exti.hpp
@@ -0,0 +1,10 @@
+#ifndef EXT_HPP_
+#define EXT_HPP_
+
+extern volatile uint32_t shutdown_now;
+
+extern EXTConfig extcfg;
+
+void power_down_cb(EXTDriver *extp, expchannel_t channel);
+
+#endif /* EXT_HPP_ */
diff --git a/devices/LightRing/global.hpp b/devices/LightRing/global.hpp
new file mode 100644
index 0000000..7141df6
--- /dev/null
+++ b/devices/LightRing/global.hpp
@@ -0,0 +1,106 @@
+#ifndef AMIRO_GLOBAL_HPP_
+#define AMIRO_GLOBAL_HPP_
+
+#include <hal.h>
+
+#include <board.h>
+#include <amiro/bus/i2c/HWI2CDriver.hpp>
+#include <amiro/bus/spi/HWSPIDriver.hpp>
+#include <amiro/leds/tlc5947.hpp>
+#include <amiro/eeprom/at24.hpp>
+#include <amiro/FileSystemInputOutput/FSIOLightRing.hpp>
+#include <LightRing.h>
+#include <amiro/Lidar.h>
+#include <amiro/radio/a2500r24a.hpp>
+#include <amiro/serial_reset/serial_can_mux.hpp>
+#include <userthread.hpp>
+
+namespace amiro {
+
+class Global final
+{
+public:
+  I2CConfig i2c2_config{
+    /* I2C mode                 */ OPMODE_I2C,
+    /* frequency                */ 400000,
+    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2
+  };
+
+  SerialConfig sd1_config{
+    /* speed        */ 115200,
+    /* CR1 register */ 0,
+    /* CR2 register */ 0,
+    /* CR3 register */ 0
+  };
+  SerialConfig sd2_config{
+    /* speed        */ 19200,
+    /* CR1 register */ 0,
+    /* CR2 register */ 0,
+    /* CR3 register */ 0
+  };
+
+  SPIConfig spi1_config{
+    /* callback function pointer   */ NULL,
+    /* chip select line port       */ GPIOC,
+    /* chip select line pad number */ GPIOC_LIGHT_XLAT,
+    /* initialzation data          */ SPI_CR1_BR_0 | SPI_CR1_BR_1
+  };
+  SPIConfig spi2_config{
+    /* callback function pointer   */ NULL,
+    /* chip select line port       */ GPIOB,
+    /* chip select line pad number */ GPIOB_WL_SS_N,
+    /* initialzation data          */ SPI_CR1_BR_0
+  };
+
+  /**
+   * @brief I2C Bus 2
+   * Conected devices:
+   *   AT24Cxx
+   */
+  HWI2CDriver HW_I2C2;
+
+  HWSPIDriver HW_SPI1;
+  HWSPIDriver HW_SPI2;
+
+  TLC5947 tlc5947;
+
+  AT24 at24c01;
+  fileSystemIo::FSIOLightRing memory;
+
+  LightRing robot;
+
+  SerialCanMux sercanmux1;
+
+  Lidar lidar;
+
+  A2500R24A a2500r24a;
+
+  UserThread userThread;
+
+  uint8_t shellTermID;
+
+public:
+  Global() :
+    HW_I2C2(&I2CD2),
+    HW_SPI1(&SPID1, &spi1_config), HW_SPI2(&SPID2, &spi2_config),
+    tlc5947(&HW_SPI1, GPIOA, GPIOA_LIGHT_BLANK),
+    at24c01(0x400u / 0x08u, 0x08u, 500u, &HW_I2C2),
+    memory(at24c01, /*BMSV*/ 1, /*bmsv*/ 2, /*HMV*/ 1, /*hmv*/ 0),
+    robot(&CAND1, &tlc5947, &memory),
+    sercanmux1(&SD1, &CAND1, CAN::LIGHT_RING_ID),
+    lidar(CAN::LIGHT_RING_ID, Lidar::SETUP::POWER_ONLY),
+    a2500r24a(&HW_SPI2),
+    userThread()
+  {
+    return;
+  }
+
+  ~Global()
+  {
+    return;
+  }
+};
+
+} // end of namespace amiro
+
+#endif /* AMIRO_GLOBAL_HPP_ */
diff --git a/devices/LightRing/halconf.h b/devices/LightRing/halconf.h
new file mode 100644
index 0000000..b7d5889
--- /dev/null
+++ b/devices/LightRing/halconf.h
@@ -0,0 +1,283 @@
+#ifndef _HALCONF_H_
+#define _HALCONF_H_
+
+#include "mcuconf.h"
+
+/**
+ * @brief   Enables the TM subsystem.
+ */
+#if !defined(HAL_USE_TM) || defined(__DOXYGEN__)
+#define HAL_USE_TM                  TRUE
+#endif
+
+/**
+ * @brief   Enables the PAL subsystem.
+ */
+#if !defined(HAL_USE_PAL) || defined(__DOXYGEN__)
+#define HAL_USE_PAL                 TRUE
+#endif
+
+/**
+ * @brief   Enables the ADC subsystem.
+ */
+#if !defined(HAL_USE_ADC) || defined(__DOXYGEN__)
+#define HAL_USE_ADC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the CAN subsystem.
+ */
+#if !defined(HAL_USE_CAN) || defined(__DOXYGEN__)
+#define HAL_USE_CAN                 TRUE
+#endif
+
+/**
+ * @brief   Enables the EXT subsystem.
+ */
+#if !defined(HAL_USE_EXT) || defined(__DOXYGEN__)
+#define HAL_USE_EXT                 TRUE
+#endif
+
+/**
+ * @brief   Enables the GPT subsystem.
+ */
+#if !defined(HAL_USE_GPT) || defined(__DOXYGEN__)
+#define HAL_USE_GPT                 FALSE
+#endif
+
+/**
+ * @brief   Enables the I2C subsystem.
+ */
+#if !defined(HAL_USE_I2C) || defined(__DOXYGEN__)
+#define HAL_USE_I2C                 TRUE
+#endif
+
+/**
+ * @brief   Enables the ICU subsystem.
+ */
+#if !defined(HAL_USE_ICU) || defined(__DOXYGEN__)
+#define HAL_USE_ICU                 FALSE
+#endif
+
+/**
+ * @brief   Enables the MAC subsystem.
+ */
+#if !defined(HAL_USE_MAC) || defined(__DOXYGEN__)
+#define HAL_USE_MAC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the MMC_SPI subsystem.
+ */
+#if !defined(HAL_USE_MMC_SPI) || defined(__DOXYGEN__)
+#define HAL_USE_MMC_SPI             FALSE
+#endif
+
+/**
+ * @brief   Enables the PWM subsystem.
+ */
+#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__)
+#define HAL_USE_PWM                 FALSE
+#endif
+
+/**
+ * @brief   Enables the RTC subsystem.
+ */
+#if !defined(HAL_USE_RTC) || defined(__DOXYGEN__)
+#define HAL_USE_RTC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the SDC subsystem.
+ */
+#if !defined(HAL_USE_SDC) || defined(__DOXYGEN__)
+#define HAL_USE_SDC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the SERIAL subsystem.
+ */
+#if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__)
+#define HAL_USE_SERIAL              TRUE
+#endif
+
+/**
+ * @brief   Enables the SERIAL over USB subsystem.
+ */
+#if !defined(HAL_USE_SERIAL_USB) || defined(__DOXYGEN__)
+#define HAL_USE_SERIAL_USB          FALSE
+#endif
+
+/**
+ * @brief   Enables the SPI subsystem.
+ */
+#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__)
+#define HAL_USE_SPI                 TRUE
+#endif
+
+/**
+ * @brief   Enables the UART subsystem.
+ */
+#if !defined(HAL_USE_UART) || defined(__DOXYGEN__)
+#define HAL_USE_UART                FALSE
+#endif
+
+/**
+ * @brief   Enables the USB subsystem.
+ */
+#if !defined(HAL_USE_USB) || defined(__DOXYGEN__)
+#define HAL_USE_USB                 FALSE
+#endif
+
+/*===========================================================================*/
+/* ADC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables synchronous APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(ADC_USE_WAIT) || defined(__DOXYGEN__)
+#define ADC_USE_WAIT                TRUE
+#endif
+
+/**
+ * @brief   Enables the @p adcAcquireBus() and @p adcReleaseBus() APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(ADC_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define ADC_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+/*===========================================================================*/
+/* CAN driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Sleep mode related APIs inclusion switch.
+ */
+#if !defined(CAN_USE_SLEEP_MODE) || defined(__DOXYGEN__)
+#define CAN_USE_SLEEP_MODE          TRUE
+#endif
+
+/*===========================================================================*/
+/* I2C driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables the mutual exclusion APIs on the I2C bus.
+ */
+#if !defined(I2C_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define I2C_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+/*===========================================================================*/
+/* MAC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables an event sources for incoming packets.
+ */
+#if !defined(MAC_USE_ZERO_COPY) || defined(__DOXYGEN__)
+#define MAC_USE_ZERO_COPY           FALSE
+#endif
+
+/**
+ * @brief   Enables an event sources for incoming packets.
+ */
+#if !defined(MAC_USE_EVENTS) || defined(__DOXYGEN__)
+#define MAC_USE_EVENTS              TRUE
+#endif
+
+/*===========================================================================*/
+/* MMC_SPI driver related settings.                                          */
+/*===========================================================================*/
+
+/**
+ * @brief   Delays insertions.
+ * @details If enabled this options inserts delays into the MMC waiting
+ *          routines releasing some extra CPU time for the threads with
+ *          lower priority, this may slow down the driver a bit however.
+ *          This option is recommended also if the SPI driver does not
+ *          use a DMA channel and heavily loads the CPU.
+ */
+#if !defined(MMC_NICE_WAITING) || defined(__DOXYGEN__)
+#define MMC_NICE_WAITING            TRUE
+#endif
+
+/*===========================================================================*/
+/* SDC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Number of initialization attempts before rejecting the card.
+ * @note    Attempts are performed at 10mS intervals.
+ */
+#if !defined(SDC_INIT_RETRY) || defined(__DOXYGEN__)
+#define SDC_INIT_RETRY              100
+#endif
+
+/**
+ * @brief   Include support for MMC cards.
+ * @note    MMC support is not yet implemented so this option must be kept
+ *          at @p FALSE.
+ */
+#if !defined(SDC_MMC_SUPPORT) || defined(__DOXYGEN__)
+#define SDC_MMC_SUPPORT             FALSE
+#endif
+
+/**
+ * @brief   Delays insertions.
+ * @details If enabled this options inserts delays into the MMC waiting
+ *          routines releasing some extra CPU time for the threads with
+ *          lower priority, this may slow down the driver a bit however.
+ */
+#if !defined(SDC_NICE_WAITING) || defined(__DOXYGEN__)
+#define SDC_NICE_WAITING            TRUE
+#endif
+
+/*===========================================================================*/
+/* SERIAL driver related settings.                                           */
+/*===========================================================================*/
+
+/**
+ * @brief   Default bit rate.
+ * @details Configuration parameter, this is the baud rate selected for the
+ *          default configuration.
+ */
+#if !defined(SERIAL_DEFAULT_BITRATE) || defined(__DOXYGEN__)
+#define SERIAL_DEFAULT_BITRATE      38400
+#endif
+
+/**
+ * @brief   Serial buffers size.
+ * @details Configuration parameter, you can change the depth of the queue
+ *          buffers depending on the requirements of your application.
+ * @note    The default is 64 bytes for both the transmission and receive
+ *          buffers.
+ */
+#if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__)
+#define SERIAL_BUFFERS_SIZE         16
+#endif
+
+/*===========================================================================*/
+/* SPI driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables synchronous APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(SPI_USE_WAIT) || defined(__DOXYGEN__)
+#define SPI_USE_WAIT                TRUE
+#endif
+
+/**
+ * @brief   Enables the @p spiAcquireBus() and @p spiReleaseBus() APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(SPI_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define SPI_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+#endif /* _HALCONF_H_ */
diff --git a/devices/LightRing/main.cpp b/devices/LightRing/main.cpp
new file mode 100644
index 0000000..7f9f3c6
--- /dev/null
+++ b/devices/LightRing/main.cpp
@@ -0,0 +1,660 @@
+#define BL_CALLBACK_TABLE_ADDR  (0x08000000 + 0x01C0)
+#define BL_MAGIC_NUMBER         ((uint32_t)0xFF669900u)
+
+#define SHUTDOWN_NONE             0
+#define SHUTDOWN_TRANSPORTATION   1
+#define SHUTDOWN_DEEPSLEEP        2
+#define SHUTDOWN_HIBERNATE        3
+#define SHUTDOWN_RESTART          4
+#define SHUTDOWN_HANDLE_REQUEST   5
+
+#include <ch.hpp>
+
+#include "global.hpp"
+#include <amiro/util/util.h>
+#include <amiro/Color.h>
+#include <exti.hpp>
+
+#include <chprintf.h>
+#include <shell.h>
+
+using namespace amiro;
+using namespace chibios_rt;
+
+Global global;
+
+void shellRequestShutdown(BaseSequentialStream* chp, int argc, char *argv[]) {
+
+  chprintf(chp, "shellRequestShutdown\n");
+
+  /* if nor argument was given, print some help text */
+  if (argc == 0 || strcmp(argv[0], "help") == 0) {
+    chprintf(chp, "\tUSAGE:\n");
+    chprintf(chp, "> shutdown <type>\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\ttype\n");
+    chprintf(chp, "The type of shutdown to perform.\n");
+    chprintf(chp, "Choose one of the following types:\n");
+    chprintf(chp, "  transportation - Ultra low-power mode with all wakeups disabled.\n");
+    chprintf(chp, "                   The robot can not be charged.\n");
+    chprintf(chp, "  deepsleep      - Ultra low-power mode with several wakeups enabled.\n");
+    chprintf(chp, "                   The robot can only be charged via the power plug.\n");
+    chprintf(chp, "  hibernate      - Medium low-power mode, but with full charging capabilities.\n");
+    chprintf(chp, "  restart        - Performs a system restart.\n");
+    chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
+    chprintf(chp, "\n");
+    return;
+  }
+
+  if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) {
+    shutdown_now = SHUTDOWN_TRANSPORTATION;
+    chprintf(chp, "shutdown to transportation mode initialized\n");
+  } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) {
+    shutdown_now = SHUTDOWN_DEEPSLEEP;
+    chprintf(chp, "shutdown to deepsleep mode initialized\n");
+  } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) {
+    shutdown_now = SHUTDOWN_HIBERNATE;
+    chprintf(chp, "shutdown to hibernate mode initialized\n");
+  } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) {
+    chprintf(chp, "restart initialized\n");
+    shutdown_now = SHUTDOWN_RESTART;
+  } else {
+    chprintf(chp, "ERROR: unknown argument!\n");
+    shutdown_now = SHUTDOWN_NONE;
+  }
+
+  return;
+}
+
+void testLights() {
+  for (int i = 0; i < (8 * 16 * 2); i++) {
+    Color color;
+    int brightness;
+
+    switch (((i / 16) / 2) % 8) {
+      case 0:
+        color = Color::WHITE;
+        break;
+      case 1:
+        color = Color::RED;
+        break;
+      case 2:
+        color = Color::FUCHSIA;
+        break;
+      case 3:
+        color = Color::BLUE;
+        break;
+      case 4:
+        color = Color::AQUA;
+        break;
+      case 5:
+        color = Color::LIME;
+        break;
+      case 6:
+        color = Color::YELLOW;
+        break;
+      case 7:
+      default:
+        color = Color::BLACK;
+        break;
+    }
+
+    if (i & (1 << 4))
+      brightness = 10 + (i % 16) * 6;
+    else
+      brightness = 10;
+
+    global.robot.setLightBrightness(brightness);
+    global.robot.setLightColor((i / 2) % 8, color);
+
+    BaseThread::sleep(MS2ST(250));
+  }
+}
+
+void testColors() {
+  Color color;
+
+  global.robot.setLightBrightness(50);
+
+  for (int i = 0; i < Color::YELLOWGREEN; i++) {
+    color = Color(static_cast<Color::GlobalColor>(i));
+    for (int j = 0; j < 8; j++)
+      global.robot.setLightColor(j, color);
+
+    BaseThread::sleep(MS2ST(250));
+  }
+
+}
+
+void testFade() {
+  for (int i = 0; i < 255; i += 16) {
+    Color color(255 - i, i, 0);
+    for (int j = 0; j < 8; j++) {
+      global.robot.setLightColor(j, color);
+      BaseThread::sleep(MS2ST(250));
+    }
+  }
+}
+
+void testBrightness() {
+  for (int j = 0; j < 8; j++)
+    global.robot.setLightColor(j, Color::WHITE);
+
+  for (int i = 0; i < 200; i += 5) {
+    int brightness = (i > 100) ? 200 - i : i;
+    global.robot.setLightBrightness(brightness);
+    BaseThread::sleep(MS2ST(100));
+  }
+}
+
+static msg_t shutdownRequest(void *arg) {
+
+  (void) arg;
+  const size_t max_str_len = strlen("shutdown");
+  uint8_t buffer[20];
+  uint8_t num_bytes = 0x00u;
+  uint8_t consume;
+
+  while (true) {
+
+    num_bytes += sdAsynchronousRead(&SD1, &buffer[num_bytes], sizeof(buffer) - num_bytes);
+
+    if (num_bytes) {
+
+      consume = 0x01u;
+
+      if (num_bytes >= strlen("shutdown") && !strncmp((char*) buffer, "shutdown", strlen("shutdown"))) {
+
+        boardRequestShutdown();
+        consume = strlen("shutdown");
+
+      }
+
+      if (consume != 0x01 || num_bytes >= max_str_len) {
+
+        num_bytes -= consume;
+        memcpy(&buffer[0], &buffer[consume], num_bytes);
+
+      }
+
+    }
+
+    BaseThread::sleep(MS2ST(1000));
+
+  }
+
+  return RDY_OK;
+
+}
+
+void systemShutdown() {
+
+  global.userThread.requestTerminate();
+  global.userThread.wait();
+
+  global.robot.setLightBrightness(0);
+  global.robot.terminate();
+
+  global.tlc5947.requestTerminate();
+  global.tlc5947.update();
+  global.tlc5947.wait();
+
+  global.lidar.requestTerminate();
+  global.lidar.wait();
+
+//  boardStandby();
+
+  return;
+}
+
+void shellRequestResetMemory(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestInitMemory\n");
+
+  msg_t res = global.memory.resetMemory();
+  if ( res != global.memory.OK)
+    chprintf(chp, "Memory Init: FAIL\n");
+  else
+    chprintf(chp, "Memory Init: OK\n");
+}
+
+void shellRequestGetBoardId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestGetBoardId\n");
+
+  uint8_t id = 0xFFu;
+  msg_t res = global.memory.getBoardId(&id);
+  if (res != global.memory.OK)
+    chprintf(chp, "Get Board ID: FAIL\n");
+  else
+    chprintf(chp, "Get Board ID: %u\n", id);
+}
+
+void shellRequestSetBoardId(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestSetBoardId\n");
+
+  if (argc == 0) {
+    chprintf(chp, "Usage: %s\n","set_board_id <idx>");
+  } else {
+    msg_t res = global.memory.setBoardId(atoi(argv[0]));
+    if (res != global.memory.OK)
+      chprintf(chp, "Set Board ID: FAIL\n");
+    else
+      chprintf(chp, "Set Board ID: OK\n");
+  }
+}
+
+void boardPeripheryCheck(BaseSequentialStream *chp) {
+  msg_t result;
+  chprintf(chp, "\nCHECK: START\n");
+
+  // Check the radio
+  result = global.a2500r24a.getCheck();
+  if (result == global.a2500r24a.CHECK_OK)
+    chprintf(chp, "A2500R24A: OK\n");
+  else
+    chprintf(chp, "A2500R24A: FAIL\n");
+
+  // Check the eeprom
+  result = global.memory.getCheck();
+  if ( result != global.memory.OK)
+    chprintf(chp, "Memory Structure: FAIL\n");
+  else
+    chprintf(chp, "Memory Structure: OK\n");
+
+  // Check the lights
+  chprintf(chp, "LEDs: Test colors\n");
+  testColors();
+  chprintf(chp, "LEDs: Off\n");
+  global.robot.setLightBrightness(0);
+
+  chprintf(chp, "CHECK: FINISH\n");
+}
+
+void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) {
+
+  enum Type {HEX, U8, U16, U32, S8, S16, S32};
+
+  chprintf(chp, "shellRequestReadData\n");
+
+  if (argc < 2 || strcmp(argv[0],"help") == 0)
+  {
+    chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]");
+    chprintf(chp, "\n");
+    chprintf(chp, "\ttype\n");
+    chprintf(chp, "The data type as which to interpret the data.\n");
+    chprintf(chp, "Choose one of the following types:\n");
+    chprintf(chp, "  hex - one byte as hexadecimal value\n");
+    chprintf(chp, "  u8  - unsigned integer (8 bit)\n");
+    chprintf(chp, "  u16 - unsigned integer (16 bit)\n");
+    chprintf(chp, "  u32 - unsigned integer (32 bit)\n");
+    chprintf(chp, "  s8  - signed integer (8 bit)\n");
+    chprintf(chp, "  s16 - signed integer (16 bit)\n");
+    chprintf(chp, "  s32 - signed integer (32 bit)\n");
+    chprintf(chp, "\tstart\n");
+    chprintf(chp, "The first byte to read from the memory.\n");
+    chprintf(chp, "\tcount [default = 1]\n");
+    chprintf(chp, "The number of elements to read.\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\tNOTE\n");
+    chprintf(chp, "Type conversions of this function might fail.\n");
+    chprintf(chp, "If so, use type=hex and convert by hand.\n");
+    chprintf(chp, "\n");
+    return;
+  }
+
+  uint8_t type_size = 0;
+  Type type = HEX;
+  if (strcmp(argv[0],"hex") == 0) {
+    type_size = sizeof(unsigned char);
+    type = HEX;
+  } else if(strcmp(argv[0],"u8") == 0) {
+    type_size = sizeof(uint8_t);
+    type = U8;
+  } else if(strcmp(argv[0],"u16") == 0) {
+    type_size = sizeof(uint16_t);
+    type = U16;
+  } else if(strcmp(argv[0],"u32") == 0) {
+    type_size = sizeof(uint32_t);
+    type = U32;
+  } else if(strcmp(argv[0],"s8") == 0) {
+    type_size = sizeof(int8_t);
+    type = S8;
+  } else if(strcmp(argv[0],"s16") == 0) {
+    type_size = sizeof(int16_t);
+    type = S16;
+  } else if(strcmp(argv[0],"s32") == 0) {
+    type_size = sizeof(int32_t);
+    type = S32;
+  } else {
+    chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
+    return;
+  }
+
+  unsigned int start_byte = atoi(argv[1]);
+
+  unsigned int num_elements = 1;
+  if (argc >= 3)
+    num_elements = atoi(argv[2]);
+
+  const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
+  uint8_t buffer[eeprom_size];
+  if (start_byte + (type_size * num_elements) > eeprom_size) {
+    num_elements = (eeprom_size - start_byte) / type_size;
+    chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
+  }
+
+  chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte);
+
+  // Work around, because stm32f1 cannot read a single byte
+  if (type_size*num_elements < 2)
+    type_size = 2;
+
+  uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements);
+
+  if (bytes_read != type_size*num_elements)
+    chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);
+
+  for (unsigned int i = 0; i < num_elements; ++i) {
+    switch (type) {
+      case HEX:
+        chprintf(chp, "%02X ", buffer[i]);
+        break;
+      case U8:
+        chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
+        break;
+      case U16:
+        chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
+        break;
+      case U32:
+        chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
+        break;
+      case S8:
+        chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
+        break;
+      case S16:
+        chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
+        break;
+      case S32:
+        chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
+        break;
+      default:
+        break;
+    }
+  }
+  chprintf(chp, "\n");
+
+  return;
+
+}
+
+void shellRequestCheck(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestCheck\n");
+  boardPeripheryCheck(chp);
+}
+
+void shellRequestGetRobotId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestGetRobotId\n");
+  chprintf(chp, "Robot ID: %u\n", global.robot.getRobotID());
+  if (global.robot.getRobotID() == 0)
+    chprintf(chp, "Warning: The ID seems to be uninitialized. Is CAN communication working correctly?\n");
+}
+
+void shellRequestGetSystemLoad(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestGetSystemLoad\n");
+  uint8_t seconds = 1;
+  if (argc >= 1) {
+    seconds = atoi(argv[0]);
+  }
+  chprintf(chp, "measuring CPU load for %u %s...\n", seconds, (seconds>1)? "seconds" : "second");
+
+  const systime_t before = chThdGetTicks(chSysGetIdleThread());
+  BaseThread::sleep(S2ST(seconds));
+  const systime_t after = chThdGetTicks(chSysGetIdleThread());
+  const float usage = 1.0f - (float(after - before) / float(seconds * CH_FREQUENCY));
+
+  chprintf(chp, "CPU load: %3.2f%%\n", usage * 100);
+  const uint32_t memory_total = 0x10000;
+  const uint32_t memory_load = memory_total - chCoreStatus();
+  chprintf(chp, "RAM load: %3.2f%% (%u / %u Byte)\n", float(memory_load)/float(memory_total) * 100, memory_load, memory_total);
+}
+
+void shellSwitchBoardCmd(BaseSequentialStream *chp, int argc, char *argv[]) {
+  if (argc != 1) {
+    chprintf(chp, "Call with decimal numbers: shell_board <idx>\n");
+    return;
+  }
+  uint8_t boardIdx = static_cast<uint8_t>(atoi(argv[0]));
+
+  chprintf(chp, "shellSwitchBoardCmd\n");
+  global.sercanmux1.sendSwitchCmd(boardIdx);
+}
+
+void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *argv[]) {
+  // check the magic number
+  if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) {
+    chprintf(chp, "Bootloader version:  %u.%u.%u\n",
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (3*4))));
+    chprintf(chp, "Callback functions:\n");if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2) {
+      chprintf(chp, "\thibernate:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (4*4))) ? "available" : "unsupported");
+      chprintf(chp, "\tdeepsleep:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (5*4))) ? "available" : "unsupported");
+      chprintf(chp, "\ttransportation: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (6*4))) ? "available" : "unsupported");
+      chprintf(chp, "\trestart:        %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (10*4))) ? "available" : "unsupported");
+    } else if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3) {
+      chprintf(chp, "\thibernate:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (4*4))) ? "available" : "unsupported");
+      chprintf(chp, "\tdeepsleep:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (5*4))) ? "available" : "unsupported");
+      chprintf(chp, "\ttransportation: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (6*4))) ? "available" : "unsupported");
+      chprintf(chp, "\trestart:        %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (7*4))) ? "available" : "unsupported");
+      chprintf(chp, "\thandle request: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (8*4))) ? "available" : "unsupported");
+    }
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader deprecated\n");
+  }
+
+  return;
+}
+
+static const ShellCommand commands[] = {
+  {"shutdown", shellRequestShutdown},
+  {"check", shellRequestCheck},
+  {"reset_memory", shellRequestResetMemory},
+  {"get_board_id", shellRequestGetBoardId},
+  {"set_board_id", shellRequestSetBoardId},
+  {"get_memory_data", shellRequestGetMemoryData},
+  {"get_robot_id", shellRequestGetRobotId},
+  {"get_system_load", shellRequestGetSystemLoad},
+  {"shell_board", shellSwitchBoardCmd},
+  {"get_bootloader_info", shellRequestGetBootloaderInfo},
+  {NULL, NULL}
+};
+
+static const ShellConfig shell_cfg1 = {
+  (BaseSequentialStream *) &global.sercanmux1,
+  commands
+};
+
+
+
+/*
+ * Application entry point.
+ */
+int main(void) {
+
+  Thread *shelltp = NULL;
+//  global.shellTermID = CAN::LIGHT_RING_ID;
+  /*
+   * System initializations.
+   * - HAL initialization, this also initializes the configured device drivers
+   *   and performs the board-specific initializations.
+   * - Kernel initialization, the main() function becomes a thread and the
+   *   RTOS is active.
+   */
+  halInit();
+  System::init();
+
+  /*
+   * Activates the serial driver 2 using the driver default configuration.
+   */
+  sdStart(&SD1, &global.sd1_config);
+  sdStart(&SD2, &global.sd2_config);
+
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+  chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n");
+  if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (3*4))));
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader deprecated\n");
+  }
+  chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n");
+  // make sure that the info text is completetly printed
+  BaseThread::sleep(10);
+
+  extStart(&EXTD1, &extcfg);
+
+  boardClearI2CBus(GPIOB_MEM_SCL);
+
+  global.HW_I2C2.start(&global.i2c2_config);
+
+  global.memory.init();
+
+  uint8_t i = 0;
+  if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
+    chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
+  }
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+
+  global.tlc5947.start(NORMALPRIO + 5);
+  global.tlc5947.enable();
+
+  global.robot.start(HIGHPRIO - 1);
+
+//   lidar.start(NORMALPRIO + 15); UNCOMMENT TO START LIDAR
+
+  global.lidar.start(NORMALPRIO);
+
+  global.userThread.start(NORMALPRIO);
+
+  /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
+  palWritePad(GPIOD, GPIOD_SYS_INT_N, PAL_HIGH);
+
+  /* wait until all modules are done */
+  while (palReadPad(GPIOD, GPIOD_SYS_INT_N) == PAL_LOW) {
+    continue;
+  }
+
+  global.robot.setLightBrightness(10);
+  global.robot.setLightColor(0, Color::RED);
+  global.robot.setLightColor(1, Color::LIME);
+  global.robot.setLightColor(2, Color::BLUE);
+  global.robot.setLightColor(3, Color::WHITE);
+  global.robot.setLightColor(4, Color::RED);
+  global.robot.setLightColor(5, Color::LIME);
+  global.robot.setLightColor(6, Color::BLUE);
+  global.robot.setLightColor(7, Color::WHITE);
+
+//   static uint16_t scannedData[NUMBER_OF_STEPS /*see lidar.h for this variable*/] = {}; UNCOMMENT TO START LIDAR
+  while (true) {
+
+    if (!shelltp)
+      shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
+    else if (chThdTerminated(shelltp)) {
+      chThdRelease(shelltp);    /* Recovers memory of the previous shell. */
+      shelltp = NULL;           /* Triggers spawning of a new shell.      */
+    }
+
+//    testLights();
+//    testColors();
+//    testFade();
+//    testBrightness();
+    if (shutdown_now != SHUTDOWN_NONE) {
+      if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER) {
+        chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
+        shutdown_now = SHUTDOWN_NONE;
+      } else {
+        uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR;
+        if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2) {
+          switch (shutdown_now) {
+            case SHUTDOWN_TRANSPORTATION:
+              blCallbackPtrAddr += 6 * 4;
+              break;
+            case SHUTDOWN_DEEPSLEEP:
+              blCallbackPtrAddr += 5 * 4;
+              break;
+            case SHUTDOWN_HIBERNATE:
+              blCallbackPtrAddr += 4 * 4;
+              break;
+            case SHUTDOWN_HANDLE_REQUEST:
+            case SHUTDOWN_RESTART:
+              blCallbackPtrAddr += 10 * 4;
+              break;
+            default:
+              blCallbackPtrAddr = 0;
+              break;
+          }
+        } else if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3) {
+          switch (shutdown_now) {
+            case SHUTDOWN_TRANSPORTATION:
+              blCallbackPtrAddr += 6 * 4;
+              break;
+            case SHUTDOWN_DEEPSLEEP:
+              blCallbackPtrAddr += 5 * 4;
+              break;
+            case SHUTDOWN_HIBERNATE:
+              blCallbackPtrAddr += 4 * 4;
+              break;
+            case SHUTDOWN_RESTART:
+              blCallbackPtrAddr += 7 * 4;
+              break;
+            case SHUTDOWN_HANDLE_REQUEST:
+              blCallbackPtrAddr += 8 * 4;
+              break;
+            default:
+              blCallbackPtrAddr = 0;
+              break;
+          }
+        }
+
+        void (*blCallback)(void) = NULL;
+        if (blCallbackPtrAddr) {
+          blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr));
+
+          if (!blCallback) {
+            chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
+            shutdown_now = SHUTDOWN_NONE;
+          } else {
+            chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
+            palWritePad(GPIOD, GPIOD_SYS_INT_N, PAL_LOW);
+            palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW);
+
+            chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
+            systemShutdown();
+            chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
+            BaseThread::sleep(MS2ST(10)); // sleep to print everything
+
+            blCallback();
+          }
+
+        } else {
+          chprintf((BaseSequentialStream*)&SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
+          shutdown_now = SHUTDOWN_NONE;
+        }
+      }
+    }
+
+//     // Print LIDAR scan
+//     if (global.lidar.getScan(scannedData)) {
+//       for (uint16_t idx = 0; idx < NUMBER_OF_STEPS; ++idx) {
+//         chprintf((BaseSequentialStream*) &SD1, "%d ", scannedData[idx]);
+//       }
+//       chprintf((BaseSequentialStream*) &SD1, "\n\n");
+//     }
+    BaseThread::sleep(MS2ST(500));
+  }
+
+  return 0;
+}
diff --git a/devices/LightRing/mcuconf.h b/devices/LightRing/mcuconf.h
new file mode 100644
index 0000000..8e7bdcf
--- /dev/null
+++ b/devices/LightRing/mcuconf.h
@@ -0,0 +1,184 @@
+#define STM32F103_MCUCONF
+
+/*
+ * STM32F103 drivers configuration.
+ * The following settings override the default settings present in
+ * the various device driver implementation headers.
+ * Note that the settings for each driver only have effect if the whole
+ * driver is enabled in halconf.h.
+ *
+ * IRQ priorities:
+ * 15...0       Lowest...Highest.
+ *
+ * DMA priorities:
+ * 0...3        Lowest...Highest.
+ */
+
+/*
+ * HAL driver system settings.
+ */
+#define STM32_NO_INIT                       FALSE
+#define STM32_HSI_ENABLED                   TRUE
+#define STM32_LSI_ENABLED                   FALSE
+#define STM32_HSE_ENABLED                   TRUE
+#define STM32_LSE_ENABLED                   FALSE
+#define STM32_SW                            STM32_SW_PLL
+#define STM32_PLLSRC                        STM32_PLLSRC_HSE
+#define STM32_PLLXTPRE                      STM32_PLLXTPRE_DIV1
+#define STM32_PLLMUL_VALUE                  9
+#define STM32_HPRE                          STM32_HPRE_DIV1
+#define STM32_PPRE1                         STM32_PPRE1_DIV2
+#define STM32_PPRE2                         STM32_PPRE2_DIV2
+#define STM32_ADCPRE                        STM32_ADCPRE_DIV4
+#define STM32_USB_CLOCK_REQUIRED            TRUE
+#define STM32_USBPRE                        STM32_USBPRE_DIV1P5
+#define STM32_MCOSEL                        STM32_MCOSEL_NOCLOCK
+#define STM32_RTCSEL                        STM32_RTCSEL_HSEDIV
+#define STM32_PVD_ENABLE                    FALSE
+#define STM32_PLS                           STM32_PLS_LEV0
+
+/*
+ * ADC driver system settings.
+ */
+#define STM32_ADC_USE_ADC1                  FALSE
+#define STM32_ADC_ADC1_DMA_PRIORITY         2
+#define STM32_ADC_ADC1_IRQ_PRIORITY         6
+
+/*
+ * CAN driver system settings.
+ */
+#define STM32_CAN_USE_CAN1                  TRUE
+#define STM32_CAN_CAN1_IRQ_PRIORITY         11
+
+/*
+ * EXT driver system settings.
+ */
+#define STM32_EXT_EXTI0_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI1_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI2_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI3_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI4_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI5_9_IRQ_PRIORITY      6
+#define STM32_EXT_EXTI10_15_IRQ_PRIORITY    6
+#define STM32_EXT_EXTI16_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI17_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI18_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI19_IRQ_PRIORITY       6
+
+/*
+ * GPT driver system settings.
+ */
+#define STM32_GPT_USE_TIM1                  FALSE
+#define STM32_GPT_USE_TIM2                  FALSE
+#define STM32_GPT_USE_TIM3                  FALSE
+#define STM32_GPT_USE_TIM4                  FALSE
+#define STM32_GPT_USE_TIM5                  FALSE
+#define STM32_GPT_USE_TIM8                  FALSE
+#define STM32_GPT_TIM1_IRQ_PRIORITY         7
+#define STM32_GPT_TIM2_IRQ_PRIORITY         7
+#define STM32_GPT_TIM3_IRQ_PRIORITY         7
+#define STM32_GPT_TIM4_IRQ_PRIORITY         7
+#define STM32_GPT_TIM5_IRQ_PRIORITY         7
+#define STM32_GPT_TIM8_IRQ_PRIORITY         7
+
+/*
+ * I2C driver system settings.
+ */
+#define STM32_I2C_USE_I2C1                  FALSE
+#define STM32_I2C_USE_I2C2                  TRUE
+#define STM32_I2C_I2C1_IRQ_PRIORITY         5
+#define STM32_I2C_I2C2_IRQ_PRIORITY         5
+#define STM32_I2C_I2C1_DMA_PRIORITY         3
+#define STM32_I2C_I2C2_DMA_PRIORITY         3
+#define STM32_I2C_I2C1_DMA_ERROR_HOOK()     chSysHalt()
+#define STM32_I2C_I2C2_DMA_ERROR_HOOK()     chSysHalt()
+#define STM32_I2C_USE_DMA                   FALSE
+
+/*
+ * ICU driver system settings.
+ */
+#define STM32_ICU_USE_TIM1                  FALSE
+#define STM32_ICU_USE_TIM2                  FALSE
+#define STM32_ICU_USE_TIM3                  FALSE
+#define STM32_ICU_USE_TIM4                  FALSE
+#define STM32_ICU_USE_TIM5                  FALSE
+#define STM32_ICU_USE_TIM8                  FALSE
+#define STM32_ICU_TIM1_IRQ_PRIORITY         7
+#define STM32_ICU_TIM2_IRQ_PRIORITY         7
+#define STM32_ICU_TIM3_IRQ_PRIORITY         7
+#define STM32_ICU_TIM4_IRQ_PRIORITY         7
+#define STM32_ICU_TIM5_IRQ_PRIORITY         7
+#define STM32_ICU_TIM8_IRQ_PRIORITY         7
+
+/*
+ * PWM driver system settings.
+ */
+#define STM32_PWM_USE_ADVANCED              FALSE
+#define STM32_PWM_USE_TIM1                  FALSE
+#define STM32_PWM_USE_TIM2                  FALSE
+#define STM32_PWM_USE_TIM3                  FALSE
+#define STM32_PWM_USE_TIM4                  FALSE
+#define STM32_PWM_USE_TIM5                  FALSE
+#define STM32_PWM_USE_TIM8                  FALSE
+#define STM32_PWM_TIM1_IRQ_PRIORITY         7
+#define STM32_PWM_TIM2_IRQ_PRIORITY         7
+#define STM32_PWM_TIM3_IRQ_PRIORITY         7
+#define STM32_PWM_TIM4_IRQ_PRIORITY         7
+#define STM32_PWM_TIM5_IRQ_PRIORITY         7
+#define STM32_PWM_TIM8_IRQ_PRIORITY         7
+
+/*
+ * RTC driver system settings.
+ */
+#define STM32_RTC_IRQ_PRIORITY              15
+
+/*
+ * SERIAL driver system settings.
+ */
+#define STM32_SERIAL_USE_USART1             TRUE
+#define STM32_SERIAL_USE_USART2             TRUE
+#define STM32_SERIAL_USE_USART3             FALSE
+#define STM32_SERIAL_USE_UART4              FALSE
+#define STM32_SERIAL_USE_UART5              FALSE
+#define STM32_SERIAL_USART1_PRIORITY        12
+#define STM32_SERIAL_USART2_PRIORITY        12
+#define STM32_SERIAL_USART3_PRIORITY        12
+#define STM32_SERIAL_UART4_PRIORITY         12
+#define STM32_SERIAL_UART5_PRIORITY         12
+
+/*
+ * SPI driver system settings.
+ */
+#define STM32_SPI_USE_SPI1                  TRUE
+#define STM32_SPI_USE_SPI2                  TRUE
+#define STM32_SPI_USE_SPI3                  FALSE
+#define STM32_SPI_SPI1_DMA_PRIORITY         1
+#define STM32_SPI_SPI2_DMA_PRIORITY         1
+#define STM32_SPI_SPI3_DMA_PRIORITY         1
+#define STM32_SPI_SPI1_IRQ_PRIORITY         10
+#define STM32_SPI_SPI2_IRQ_PRIORITY         10
+#define STM32_SPI_SPI3_IRQ_PRIORITY         10
+#define STM32_SPI_DMA_ERROR_HOOK(spip)      chSysHalt()
+
+/*
+ * UART driver system settings.
+ */
+#define STM32_UART_USE_USART1               FALSE
+#define STM32_UART_USE_USART2               FALSE
+#define STM32_UART_USE_USART3               FALSE
+#define STM32_UART_USART1_IRQ_PRIORITY      12
+#define STM32_UART_USART2_IRQ_PRIORITY      12
+#define STM32_UART_USART3_IRQ_PRIORITY      12
+#define STM32_UART_USART1_DMA_PRIORITY      0
+#define STM32_UART_USART2_DMA_PRIORITY      0
+#define STM32_UART_USART3_DMA_PRIORITY      0
+#define STM32_UART_DMA_ERROR_HOOK(uartp)    chSysHalt()
+
+/*
+ * USB driver system settings.
+ */
+#define STM32_USB_USE_USB1                  FALSE
+#define STM32_USB_LOW_POWER_ON_SUSPEND      FALSE
+#define STM32_USB_USB1_HP_IRQ_PRIORITY      13
+#define STM32_USB_USB1_LP_IRQ_PRIORITY      14
+
diff --git a/devices/LightRing/userthread.cpp b/devices/LightRing/userthread.cpp
new file mode 100644
index 0000000..b21efac
--- /dev/null
+++ b/devices/LightRing/userthread.cpp
@@ -0,0 +1,32 @@
+#include "userthread.hpp"
+
+#include <global.hpp>
+
+using namespace amiro;
+
+extern Global global;
+
+UserThread::UserThread() :
+  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
+{
+}
+
+UserThread::~UserThread()
+{
+}
+
+msg_t
+UserThread::main()
+{
+  while (!this->shouldTerminate())
+  {
+    /*
+     * INSERT CUSTOM CODE HERE
+     */
+
+    this->sleep(MS2ST(1000));
+  }
+
+  return RDY_OK;
+}
+
diff --git a/devices/LightRing/userthread.hpp b/devices/LightRing/userthread.hpp
new file mode 100644
index 0000000..80304fb
--- /dev/null
+++ b/devices/LightRing/userthread.hpp
@@ -0,0 +1,20 @@
+#ifndef AMIRO_USERTHREAD_H_
+#define AMIRO_USERTHREAD_H_
+
+#include <ch.hpp>
+#include <amiroosconf.h>
+
+namespace amiro {
+
+class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
+{
+public:
+  explicit UserThread();
+  virtual ~UserThread();
+
+  virtual msg_t main();
+};
+
+} // end of namespace amiro
+
+#endif // AMIRO_USERTHREAD_H_
diff --git a/devices/Makefile b/devices/Makefile
new file mode 100644
index 0000000..1b0e004
--- /dev/null
+++ b/devices/Makefile
@@ -0,0 +1,19 @@
+all: devices
+
+include flash.mk
+
+CLEANTRGS = $(DEVICES:%=clean-%)
+
+.PHONY: $(DEVICES)
+
+devices: $(DEVICES)
+
+$(DEVICES):
+	$(MAKE) -C $@
+
+$(FLASHTRGS):
+	$(MAKE) -C $(firstword $(subst /, ,$@))
+
+clean: $(CLEANTRGS)
+$(CLEANTRGS):
+	$(MAKE) -C $(@:clean-%=%) clean
diff --git a/devices/PowerManagement/Makefile b/devices/PowerManagement/Makefile
new file mode 100644
index 0000000..536824d
--- /dev/null
+++ b/devices/PowerManagement/Makefile
@@ -0,0 +1,270 @@
+##############################################################################
+# Build global options
+# NOTE: Can be overridden externally.
+#
+
+# Compiler options here.
+ifeq ($(USE_OPT),)
+  USE_OPT = -O2 -fomit-frame-pointer -falign-functions=16 -DCHPRINTF_USE_FLOAT=1 -fstack-usage -DSERIAL_BUFFERS_SIZE=64
+endif
+
+# C specific options here (added to USE_OPT).
+ifeq ($(USE_COPT),)
+  USE_COPT =
+endif
+
+# C++ specific options here (added to USE_OPT).
+ifeq ($(USE_CPPOPT),)
+  USE_CPPOPT = -fno-rtti -fno-exceptions -std=c++11 -U__STRICT_ANSI__
+endif
+
+# Enable this if you want the linker to remove unused code and data
+ifeq ($(USE_LINK_GC),)
+  USE_LINK_GC = yes
+endif
+
+# Linker extra options here.
+ifeq ($(USE_LDOPT),)
+  USE_LDOPT =
+endif
+
+# Enable this if you want link time optimizations (LTO)
+ifeq ($(USE_LTO),)
+  USE_LTO = no
+endif
+
+# If enabled, this option allows to compile the application in THUMB mode.
+ifeq ($(USE_THUMB),)
+  USE_THUMB = yes
+endif
+
+# Enable this if you want to see the full log while compiling.
+ifeq ($(USE_VERBOSE_COMPILE),)
+  USE_VERBOSE_COMPILE = no
+endif
+
+#
+# Build global options
+##############################################################################
+
+##############################################################################
+# Architecture or project specific options
+#
+
+# Enables the use of FPU on Cortex-M4.
+# Possible selections are:
+#   no     - no FPU is used (propably equals 'soft')
+#   soft   - does not use the FPU, thus all floating point operations are emulated
+#   softfp - uses the FPU, but uses the integer registers only
+#   hard   - uses the FPU and passes data via the FPU registers
+ifeq ($(USE_FPU),)
+  USE_FPU = hard
+endif
+
+# Enable this if you really want to use the STM FWLib.
+ifeq ($(USE_FWLIB),)
+  USE_FWLIB = no
+endif
+
+#
+# Architecture or project specific options
+##############################################################################
+
+##############################################################################
+# Project, sources and paths
+#
+
+# Define project name here
+PROJECT = PowerManagement
+
+# Imported source files and paths
+CHIBIOS = ../../../ChibiOS
+AMIRO = ../..
+include $(AMIRO)/boards/PowerManagement/board.mk
+include $(CHIBIOS)/os/hal/platforms/STM32F4xx/platform.mk
+include $(CHIBIOS)/os/hal/hal.mk
+include $(CHIBIOS)/os/ports/GCC/ARMCMx/STM32F4xx/port.mk
+include $(CHIBIOS)/os/kernel/kernel.mk
+include $(CHIBIOS)/os/various/cpp_wrappers/kernel.mk
+
+# Define linker script file here
+LDSCRIPT= $(BOARDLD)/STM32F405xG.ld
+
+# C sources that can be compiled in ARM or THUMB mode depending on the global
+# setting.
+CSRC = $(PORTSRC) \
+       $(KERNSRC) \
+       $(HALSRC) \
+       $(PLATFORMSRC) \
+       $(BOARDSRC) \
+       $(CHIBIOS)/os/various/evtimer.c \
+       $(CHIBIOS)/os/various/syscalls.c \
+       $(CHIBIOS)/os/various/chprintf.c \
+       $(CHIBIOS)/os/various/shell.c \
+       $(CHIBIOS)/os/various/memstreams.c \
+       $(AMIRO)/stubs.c \
+       $(AMIRO)/components/Debug.c
+
+# C++ sources that can be compiled in ARM or THUMB mode depending on the global
+# setting.
+CPPSRC = $(CHCPPSRC) \
+         $(AMIRO)/components/Color.cpp \
+         $(AMIRO)/components/ControllerAreaNetworkRx.cpp \
+         $(AMIRO)/components/ControllerAreaNetworkTx.cpp \
+         $(AMIRO)/components/bus/i2c/HWI2CDriver.cpp \
+         $(AMIRO)/components/bus/i2c/I2CMultiplexer.cpp \
+         $(AMIRO)/components/bus/i2c/VI2CDriver.cpp \
+         $(AMIRO)/components/proximity/vcnl4020.cpp \
+         $(AMIRO)/components/eeprom/eeprom.cpp \
+         $(AMIRO)/components/eeprom/at24.cpp \
+         $(AMIRO)/components/FileSystemInputOutput/FileSystemInputOutputBase.cpp \
+         $(AMIRO)/components/FileSystemInputOutput/FSIOPowerManagement.cpp \
+         $(AMIRO)/components/input/mpr121.cpp \
+         $(AMIRO)/components/power/adconverter.cpp \
+         $(AMIRO)/components/power/ina219.cpp \
+         $(AMIRO)/components/power/bq27500.cpp \
+         $(AMIRO)/components/bluetooth/bluetooth-descriptor.cpp \
+         $(AMIRO)/components/bluetooth/bluetooth-transport.cpp \
+         $(AMIRO)/components/bluetooth/bluetooth-connector.cpp \
+         $(AMIRO)/components/bluetooth/bluetooth-wiimote.cpp \
+         $(AMIRO)/components/bluetooth/bluetooth-serial.cpp \
+         $(AMIRO)/components/bluetooth/bluetooth-iwrap.cpp \
+         $(AMIRO)/components/bluetooth/bluetooth.cpp \
+         $(AMIRO)/components/serial_reset/iwrap_can_mux.cpp \
+         $(AMIRO)/components/serial_reset/serial_can_mux.cpp \
+         PowerManagement.cpp \
+         userthread.cpp \
+         main.cpp \
+         exti.cpp
+
+# C sources to be compiled in ARM mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+ACSRC =
+
+# C++ sources to be compiled in ARM mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+ACPPSRC =
+
+# C sources to be compiled in THUMB mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+TCSRC =
+
+# C sources to be compiled in THUMB mode regardless of the global setting.
+# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
+#       option that results in lower performance and larger code size.
+TCPPSRC =
+
+# List ASM source files here
+ASMSRC = $(PORTASM)
+
+INCDIR = $(PORTINC) $(KERNINC) $(TESTINC) \
+         $(HALINC) $(PLATFORMINC) $(BOARDINC) \
+         $(CHCPPINC) \
+         $(CHIBIOS)/os/various \
+         $(AMIRO) \
+         $(AMIRO)/include
+
+#
+# Project, sources and paths
+##############################################################################
+
+##############################################################################
+# Compiler settings
+#
+
+MCU  = cortex-m4
+
+#TRGT = arm-elf-
+TRGT = arm-none-eabi-
+CC   = $(TRGT)gcc
+CPPC = $(TRGT)g++
+# Enable loading with g++ only if you need C++ runtime support.
+# NOTE: You can use C++ even without C++ support if you are careful. C++
+#       runtime support makes code size explode.
+#LD   = $(TRGT)gcc
+LD   = $(TRGT)g++
+CP   = $(TRGT)objcopy
+AS   = $(TRGT)gcc -x assembler-with-cpp
+OD   = $(TRGT)objdump
+SZ   = $(TRGT)size
+HEX  = $(CP) -O ihex
+BIN  = $(CP) -O binary
+SREC = $(CP) -O srec --srec-len=248
+
+# ARM-specific options here
+AOPT =
+
+# THUMB-specific options here
+TOPT = -mthumb -DTHUMB
+
+# Define C warning options here
+CWARN = -Wall -Wextra -Wstrict-prototypes
+
+# Define C++ warning options here
+CPPWARN = -Wall -Wextra
+
+#
+# Compiler settings
+##############################################################################
+
+##############################################################################
+# Start of default section
+#
+
+# List all default C defines here, like -D_DEBUG=1
+DDEFS =
+
+# List all default ASM defines here, like -D_DEBUG=1
+DADEFS =
+
+# List all default directories to look for include files here
+DINCDIR =
+
+# List the default directory to look for the libraries here
+DLIBDIR =
+
+# List all default libraries here
+DLIBS =
+
+#
+# End of default section
+##############################################################################
+
+##############################################################################
+# Start of user section
+#
+
+# List all user C define here, like -D_DEBUG=1
+UDEFS =
+
+# Define ASM defines here
+UADEFS =
+
+# List all user directories here
+UINCDIR =
+
+# List the user directory to look for the libraries here
+ULIBDIR =
+
+# List all user libraries here
+ULIBS =
+
+#
+# End of user defines
+##############################################################################
+
+ifeq ($(USE_FWLIB),yes)
+  include $(CHIBIOS)/ext/stm32lib/stm32lib.mk
+  CSRC += $(STM32SRC)
+  INCDIR += $(STM32INC)
+  USE_OPT += -DUSE_STDPERIPH_DRIVER
+endif
+
+RULESPATH = $(CHIBIOS)/os/ports/GCC/ARMCMx
+include $(RULESPATH)/rules.mk
+
+include $(AMIRO)/ports/rules.mk
+include $(AMIRO)/devices/flash.mk
diff --git a/devices/PowerManagement/PowerManagement.cpp b/devices/PowerManagement/PowerManagement.cpp
new file mode 100644
index 0000000..02333f3
--- /dev/null
+++ b/devices/PowerManagement/PowerManagement.cpp
@@ -0,0 +1,186 @@
+#include "ch.hpp"
+#include "hal.h"
+#include "PowerManagement.h"
+
+#include <amiro/proximity/vcnl4020.hpp>
+#include <global.hpp>
+
+#include <algorithm>
+#include <chprintf.h>
+
+using namespace chibios_rt;
+using namespace amiro;
+
+extern Global global;
+
+PowerManagement::PowerManagement(CANDriver *can)
+    : ControllerAreaNetworkTx(can, CAN::POWER_MANAGEMENT_ID),
+      ControllerAreaNetworkRx(can, CAN::POWER_MANAGEMENT_ID),
+      bc_counter(0)
+{
+  this->powerStatus.charging_flags.value = 0;
+}
+
+msg_t PowerManagement::receiveMessage(CANRxFrame *frame) {
+  int deviceId = this->decodeDeviceId(frame);
+  switch (deviceId) {
+
+    case CAN::SHELL_REPLY_ID(CAN::POWER_MANAGEMENT_ID):
+      if (frame->DLC > 0) {
+        sdWrite(&SD1, frame->data8, frame->DLC);
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::SHELL_QUERY_ID(CAN::POWER_MANAGEMENT_ID):
+      if (frame->DLC != 0) {
+        global.sercanmux1.convCan2Serial(frame->data8, frame->DLC);
+        return RDY_OK;
+      } else {
+        global.sercanmux1.rcvSwitchCmd(this->decodeBoardId(frame));
+        return RDY_OK;
+      }
+      break;
+
+    case CAN::CALIBRATE_PROXIMITY_RING:
+      // Dont care about the payload but start the calibration
+      // TODO Care about the payload. Differ between:
+      // 1: Do fresh calibration (Save values to memory and to temporary values)
+      // 2: Remove temporary Calibration and get uncalibrated values
+      // 3: Load calibration from memory
+      this->calibrate();
+      break;
+
+    case CAN::ROBOT_ID:
+      if (frame->DLC == 1) {
+        this->robotId = frame->data8[0];
+        return RDY_OK;
+      }
+      break;
+
+    default:
+      break;
+  }
+
+  return -1;
+}
+
+msg_t PowerManagement::updateSensorVal() {
+
+  // update charger status
+  this->powerStatus.charging_flags.content.powermanagement_plugged_in = global.ltc4412.isPluggedIn();
+
+  // update fuel gauges values
+  const BQ27500::Driver::UpdateData* power[2] {
+    &global.bq27500[constants::PowerManagement::BAT_A].getStatus(),
+    &global.bq27500[constants::PowerManagement::BAT_B].getStatus()
+  };
+  this->powerStatus.charging_flags.content.powermanagement_charging = (this->powerStatus.charging_flags.content.powermanagement_plugged_in &&
+                                                                       this->powerStatus.charging_flags.content.vsys_higher_than_9V &&
+                                                                       power[0]->minutes_to_empty == uint16_t(~0) &&
+                                                                       power[1]->minutes_to_empty == uint16_t(~0))?
+                                                                       true : false;
+  this->powerStatus.charging_flags.content.diwheeldrive_charging = (this->powerStatus.charging_flags.content.diwheeldrive_enable_power_path &&
+                                                                    this->powerStatus.charging_flags.content.vsys_higher_than_9V &&
+                                                                    power[0]->minutes_to_empty == uint16_t(~0) &&
+                                                                    power[1]->minutes_to_empty == uint16_t(~0))?
+                                                                    true : false;
+  this->powerStatus.state_of_charge = (power[0]->state_of_charge + power[1]->state_of_charge) / 2;
+  if (this->powerStatus.charging_flags.content.powermanagement_charging || this->powerStatus.charging_flags.content.diwheeldrive_charging) {
+    /*
+     * Assumption:
+     * When charging there is enough power available to charge both batteries at full rate simultaneously.
+     * Thus, the second battery will not charge faster when the first battery is fully charged.
+     */
+    this->powerStatus.minutes_remaining = std::max(power[0]->minutes_to_full, power[1]->minutes_to_full);
+  } else {
+    /*
+     * Computation of the remaining discharging time:
+     * Take the time until the first of the two batteries is empty and add the remaining time of the second battery but half.
+     *        time = min(a,b) + (max(a,b) - min(a,b))/2
+     * <=>  2*time = 2*min(a,b) + max(a,b) - min(a,b)
+     * <=>  2*time = min(a,b) + max(a,b)
+     * <=>  2*time = a + b
+     * <=>    time = (a + b)/2
+     */
+    this->powerStatus.minutes_remaining = (power[0]->minutes_to_empty + power[1]->minutes_to_empty) / 2;
+  }
+  this->powerStatus.power_consumption = (power[0]->average_power_mW + power[1]->average_power_mW) / 2;
+
+  // update infrared sensor value
+  // Note: The CANRx Value will never be updated in this thread
+  for (int idx = 0; idx < 8; idx++)
+    this->proximityRingValue[idx] = global.vcnl4020[idx].getProximityScaledWoOffset();
+
+  return 0;
+}
+
+void PowerManagement::periodicBroadcast() {
+  CANTxFrame frame;
+  if (this->bc_counter % 10 == 0) {
+    frame.SID = 0;
+    this->encodeDeviceId(&frame, CAN::POWER_STATUS_ID);
+    frame.data8[0] = this->powerStatus.charging_flags.value;
+    frame.data8[1] = this->powerStatus.state_of_charge;
+    frame.data16[1] = this->powerStatus.minutes_remaining;
+    frame.data16[2] = this->powerStatus.power_consumption;
+    frame.DLC = 6;
+    this->transmitMessage(&frame);
+  }
+  for (int i = 0; i < 8; i++) {
+    frame.SID = 0;
+    this->encodeDeviceId(&frame, CAN::PROXIMITY_RING_ID(i));
+    frame.data16[0] = this->proximityRingValue[i];
+    frame.DLC = 2;
+    this->transmitMessage(&frame);
+    // HACK This is a first workaround (do wrapping of 4 sensors, better timing than 10 ms in sleep)
+    // I choosed 10 ms, because 10 ms x 8 = 80 ms < 125 ms which is the updaterate CAN::UPDATE_PERIOD_MSEC
+    BaseThread::sleep(MS2ST(10));  // Sleep, otherwise the cognition-board wont receive all messages
+  }
+  ++this->bc_counter;
+}
+
+void PowerManagement::calibrate() {
+  // Stop sending and receiving of values to indicate the calibration phase
+//   eventTimerEvtSource->unregister(&this->eventTimerEvtListener);
+//   rxFullCanEvtSource->unregister(&this->rxFullCanEvtListener);
+
+  this->calibrateProximityRingValues();
+
+  // Start sending and receving of values
+//   eventTimerEvtSource->registerOne(&this->eventTimerEvtListener, CAN::PERIODIC_TIMER_ID);
+//   rxFullCanEvtSource->registerOne(&this->rxFullCanEvtListener, CAN::RECEIVED_ID);
+}
+
+void PowerManagement::calibrateProximityRingValues() {
+
+  uint16_t buffer;
+  for (uint8_t idx = 0; idx < 8; ++idx) {
+    global.vcnl4020[idx].calibrate();
+    buffer = global.vcnl4020[idx].getProximityOffset();
+    global.memory.setVcnl4020Offset(buffer,idx);
+  }
+}
+
+ThreadReference PowerManagement::start(tprio_t PRIO) {
+  this->ControllerAreaNetworkRx::start(PRIO + 1);
+  this->ControllerAreaNetworkTx::start(PRIO);
+  return NULL;
+}
+
+types::power_status&
+PowerManagement::getPowerStatus()
+{
+  return this->powerStatus;
+}
+
+msg_t PowerManagement::terminate(void) {
+  msg_t ret = RDY_OK;
+
+  this->ControllerAreaNetworkTx::requestTerminate();
+  ret |= this->ControllerAreaNetworkTx::wait();
+  this->ControllerAreaNetworkRx::requestTerminate();
+  ret |= this->ControllerAreaNetworkRx::wait();
+
+  return ret;
+}
diff --git a/devices/PowerManagement/PowerManagement.h b/devices/PowerManagement/PowerManagement.h
new file mode 100644
index 0000000..9927167
--- /dev/null
+++ b/devices/PowerManagement/PowerManagement.h
@@ -0,0 +1,44 @@
+#ifndef AMIRO_POWER_MANAGEMENT_H_
+#define AMIRO_POWER_MANAGEMENT_H_
+
+#include <amiro/ControllerAreaNetworkRx.h>
+#include <amiro/ControllerAreaNetworkTx.h>
+#include <amiro/proximity/vcnl4020.hpp>
+#include <amiro/FileSystemInputOutput/FSIOPowerManagement.hpp>
+#include <amiro/power/bq27500.hpp>
+#include <amiro/power/ltc4412.hpp>
+
+namespace amiro {
+
+  class VCNL4020;
+
+  class PowerManagement : public ControllerAreaNetworkTx, public ControllerAreaNetworkRx  {
+  public:
+    PowerManagement(CANDriver *can);
+    virtual void  calibrate();
+
+    /** \brief Handle the termination of ControllerAreaNetworkTx and ControllerAreaNetworkRx threads
+     *         This includes waiting until the threads have terminated, thus this function might block a relatively long time.
+     */
+    msg_t terminate(void);
+
+    /** \brief Handle the start of ControllerAreaNetworkTx and ControllerAreaNetworkRx threads */
+    ThreadReference start(tprio_t PRIO);
+
+    types::power_status& getPowerStatus();
+
+  protected:
+    virtual msg_t receiveMessage(CANRxFrame *frame);
+    virtual msg_t updateSensorVal();
+    virtual void periodicBroadcast();
+
+    void  calibrateProximityRingValues();
+
+  private:
+    uint32_t bc_counter;
+
+  };
+
+}
+
+#endif /* AMIRO_POWER_MANAGEMENT_H_ */
diff --git a/devices/PowerManagement/amiroosconf.h b/devices/PowerManagement/amiroosconf.h
new file mode 100644
index 0000000..4bb1b27
--- /dev/null
+++ b/devices/PowerManagement/amiroosconf.h
@@ -0,0 +1,19 @@
+#ifndef AMIRO_AMIROOSCONF_H_
+#define AMIRO_AMIROOSCONF_H_
+
+/**
+ * @brief Stack size of the user thread object
+ */
+#if !defined(USER_THREAD_STACK_SIZE) || defined(__DOXYGEN__)
+#define USER_THREAD_STACK_SIZE         1024
+#endif
+
+/**
+ * @brief Flag to enable/disable als amiroDbgXXX macros
+ */
+#if !defined(AMIRO_DBG) || defined(__DOXYGEN__)
+#define AMIRO_DBG                      TRUE
+#endif
+
+#endif // AMIRO_AMIROOSCONF_H_
+
diff --git a/devices/PowerManagement/chconf.h b/devices/PowerManagement/chconf.h
new file mode 100644
index 0000000..161fd3c
--- /dev/null
+++ b/devices/PowerManagement/chconf.h
@@ -0,0 +1,507 @@
+#ifndef _CHCONF_H_
+#define _CHCONF_H_
+
+/*===========================================================================*/
+/**
+ * @name Kernel parameters and options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   System tick frequency.
+ * @details Frequency of the system timer that drives the system ticks. This
+ *          setting also defines the system tick time unit.
+ */
+#if !defined(CH_FREQUENCY) || defined(__DOXYGEN__)
+#define CH_FREQUENCY                    1000
+#endif
+
+/**
+ * @brief   Round robin interval.
+ * @details This constant is the number of system ticks allowed for the
+ *          threads before preemption occurs. Setting this value to zero
+ *          disables the preemption for threads with equal priority and the
+ *          round robin becomes cooperative. Note that higher priority
+ *          threads can still preempt, the kernel is always preemptive.
+ *
+ * @note    Disabling the round robin preemption makes the kernel more compact
+ *          and generally faster.
+ */
+#if !defined(CH_TIME_QUANTUM) || defined(__DOXYGEN__)
+#define CH_TIME_QUANTUM                 20
+#endif
+
+/**
+ * @brief   Managed RAM size.
+ * @details Size of the RAM area to be managed by the OS. If set to zero
+ *          then the whole available RAM is used. The core memory is made
+ *          available to the heap allocator and/or can be used directly through
+ *          the simplified core memory allocator.
+ *
+ * @note    In order to let the OS manage the whole RAM the linker script must
+ *          provide the @p __heap_base__ and @p __heap_end__ symbols.
+ * @note    Requires @p CH_USE_MEMCORE.
+ */
+#if !defined(CH_MEMCORE_SIZE) || defined(__DOXYGEN__)
+#define CH_MEMCORE_SIZE                 0
+#endif
+
+/**
+ * @brief   Idle thread automatic spawn suppression.
+ * @details When this option is activated the function @p chSysInit()
+ *          does not spawn the idle thread automatically. The application has
+ *          then the responsibility to do one of the following:
+ *          - Spawn a custom idle thread at priority @p IDLEPRIO.
+ *          - Change the main() thread priority to @p IDLEPRIO then enter
+ *            an endless loop. In this scenario the @p main() thread acts as
+ *            the idle thread.
+ *          .
+ * @note    Unless an idle thread is spawned the @p main() thread must not
+ *          enter a sleep state.
+ */
+#if !defined(CH_NO_IDLE_THREAD) || defined(__DOXYGEN__)
+#define CH_NO_IDLE_THREAD               FALSE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Performance options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   OS optimization.
+ * @details If enabled then time efficient rather than space efficient code
+ *          is used when two possible implementations exist.
+ *
+ * @note    This is not related to the compiler optimization options.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_OPTIMIZE_SPEED) || defined(__DOXYGEN__)
+#define CH_OPTIMIZE_SPEED               TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Subsystem options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Threads registry APIs.
+ * @details If enabled then the registry APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_REGISTRY) || defined(__DOXYGEN__)
+#define CH_USE_REGISTRY                 TRUE
+#endif
+
+/**
+ * @brief   Threads synchronization APIs.
+ * @details If enabled then the @p chThdWait() function is included in
+ *          the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_WAITEXIT) || defined(__DOXYGEN__)
+#define CH_USE_WAITEXIT                 TRUE
+#endif
+
+/**
+ * @brief   Semaphores APIs.
+ * @details If enabled then the Semaphores APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_SEMAPHORES) || defined(__DOXYGEN__)
+#define CH_USE_SEMAPHORES               TRUE
+#endif
+
+/**
+ * @brief   Semaphores queuing mode.
+ * @details If enabled then the threads are enqueued on semaphores by
+ *          priority rather than in FIFO order.
+ *
+ * @note    The default is @p FALSE. Enable this if you have special requirements.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_SEMAPHORES_PRIORITY) || defined(__DOXYGEN__)
+#define CH_USE_SEMAPHORES_PRIORITY      FALSE
+#endif
+
+/**
+ * @brief   Atomic semaphore API.
+ * @details If enabled then the semaphores the @p chSemSignalWait() API
+ *          is included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_SEMSW) || defined(__DOXYGEN__)
+#define CH_USE_SEMSW                    TRUE
+#endif
+
+/**
+ * @brief   Mutexes APIs.
+ * @details If enabled then the mutexes APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MUTEXES) || defined(__DOXYGEN__)
+#define CH_USE_MUTEXES                  TRUE
+#endif
+
+/**
+ * @brief   Conditional Variables APIs.
+ * @details If enabled then the conditional variables APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_MUTEXES.
+ */
+#if !defined(CH_USE_CONDVARS) || defined(__DOXYGEN__)
+#define CH_USE_CONDVARS                 TRUE
+#endif
+
+/**
+ * @brief   Conditional Variables APIs with timeout.
+ * @details If enabled then the conditional variables APIs with timeout
+ *          specification are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_CONDVARS.
+ */
+#if !defined(CH_USE_CONDVARS_TIMEOUT) || defined(__DOXYGEN__)
+#define CH_USE_CONDVARS_TIMEOUT         TRUE
+#endif
+
+/**
+ * @brief   Events Flags APIs.
+ * @details If enabled then the event flags APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_EVENTS) || defined(__DOXYGEN__)
+#define CH_USE_EVENTS                   TRUE
+#endif
+
+/**
+ * @brief   Events Flags APIs with timeout.
+ * @details If enabled then the events APIs with timeout specification
+ *          are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_EVENTS.
+ */
+#if !defined(CH_USE_EVENTS_TIMEOUT) || defined(__DOXYGEN__)
+#define CH_USE_EVENTS_TIMEOUT           TRUE
+#endif
+
+/**
+ * @brief   Synchronous Messages APIs.
+ * @details If enabled then the synchronous messages APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MESSAGES) || defined(__DOXYGEN__)
+#define CH_USE_MESSAGES                 TRUE
+#endif
+
+/**
+ * @brief   Synchronous Messages queuing mode.
+ * @details If enabled then messages are served by priority rather than in
+ *          FIFO order.
+ *
+ * @note    The default is @p FALSE. Enable this if you have special requirements.
+ * @note    Requires @p CH_USE_MESSAGES.
+ */
+#if !defined(CH_USE_MESSAGES_PRIORITY) || defined(__DOXYGEN__)
+#define CH_USE_MESSAGES_PRIORITY        FALSE
+#endif
+
+/**
+ * @brief   Mailboxes APIs.
+ * @details If enabled then the asynchronous messages (mailboxes) APIs are
+ *          included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_SEMAPHORES.
+ */
+#if !defined(CH_USE_MAILBOXES) || defined(__DOXYGEN__)
+#define CH_USE_MAILBOXES                TRUE
+#endif
+
+/**
+ * @brief   I/O Queues APIs.
+ * @details If enabled then the I/O queues APIs are included in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_QUEUES) || defined(__DOXYGEN__)
+#define CH_USE_QUEUES                   TRUE
+#endif
+
+/**
+ * @brief   Core Memory Manager APIs.
+ * @details If enabled then the core memory manager APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MEMCORE) || defined(__DOXYGEN__)
+#define CH_USE_MEMCORE                  TRUE
+#endif
+
+/**
+ * @brief   Heap Allocator APIs.
+ * @details If enabled then the memory heap allocator APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_MEMCORE and either @p CH_USE_MUTEXES or
+ *          @p CH_USE_SEMAPHORES.
+ * @note    Mutexes are recommended.
+ */
+#if !defined(CH_USE_HEAP) || defined(__DOXYGEN__)
+#define CH_USE_HEAP                     TRUE
+#endif
+
+/**
+ * @brief   C-runtime allocator.
+ * @details If enabled the the heap allocator APIs just wrap the C-runtime
+ *          @p malloc() and @p free() functions.
+ *
+ * @note    The default is @p FALSE.
+ * @note    Requires @p CH_USE_HEAP.
+ * @note    The C-runtime may or may not require @p CH_USE_MEMCORE, see the
+ *          appropriate documentation.
+ */
+#if !defined(CH_USE_MALLOC_HEAP) || defined(__DOXYGEN__)
+#define CH_USE_MALLOC_HEAP              FALSE
+#endif
+
+/**
+ * @brief   Memory Pools Allocator APIs.
+ * @details If enabled then the memory pools allocator APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ */
+#if !defined(CH_USE_MEMPOOLS) || defined(__DOXYGEN__)
+#define CH_USE_MEMPOOLS                 TRUE
+#endif
+
+/**
+ * @brief   Dynamic Threads APIs.
+ * @details If enabled then the dynamic threads creation APIs are included
+ *          in the kernel.
+ *
+ * @note    The default is @p TRUE.
+ * @note    Requires @p CH_USE_WAITEXIT.
+ * @note    Requires @p CH_USE_HEAP and/or @p CH_USE_MEMPOOLS.
+ */
+#if !defined(CH_USE_DYNAMIC) || defined(__DOXYGEN__)
+#define CH_USE_DYNAMIC                  TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Debug options
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Debug option, system state check.
+ * @details If enabled the correct call protocol for system APIs is checked
+ *          at runtime.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_SYSTEM_STATE_CHECK) || defined(__DOXYGEN__)
+#define CH_DBG_SYSTEM_STATE_CHECK       TRUE
+#endif
+
+/**
+ * @brief   Debug option, parameters checks.
+ * @details If enabled then the checks on the API functions input
+ *          parameters are activated.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_CHECKS) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_CHECKS            TRUE
+#endif
+
+/**
+ * @brief   Debug option, consistency checks.
+ * @details If enabled then all the assertions in the kernel code are
+ *          activated. This includes consistency checks inside the kernel,
+ *          runtime anomalies and port-defined checks.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_ASSERTS) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_ASSERTS           TRUE
+#endif
+
+/**
+ * @brief   Debug option, trace buffer.
+ * @details If enabled then the context switch circular trace buffer is
+ *          activated.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_ENABLE_TRACE) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_TRACE             TRUE
+#endif
+
+/**
+ * @brief   Debug option, stack checks.
+ * @details If enabled then a runtime stack check is performed.
+ *
+ * @note    The default is @p FALSE.
+ * @note    The stack check is performed in a architecture/port dependent way.
+ *          It may not be implemented or some ports.
+ * @note    The default failure mode is to halt the system with the global
+ *          @p panic_msg variable set to @p NULL.
+ */
+#if !defined(CH_DBG_ENABLE_STACK_CHECK) || defined(__DOXYGEN__)
+#define CH_DBG_ENABLE_STACK_CHECK       TRUE
+#endif
+
+/**
+ * @brief   Debug option, stacks initialization.
+ * @details If enabled then the threads working area is filled with a byte
+ *          value when a thread is created. This can be useful for the
+ *          runtime measurement of the used stack.
+ *
+ * @note    The default is @p FALSE.
+ */
+#if !defined(CH_DBG_FILL_THREADS) || defined(__DOXYGEN__)
+#define CH_DBG_FILL_THREADS             FALSE
+#endif
+
+/**
+ * @brief   Debug option, threads profiling.
+ * @details If enabled then a field is added to the @p Thread structure that
+ *          counts the system ticks occurred while executing the thread.
+ *
+ * @note    The default is @p TRUE.
+ * @note    This debug option is defaulted to TRUE because it is required by
+ *          some test cases into the test suite.
+ */
+#if !defined(CH_DBG_THREADS_PROFILING) || defined(__DOXYGEN__)
+#define CH_DBG_THREADS_PROFILING        TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/**
+ * @name Kernel hooks
+ * @{
+ */
+/*===========================================================================*/
+
+/**
+ * @brief   Threads descriptor structure extension.
+ * @details User fields added to the end of the @p Thread structure.
+ */
+#if !defined(THREAD_EXT_FIELDS) || defined(__DOXYGEN__)
+#define THREAD_EXT_FIELDS                                                   \
+  /* Add threads custom fields here.*/
+#endif
+
+/**
+ * @brief   Threads initialization hook.
+ * @details User initialization code added to the @p chThdInit() API.
+ *
+ * @note    It is invoked from within @p chThdInit() and implicitly from all
+ *          the threads creation APIs.
+ */
+#if !defined(THREAD_EXT_INIT_HOOK) || defined(__DOXYGEN__)
+#define THREAD_EXT_INIT_HOOK(tp) {                                          \
+  /* Add threads initialization code here.*/                                \
+}
+#endif
+
+/**
+ * @brief   Threads finalization hook.
+ * @details User finalization code added to the @p chThdExit() API.
+ *
+ * @note    It is inserted into lock zone.
+ * @note    It is also invoked when the threads simply return in order to
+ *          terminate.
+ */
+#if !defined(THREAD_EXT_EXIT_HOOK) || defined(__DOXYGEN__)
+#define THREAD_EXT_EXIT_HOOK(tp) {                                          \
+  /* Add threads finalization code here.*/                                  \
+}
+#endif
+
+/**
+ * @brief   Context switch hook.
+ * @details This hook is invoked just before switching between threads.
+ */
+#if !defined(THREAD_CONTEXT_SWITCH_HOOK) || defined(__DOXYGEN__)
+#define THREAD_CONTEXT_SWITCH_HOOK(ntp, otp) {                              \
+  /* Context switch code here.*/                                            \
+}
+#endif
+
+/**
+ * @brief   Idle Loop hook.
+ * @details This hook is continuously invoked by the idle thread loop.
+ */
+#if !defined(IDLE_LOOP_HOOK) || defined(__DOXYGEN__)
+#define IDLE_LOOP_HOOK() {                                                  \
+  /* Idle loop code here.*/                                                 \
+}
+#endif
+
+/**
+ * @brief   System tick event hook.
+ * @details This hook is invoked in the system tick handler immediately
+ *          after processing the virtual timers queue.
+ */
+#if !defined(SYSTEM_TICK_EVENT_HOOK) || defined(__DOXYGEN__)
+#define SYSTEM_TICK_EVENT_HOOK() {                                          \
+  /* System tick event code here.*/                                         \
+}
+#endif
+
+/**
+ * @brief   System halt hook.
+ * @details This hook is invoked in case to a system halting error before
+ *          the system is halted.
+ */
+#if !defined(SYSTEM_HALT_HOOK) || defined(__DOXYGEN__)
+void haltErrorCode(void);
+#define SYSTEM_HALT_HOOK() {                                                \
+  /* System halt code here.*/                                               \
+  haltErrorCode();                                                          \
+}
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/* Port-specific settings (override port settings defaulted in chcore.h).    */
+/*===========================================================================*/
+
+/* NVIC VTOR initialization (only offset!) */
+#define CORTEX_VTOR_INIT 0x00008000
+
+#endif  /* _CHCONF_H_ */
diff --git a/devices/PowerManagement/exti.cpp b/devices/PowerManagement/exti.cpp
new file mode 100644
index 0000000..2315337
--- /dev/null
+++ b/devices/PowerManagement/exti.cpp
@@ -0,0 +1,149 @@
+#include <ch.hpp>
+#include <hal.h>
+
+#include <board.h>
+
+#include "exti.hpp"
+
+volatile uint32_t shutdown_now = 0x00000000u;
+volatile uint32_t pathdc_change = 0x00000000u;
+
+EXTConfig extcfg = {
+
+  {
+    /* channel 0 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 1 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 2 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 3 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 4 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 5 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 6 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 7 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 8 */
+    {
+      /* mode */ EXT_MODE_GPIOC | EXT_CH_MODE_AUTOSTART | EXT_CH_MODE_BOTH_EDGES,
+      /* cb   */ pathdc_cb,
+    },
+    /* channel 9 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel A */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel B */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel C */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel D */
+    {
+      /* mode */ EXT_MODE_GPIOC | EXT_CH_MODE_AUTOSTART | EXT_CH_MODE_FALLING_EDGE,
+      /* cb   */ power_down_cb,
+    },
+    /* channel E */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel F */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 10 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 11 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 12 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 13 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 14 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 15 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+    /* channel 16 */
+    {
+      /* mode */ EXT_CH_MODE_DISABLED,
+      /* cb   */ NULL,
+    },
+  }
+};
+
+void power_down_cb(EXTDriver *extp, expchannel_t channel) {
+
+  (void) extp;
+  (void) channel;
+  // make sure we don't accidentally power down when somebody tries to wake us
+  if (!palReadPad(GPIOC, GPIOC_SYS_PD_N))
+    palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW); // indicate that the module needs some time to shut down
+    shutdown_now = 5; // = SHUTDOWN_HANDLE_REQUEST in main.cpp
+}
+
+void pathdc_cb(EXTDriver *extp, expchannel_t channel) {
+
+  (void) extp;
+  (void) channel;
+
+  pathdc_change = ~0u;
+
+}
diff --git a/devices/PowerManagement/exti.hpp b/devices/PowerManagement/exti.hpp
new file mode 100644
index 0000000..ace6689
--- /dev/null
+++ b/devices/PowerManagement/exti.hpp
@@ -0,0 +1,13 @@
+#ifndef EXT_HPP_
+#define EXT_HPP_
+
+extern volatile uint32_t shutdown_now;
+extern volatile uint32_t pathdc_change;
+extern volatile uint32_t pathdc_value;
+
+extern EXTConfig extcfg;
+
+void power_down_cb(EXTDriver *extp, expchannel_t channel);
+void pathdc_cb(EXTDriver *extp, expchannel_t channel);
+
+#endif /* EXT_HPP_ */
diff --git a/devices/PowerManagement/global.hpp b/devices/PowerManagement/global.hpp
new file mode 100644
index 0000000..949a118
--- /dev/null
+++ b/devices/PowerManagement/global.hpp
@@ -0,0 +1,212 @@
+#ifndef AMIRO_GLOBAL_HPP_
+#define AMIRO_GLOBAL_HPP_
+
+#include <hal.h>
+
+#include <array>
+
+#include <amiro/power/adconverter.hpp>
+#include <amiro/bus/i2c/HWI2CDriver.hpp>
+#include <amiro/bus/i2c/VI2CDriver.hpp>
+#include <amiro/bus/i2c/mux/pca9544.hpp>
+#include <amiro/proximity/vcnl4020.hpp>
+#include <amiro/eeprom/at24.hpp>
+#include <amiro/FileSystemInputOutput/FSIOPowerManagement.hpp>
+#include <amiro/input/mpr121.hpp>
+#include <amiro/power/ltc4412.hpp>
+#include <amiro/power/ina219.hpp>
+#include <amiro/power/bq27500.hpp>
+#include <amiro/power/bq24103a.hpp>
+#include <amiro/power/tps62113.hpp>
+#include <amiro/bluetooth/bluetooth.hpp>
+#include <amiro/bluetooth/bluetooth-serial.hpp>
+#include <amiro/bluetooth/bluetooth-wiimote.hpp>
+#include <amiro/serial_reset/iwrap_can_mux.hpp>
+#include <amiro/serial_reset/serial_can_mux.hpp>
+#include <PowerManagement.h>
+#include <userthread.h>
+
+namespace amiro {
+
+class Global final
+{
+public:
+
+  SerialConfig sd1_config{
+    /* speed        */ 115200,
+    /* CR1 register */ 0,
+    /* CR2 register */ 0,
+    /* CR3 register */ 0
+  };
+
+  PWMConfig pwm3_config{
+    /* frequency [Hz]    */ 10000,
+    /* period [ticks]    */ 100,
+    /* callback          */ NULL,
+    /* channels          */ {
+      {PWM_OUTPUT_DISABLED, NULL},
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL},
+      {PWM_OUTPUT_DISABLED, NULL},
+      {PWM_OUTPUT_DISABLED, NULL}
+    },
+    /* CR2 register      */ 0,
+    /* break & dead time */ 0
+  };
+
+  I2CConfig i2c1_config{
+    /* I2C mode                 */ OPMODE_I2C,
+    /* frequency                */ 200000,
+    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2
+  };
+  I2CConfig i2c2_config{
+    /* I2C mode                 */ OPMODE_I2C,
+    /* frequency                */ 200000,
+    /* I2C fast mode duty cycle */ FAST_DUTY_CYCLE_2
+  };
+
+  VCNL4020::VCNL4020Config vcnl4020_config{
+    /* command             */ VCNL4020::ALS_EN | VCNL4020::PROX_EN | VCNL4020::SELFTIMED_EN,
+    /* ambient parameter   */ VCNL4020::AMBIENT_RATE_2 | VCNL4020::AMBIENT_AUTO_OFFSET | VCNL4020::AMBIENT_AVG_32,
+    /* IR LED current [mA] */ 200u,
+    /* proximity rate      */ VCNL4020::PROX_RATE_7_8125
+  };
+
+  adcsample_t adc1_buffer[1];
+  ADCConversionGroup adc1_conversion_group{
+    /* buffer type        */ TRUE, // TRUE=continuous; FALSE=linear
+    /* number of channels */ 1,
+    /* callback function  */ NULL,
+    /* error callback     */ NULL,
+    /* CR1                */ 0,
+    /* CR2                */ ADC_CR2_SWSTART | ADC_CR2_CONT,
+    /* SMPR1              */ 0,
+    /* SMPR2              */ ADC_SMPR2_SMP_AN9(ADC_SAMPLE_480),
+    /* HTR                */ ADC_HTR_HT,
+    /* LTR                */ 0,
+    /* SQR1               */ ADC_SQR1_NUM_CH(1),
+    /* SQR2               */ 0,
+    /* SQR3               */ ADC_SQR3_SQ1_N(ADC_CHANNEL_IN9)
+  };
+  ADConverter adc1_vsys;
+
+  HWI2CDriver HW_I2C1;
+  HWI2CDriver HW_I2C2;
+
+  AT24 at24c01;
+  fileSystemIo::FSIOPowerManagement memory;
+
+  LTC4412<(uintptr_t)GPIOC, GPIOC_PATH_DC> ltc4412;
+
+  std::array<INA219::Driver,5> ina219;
+
+  std::array<BQ27500::Driver,2> bq27500;
+
+private:
+  BQ24103A<(uintptr_t)GPIOC, GPIOC_CHARGE_EN1_N, (uintptr_t)GPIOC, GPIOC_CHARGE_STAT1A> bq24103a_p7;
+  BQ24103A<(uintptr_t)GPIOD, GPIOD_CHARGE_EN2_N, (uintptr_t)GPIOB, GPIOB_CHARGE_STAT2A> bq24103a_p8;
+public:
+  std::array<BaseBQ24103A*,2> bq24103a;
+
+  TPS62113<(uintptr_t)GPIOA, GPIOA_SYS_REG_EN> tps62113_vio18_33_50;
+  TPS62113<(uintptr_t)GPIOB, GPIOB_POWER_EN> tps62113_vsys42;
+
+  BLUETOOTH wt12;
+
+  std::array<PCA9544<true>, 2> HW_PCA9544;
+
+  std::array<VI2CDriver, 4> V_I2C1;
+  std::array<VI2CDriver, 4> V_I2C2;
+
+  std::array<VCNL4020, 8> vcnl4020;
+
+  MPR121::MPR121Config mpr121_run_config{
+    /* global_config  */ MPR121::CDT_1 | MPR121::SFI_10 | MPR121::ESI_32 | MPR121::FFI_18 | 16,
+    /* ele_config     */ MPR121::CL_ON_ALL | MPR121::ELEPROX_0 | 4,
+    /* auto_config    */ MPR121::FFI_18 | MPR121::RETRY_2 | MPR121::BVA_ON_ALL | MPR121::AC_RECONF_EN | MPR121::AC_ENABLE,
+    /* up_side_limit  */ 0x96u,
+    /* low_side_limit */ 0x58u,
+    /* target_level   */ 0x68u
+  };
+  MPR121::MPR121Config mpr121_stdby_config{
+    /* global_config  */ MPR121::CDT_1 | MPR121::SFI_10 | MPR121::ESI_32 | MPR121::FFI_18 | 0,
+    /* ele_config     */ MPR121::CL_ON_ALL | MPR121::ELEPROX_0 | 0, /* TODO: hack: turn buttons off for standby */
+    /* auto_config    */ MPR121::FFI_18 | MPR121::RETRY_2 | MPR121::BVA_ON_ALL | MPR121::AC_RECONF_EN | MPR121::AC_ENABLE,
+    /* up_side_limit  */ 0x96u,
+    /* low_side_limit */ 0x58u,
+    /* target_level   */ 0x68u
+  };
+  MPR121 mpr121;
+
+  IwrapCanMux iwrapcanmux1;
+  SerialCanMux sercanmux1;
+
+  PowerManagement robot;
+
+  UserThread userThread;
+
+  uint8_t shellTermID;
+
+public:
+  Global() :
+    adc1_vsys(ADCD1, adc1_conversion_group, 9000000),
+    HW_I2C1(&I2CD1), HW_I2C2(&I2CD2),
+    at24c01(0x400u / 0x08u, 0x08u, 500u, &HW_I2C2),
+    memory(at24c01, /*BMSV*/ 1, /*bmsv*/ 2, /*HMV*/ 1, /*hmv*/ 1),
+    ina219{{/* VDD      */ INA219::Driver(HW_I2C2, 0x45u),
+            /* VIO 1.8V */ INA219::Driver(HW_I2C1, 0x44u),
+            /* VIO 3.3V */ INA219::Driver(HW_I2C1, 0x40u),
+            /* VIO 4.2V */ INA219::Driver(HW_I2C2, 0x40u),
+            /* VIO 5.0V */ INA219::Driver(HW_I2C2, 0x44u)}
+          },
+    bq27500{{/* connector P7 */ BQ27500::Driver(HW_I2C2, *GPIOC, GPIOC_GAUGE_BATGD1_N, *GPIOC, GPIOC_GAUGE_BATLOW1),
+             /* connector P8 */ BQ27500::Driver(HW_I2C1, *GPIOB, GPIOB_GAUGE_BATGD2_N, *GPIOB, GPIOB_GAUGE_BATLOW2)}
+           },
+    bq24103a_p7(), bq24103a_p8(),
+    bq24103a{{&bq24103a_p7,
+              &bq24103a_p8}
+            },
+    tps62113_vio18_33_50(), tps62113_vsys42(),
+    wt12(&UARTD3),
+    HW_PCA9544{{PCA9544<true>(&HW_I2C1, 0x07u),
+                PCA9544<true>(&HW_I2C2, 0x07u)}
+              },
+    V_I2C1{{VI2CDriver(&HW_PCA9544[0], 0),
+            VI2CDriver(&HW_PCA9544[0], 1),
+            VI2CDriver(&HW_PCA9544[0], 2),
+            VI2CDriver(&HW_PCA9544[0], 3)}
+          },
+    V_I2C2{{VI2CDriver(&HW_PCA9544[1], 0),
+            VI2CDriver(&HW_PCA9544[1], 1),
+            VI2CDriver(&HW_PCA9544[1], 2),
+            VI2CDriver(&HW_PCA9544[1], 3)}
+          },
+    vcnl4020{{/* rear left        */ VCNL4020(&V_I2C1[1], &vcnl4020_config),
+              /* left side rear   */ VCNL4020(&V_I2C1[3], &vcnl4020_config),
+              /* left side front  */ VCNL4020(&V_I2C1[2], &vcnl4020_config),
+              /* front left       */ VCNL4020(&V_I2C2[0], &vcnl4020_config),
+              /* front right      */ VCNL4020(&V_I2C2[1], &vcnl4020_config),
+              /* right side front */ VCNL4020(&V_I2C2[3], &vcnl4020_config),
+              /* right side rear  */ VCNL4020(&V_I2C2[2], &vcnl4020_config),
+              /* rear right       */ VCNL4020(&V_I2C1[0], &vcnl4020_config)}
+            },
+    mpr121(&HW_I2C2, 0),
+    iwrapcanmux1(&SD1, &CAND1, CAN::POWER_MANAGEMENT_ID),
+    sercanmux1(&SD1, &CAND1, CAN::POWER_MANAGEMENT_ID),
+    robot(&CAND1),
+    userThread()
+  {
+    memset(this->adc1_buffer, 0, sizeof(adc1_buffer));
+
+    return;
+  }
+
+  ~Global()
+  {
+    return;
+  }
+
+};
+
+} // end of namespace amiro
+
+#endif /* AMIRO_GLOBAL_HPP_ */
diff --git a/devices/PowerManagement/halconf.h b/devices/PowerManagement/halconf.h
new file mode 100644
index 0000000..f08649d
--- /dev/null
+++ b/devices/PowerManagement/halconf.h
@@ -0,0 +1,283 @@
+#ifndef _HALCONF_H_
+#define _HALCONF_H_
+
+#include "mcuconf.h"
+
+/**
+ * @brief   Enables the TM subsystem.
+ */
+#if !defined(HAL_USE_TM) || defined(__DOXYGEN__)
+#define HAL_USE_TM                  FALSE
+#endif
+
+/**
+ * @brief   Enables the PAL subsystem.
+ */
+#if !defined(HAL_USE_PAL) || defined(__DOXYGEN__)
+#define HAL_USE_PAL                 TRUE
+#endif
+
+/**
+ * @brief   Enables the ADC subsystem.
+ */
+#if !defined(HAL_USE_ADC) || defined(__DOXYGEN__)
+#define HAL_USE_ADC                 TRUE
+#endif
+
+/**
+ * @brief   Enables the CAN subsystem.
+ */
+#if !defined(HAL_USE_CAN) || defined(__DOXYGEN__)
+#define HAL_USE_CAN                 TRUE
+#endif
+
+/**
+ * @brief   Enables the EXT subsystem.
+ */
+#if !defined(HAL_USE_EXT) || defined(__DOXYGEN__)
+#define HAL_USE_EXT                 TRUE
+#endif
+
+/**
+ * @brief   Enables the GPT subsystem.
+ */
+#if !defined(HAL_USE_GPT) || defined(__DOXYGEN__)
+#define HAL_USE_GPT                 FALSE
+#endif
+
+/**
+ * @brief   Enables the I2C subsystem.
+ */
+#if !defined(HAL_USE_I2C) || defined(__DOXYGEN__)
+#define HAL_USE_I2C                 TRUE
+#endif
+
+/**
+ * @brief   Enables the ICU subsystem.
+ */
+#if !defined(HAL_USE_ICU) || defined(__DOXYGEN__)
+#define HAL_USE_ICU                 FALSE
+#endif
+
+/**
+ * @brief   Enables the MAC subsystem.
+ */
+#if !defined(HAL_USE_MAC) || defined(__DOXYGEN__)
+#define HAL_USE_MAC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the MMC_SPI subsystem.
+ */
+#if !defined(HAL_USE_MMC_SPI) || defined(__DOXYGEN__)
+#define HAL_USE_MMC_SPI             FALSE
+#endif
+
+/**
+ * @brief   Enables the PWM subsystem.
+ */
+#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__)
+#define HAL_USE_PWM                 TRUE
+#endif
+
+/**
+ * @brief   Enables the RTC subsystem.
+ */
+#if !defined(HAL_USE_RTC) || defined(__DOXYGEN__)
+#define HAL_USE_RTC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the SDC subsystem.
+ */
+#if !defined(HAL_USE_SDC) || defined(__DOXYGEN__)
+#define HAL_USE_SDC                 FALSE
+#endif
+
+/**
+ * @brief   Enables the SERIAL subsystem.
+ */
+#if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__)
+#define HAL_USE_SERIAL              TRUE
+#endif
+
+/**
+ * @brief   Enables the SERIAL over USB subsystem.
+ */
+#if !defined(HAL_USE_SERIAL_USB) || defined(__DOXYGEN__)
+#define HAL_USE_SERIAL_USB          FALSE
+#endif
+
+/**
+ * @brief   Enables the SPI subsystem.
+ */
+#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__)
+#define HAL_USE_SPI                 FALSE
+#endif
+
+/**
+ * @brief   Enables the UART subsystem.
+ */
+#if !defined(HAL_USE_UART) || defined(__DOXYGEN__)
+#define HAL_USE_UART                TRUE
+#endif
+
+/**
+ * @brief   Enables the USB subsystem.
+ */
+#if !defined(HAL_USE_USB) || defined(__DOXYGEN__)
+#define HAL_USE_USB                 FALSE
+#endif
+
+/*===========================================================================*/
+/* ADC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables synchronous APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(ADC_USE_WAIT) || defined(__DOXYGEN__)
+#define ADC_USE_WAIT                TRUE
+#endif
+
+/**
+ * @brief   Enables the @p adcAcquireBus() and @p adcReleaseBus() APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(ADC_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define ADC_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+/*===========================================================================*/
+/* CAN driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Sleep mode related APIs inclusion switch.
+ */
+#if !defined(CAN_USE_SLEEP_MODE) || defined(__DOXYGEN__)
+#define CAN_USE_SLEEP_MODE          TRUE
+#endif
+
+/*===========================================================================*/
+/* I2C driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables the mutual exclusion APIs on the I2C bus.
+ */
+#if !defined(I2C_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define I2C_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+/*===========================================================================*/
+/* MAC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables an event sources for incoming packets.
+ */
+#if !defined(MAC_USE_ZERO_COPY) || defined(__DOXYGEN__)
+#define MAC_USE_ZERO_COPY           FALSE
+#endif
+
+/**
+ * @brief   Enables an event sources for incoming packets.
+ */
+#if !defined(MAC_USE_EVENTS) || defined(__DOXYGEN__)
+#define MAC_USE_EVENTS              TRUE
+#endif
+
+/*===========================================================================*/
+/* MMC_SPI driver related settings.                                          */
+/*===========================================================================*/
+
+/**
+ * @brief   Delays insertions.
+ * @details If enabled this options inserts delays into the MMC waiting
+ *          routines releasing some extra CPU time for the threads with
+ *          lower priority, this may slow down the driver a bit however.
+ *          This option is recommended also if the SPI driver does not
+ *          use a DMA channel and heavily loads the CPU.
+ */
+#if !defined(MMC_NICE_WAITING) || defined(__DOXYGEN__)
+#define MMC_NICE_WAITING            TRUE
+#endif
+
+/*===========================================================================*/
+/* SDC driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Number of initialization attempts before rejecting the card.
+ * @note    Attempts are performed at 10mS intervals.
+ */
+#if !defined(SDC_INIT_RETRY) || defined(__DOXYGEN__)
+#define SDC_INIT_RETRY              100
+#endif
+
+/**
+ * @brief   Include support for MMC cards.
+ * @note    MMC support is not yet implemented so this option must be kept
+ *          at @p FALSE.
+ */
+#if !defined(SDC_MMC_SUPPORT) || defined(__DOXYGEN__)
+#define SDC_MMC_SUPPORT             FALSE
+#endif
+
+/**
+ * @brief   Delays insertions.
+ * @details If enabled this options inserts delays into the MMC waiting
+ *          routines releasing some extra CPU time for the threads with
+ *          lower priority, this may slow down the driver a bit however.
+ */
+#if !defined(SDC_NICE_WAITING) || defined(__DOXYGEN__)
+#define SDC_NICE_WAITING            TRUE
+#endif
+
+/*===========================================================================*/
+/* SERIAL driver related settings.                                           */
+/*===========================================================================*/
+
+/**
+ * @brief   Default bit rate.
+ * @details Configuration parameter, this is the baud rate selected for the
+ *          default configuration.
+ */
+#if !defined(SERIAL_DEFAULT_BITRATE) || defined(__DOXYGEN__)
+#define SERIAL_DEFAULT_BITRATE      38400
+#endif
+
+/**
+ * @brief   Serial buffers size.
+ * @details Configuration parameter, you can change the depth of the queue
+ *          buffers depending on the requirements of your application.
+ * @note    The default is 64 bytes for both the transmission and receive
+ *          buffers.
+ */
+#if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__)
+#define SERIAL_BUFFERS_SIZE         16
+#endif
+
+/*===========================================================================*/
+/* SPI driver related settings.                                              */
+/*===========================================================================*/
+
+/**
+ * @brief   Enables synchronous APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(SPI_USE_WAIT) || defined(__DOXYGEN__)
+#define SPI_USE_WAIT                TRUE
+#endif
+
+/**
+ * @brief   Enables the @p spiAcquireBus() and @p spiReleaseBus() APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(SPI_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define SPI_USE_MUTUAL_EXCLUSION    TRUE
+#endif
+
+#endif /* _HALCONF_H_ */
diff --git a/devices/PowerManagement/main.cpp b/devices/PowerManagement/main.cpp
new file mode 100644
index 0000000..39e02ee
--- /dev/null
+++ b/devices/PowerManagement/main.cpp
@@ -0,0 +1,1161 @@
+#ifndef IN_CCM
+/*
+ * @brief Makro to store data in the core coupled memory (ccm).
+ *        Example:
+ *        int compute_buffer[128] IN_CCM;
+ *
+ * @note The ccm is not connected to any bus system.
+ */
+#define IN_CCM  __attribute__((section(".ccm"))) __attribute__((aligned(4)))
+#endif
+
+#ifndef IN_ETH
+/*
+ * @brief Makro to store data in the ethernet memory (eth).
+ *        Example:
+ *        int dma_buffer[128] IN_ETH;
+ *
+ * @note The eth is a dedicated memory block with its own DMA controller.
+ */
+#define IN_ETH  __attribute__((section(".eth"))) __attribute__((aligned(4)))
+#endif
+
+#define BL_CALLBACK_TABLE_ADDR  (0x08000000 + 0x01C0)
+#define BL_MAGIC_NUMBER         ((uint32_t)0xFF669900u)
+
+#define SHUTDOWN_NONE             0
+#define SHUTDOWN_TRANSPORTATION   1
+#define SHUTDOWN_DEEPSLEEP        2
+#define SHUTDOWN_HIBERNATE        3
+#define SHUTDOWN_RESTART          4
+#define SHUTDOWN_HANDLE_REQUEST   5
+
+#include <ch.hpp>
+#include <shell.h>
+#include <chprintf.h>
+#include <wakeup.h>
+#include <cstdlib>
+#include <cstring>
+#include <amiro/util/util.h>
+#include <global.hpp>
+#include <exti.hpp>
+
+using namespace amiro;
+using namespace constants::PowerManagement;
+
+Global global;
+
+void shutdownTimeoutISR(void *arg) {
+
+  (void) arg;
+
+}
+
+void systemStop() {
+
+//  VirtualTimer shutdownTimeout;
+  uint8_t i;
+
+  // tell all boards that it's time to shut down
+  global.robot.broadcastShutdown();
+
+  global.userThread.requestTerminate();
+  global.userThread.wait();
+
+  // kill bluetooth
+  boardBluetoothSetState(0);
+
+  global.adc1_vsys.requestTerminate();
+  global.adc1_vsys.wait();
+
+  for (i = 0; i < global.vcnl4020.size(); ++i) {
+    global.vcnl4020[i].requestTerminate();
+    global.vcnl4020[i].wait();
+  }
+
+  for (i = 0; i < global.bq27500.size(); ++i) {
+    global.bq27500[i].requestTerminate();
+    global.bq27500[i].wait();
+  }
+
+  for (i = 0; i < global.ina219.size(); ++i) {
+    global.ina219[i].requestTerminate();
+    global.ina219[i].wait();
+  }
+
+//  boardWriteIoPower(0);
+  global.mpr121.configure(&global.mpr121_stdby_config);
+  /* cannot shut down touch, b/c we need it to
+   * clear any interrupt, so WKUP is not blocked
+   */
+
+  // stop I²C
+  for (i = 0; i < global.V_I2C1.size(); ++i)
+    global.V_I2C1[i].stop();
+  for (i = 0; i < global.V_I2C2.size(); ++i)
+    global.V_I2C2[i].stop();
+
+  global.mpr121.requestTerminate();
+  global.mpr121.wait();
+
+  global.HW_I2C2.stop();
+  global.HW_I2C1.stop();
+
+  // stop all threads
+  global.robot.terminate();
+
+//  // 60 sec timeout
+//  palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_HIGH);
+//  chVTSet(&shutdownTimeout, MS2ST(60000), shutdownTimeoutISR, NULL);
+//  // wait for all boards to release SYS_INT_N
+//  while (palReadPad(GPIOC, GPIOC_SYS_INT_N)!=PAL_HIGH && chVTIsArmedI(&shutdownTimeout)) {
+//    BaseThread::sleep(MS2ST(1)); /* must sleep for VT, else it will never fire */
+//  }
+//  chVTReset(&shutdownTimeout);
+
+//  chprintf((BaseSequentialStream*) &SD1, "Stop\n");
+//  boardWriteSystemPower(0);
+//  boardWriteLed(1);
+//  boardStop(0x00, 0x00);
+
+//  /*
+//   * HSI-PLL domain now.
+//   */
+//  //chprintf((BaseSequentialStream*) &SD1, "After Stop\n");
+//  boardWriteLed(1);
+
+//  while (true)
+//    BaseThread::sleep(MS2ST(250));
+
+  return;
+}
+
+void systemShutdown() {
+
+  VirtualTimer shutdownTimeout;
+  uint8_t i;
+
+  // tell all boards that it's time to shut down
+  global.robot.broadcastShutdown();
+
+  // wait a little to make sure all boards got the message and had time to pull their SYS_PD_N pins down
+  BaseThread::sleep(MS2ST(500));
+
+  // stop the user thread
+  global.userThread.requestTerminate();
+  global.userThread.wait();
+
+  // kill bluetooth
+  boardBluetoothSetState(0);
+
+  // stop all threads
+  global.robot.terminate();
+
+  global.adc1_vsys.requestTerminate();
+  global.adc1_vsys.wait();
+
+  for (i = 0; i < global.vcnl4020.size(); ++i) {
+    global.vcnl4020[i].requestTerminate();
+    global.vcnl4020[i].wait();
+  }
+
+  for (i = 0; i < global.bq27500.size(); ++i) {
+    global.bq27500[i].requestTerminate();
+    global.bq27500[i].wait();
+  }
+
+  for (i = 0; i < global.ina219.size(); ++i) {
+    global.ina219[i].requestTerminate();
+    global.ina219[i].wait();
+  }
+
+  // 60 sec timeout
+  chVTSet(&shutdownTimeout, MS2ST(60000), shutdownTimeoutISR, NULL);
+
+  // wait for all boards to release SYS_PD_N
+  while (!palReadPad(GPIOC, GPIOC_SYS_PD_N) && chVTIsArmedI(&shutdownTimeout))
+    BaseThread::sleep(MS2ST(1)); /* must sleep for VT, else it will never fire */
+
+  chVTReset(&shutdownTimeout);
+  boardWriteIoPower(0);
+  global.mpr121.configure(&global.mpr121_stdby_config);
+  /* cannot shut down touch, b/c we need it to
+   * clear any interrupt, so WKUP is not blocked
+   */
+
+  // stop I²C
+  for (i = 0; i < global.V_I2C1.size(); ++i)
+    global.V_I2C1[i].stop();
+  for (i = 0; i < global.V_I2C2.size(); ++i)
+    global.V_I2C2[i].stop();
+
+  boardWriteSystemPower(0);
+  boardStandby();
+
+}
+
+
+void boardPeripheryCheck(BaseSequentialStream *chp) {
+
+#ifndef AMIRO_NSELFTEST
+  chprintf(chp, "\nCHECK: START\n");
+  msg_t result = 0;
+
+    // Check the proximitysensors
+  for (uint8_t i = 0; i < global.vcnl4020.size(); i++) {
+    result = global.vcnl4020[i].getCheck();
+    if (result == global.vcnl4020[i].CHECK_OK)
+      chprintf(chp, "VCNL4020: %d OK\n", i);
+    else
+      chprintf(chp, "VCNL4020: %d FAIL\n", i);
+  }
+  chprintf(chp, "----------------------------------------\n");
+
+  // check the PowerPath controller
+  chprintf(chp, "\n");
+  if (global.ltc4412.isPluggedIn())
+    chprintf(chp, "LTC4412: plugged in\n");
+  else
+    chprintf(chp, "LTC4412: not plugged in\n");
+  chprintf(chp, "----------------------------------------\n");
+
+  // Check the eeprom
+  result = global.memory.getCheck();
+  if ( result != global.memory.OK)
+    chprintf(chp, "Memory Structure: FAIL\n");
+  else
+    chprintf(chp, "Memory Structure: OK\n");
+  chprintf(chp, "----------------------------------------\n");
+
+  // Check the power monitors
+  INA219::BusVoltage bus_voltage;
+  chprintf(chp, "\n");
+  chprintf(chp, "INA219:\n");
+  chprintf(chp, "\tVDD (3.3V):\n");
+  uint8_t result_ina219_vdd = global.ina219[INA_VDD].selftest();
+  chprintf(chp, "->\t");
+  if (result_ina219_vdd == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "not implemented");
+  else if (result_ina219_vdd != INA219::Driver::ST_OK)
+    chprintf(chp, "FAIL (error code 0x%02X)", result_ina219_vdd);
+  else
+    chprintf(chp, "OK");
+
+  chprintf(chp, "\n\n");
+  chprintf(chp, "\tVIO1.8:\n");
+  uint8_t result_ina219_vio18 = global.ina219[INA_VIO18].selftest();
+  chprintf(chp, "->\t");
+  if (result_ina219_vio18 == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "not implemented");
+  else if (result_ina219_vio18 != INA219::Driver::ST_OK)
+    chprintf(chp, "FAIL (error code 0x%02X)", result_ina219_vio18);
+  else
+    chprintf(chp, "OK");
+
+  chprintf(chp, "\n\n");
+  chprintf(chp, "\tVIO3.3:\n");
+  uint8_t result_ina219_vio33 = global.ina219[INA_VIO33].selftest();
+  chprintf(chp, "->\t");
+  if (result_ina219_vio33 == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "not implemented");
+  else if (result_ina219_vio33 != INA219::Driver::ST_OK)
+    chprintf(chp, "FAIL (error code 0x%02X)", result_ina219_vio33);
+  else
+    chprintf(chp, "OK");
+
+  chprintf(chp, "\n\n");
+  chprintf(chp, "\tVIO4.2:\n");
+  uint8_t result_ina219_vio42 = global.ina219[INA_VIO42].selftest();
+  chprintf(chp, "->\t");
+  if (result_ina219_vio42 == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "not implemented");
+  else if (result_ina219_vio42 != INA219::Driver::ST_OK)
+    chprintf(chp, "FAIL (error code 0x%02X)", result_ina219_vio42);
+  else
+    chprintf(chp, "OK");
+
+  bus_voltage = global.ina219[INA_VIO42].readBusVoltage();
+  chprintf(chp, "\n\n");
+  chprintf(chp, "\tVIO5.0:\n");
+  uint8_t result_ina219_vio50 = global.ina219[INA_VIO50].selftest();
+  chprintf(chp, "->\t");
+  if (result_ina219_vio50 == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "not implemented");
+  else if (result_ina219_vio50 != INA219::Driver::ST_OK)
+    chprintf(chp, "FAIL (error code 0x%02X)", result_ina219_vio50);
+  else
+    chprintf(chp, "OK");
+
+  chprintf(chp, "\n\n");
+  result = result_ina219_vdd | result_ina219_vio18 | result_ina219_vio33 | result_ina219_vio42 | result_ina219_vio50;
+  if (result == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "->\tINA219: not implemented\n");
+  else
+    chprintf(chp, "->\tINA219: %s\n", (result != INA219::Driver::ST_OK)? "FAIL" : "OK");
+  chprintf(chp, "----------------------------------------\n");
+
+  // check the fuel gauges
+  chprintf(chp, "\n");
+  chprintf(chp, "BQ27500:\n");
+  chprintf(chp, "\tP7:\n");
+  msg_t result_bq27500_p7 = global.bq27500[BAT_P7].selftest();
+  chprintf(chp, "->\tP7: ");
+  if (result == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "not implemented");
+  else if (result_bq27500_p7 == BQ27500::Driver::ST_ABORT_NO_BAT)
+    chprintf(chp, "ABORT (no battery detected)");
+  else if (result_bq27500_p7 != BQ27500::Driver::ST_OK)
+    chprintf(chp, "FAIL (error code 0x%02X)", result);
+  else
+    chprintf(chp, "OK");
+
+  chprintf(chp, "\n\n");
+  chprintf(chp, "\tP8:\n");
+  msg_t result_bq27500_p8 = global.bq27500[BAT_P8].selftest();
+  chprintf(chp, "->\tP8: ");
+  if (result == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "not implemented");
+  else if (result_bq27500_p8 == BQ27500::Driver::ST_ABORT_NO_BAT)
+      chprintf(chp, "ABORT (no battery detected)");
+  else if (result_bq27500_p8 != BQ27500::Driver::ST_OK)
+    chprintf(chp, "FAIL (error code 0x%02X)", result);
+  else
+    chprintf(chp, "OK");
+
+  chprintf(chp, "\n");
+  result = result_bq27500_p7 | result_bq27500_p8;
+  if (result == BaseSensor<>::NOT_IMPLEMENTED)
+    chprintf(chp, "\n->\tBQ27500: not implemented\n");
+  else
+    chprintf(chp, "\n->\tBQ27500: %s\n", (result != BQ27500::Driver::ST_OK)? "FAIL" : "OK");
+  chprintf(chp, "----------------------------------------\n");
+
+  // check the chargers
+  chprintf(chp, "\n");
+  chprintf(chp, "BQ24103A:\n");
+  if (!global.ltc4412.isPluggedIn())
+    chprintf(chp, "This test is skipped. Rerun when plugged in.\n");
+  else {
+    bool status1, status2, status3;
+    chprintf(chp, "\tP7:\n");
+    bool status_p7 = global.bq27500[BAT_P7].isBatteryGood();
+    chprintf(chp, "Battery good: %s\n", (status_p7? "yes" : "no"));
+    if (!status_p7) {
+      chprintf(chp, "-> Rerun test with (another) battery!\n");
+      status_p7 = true;
+    } else {
+      status1 = global.bq24103a[BAT_P7]->isCharging();
+      chprintf(chp, "status:%scharging\n", (status1? " " : " not "));
+      chprintf(chp, "%sabling charger...\n", (status1? "dis" : "en"));
+      global.bq24103a[BAT_P7]->enable(!status1);
+      BaseThread::sleep(MS2ST(1500));
+      status2 = global.bq24103a[BAT_P7]->isCharging();
+      chprintf(chp, "status:%scharging\n", (status2? " " : " not "));
+      chprintf(chp, "%sabling charger...\n", (!status1? "dis" : "en"));
+      global.bq24103a[BAT_P7]->enable(status1);
+      BaseThread::sleep(MS2ST(1500));
+      status3 = global.bq24103a[BAT_P7]->isCharging();
+      chprintf(chp, "status:%scharging\n", (status3? " " : " not "));
+      status_p7 = status2 != status1 && status3 == status1;
+      chprintf(chp, "->\t");
+      if (status_p7) {
+        chprintf(chp, "OK");
+      } else {
+        chprintf(chp, "FAIL");
+      }
+      chprintf(chp, "\n");
+    }
+    chprintf(chp, "\n");
+    chprintf(chp, "\tP8:\n");
+    bool status_p8 = global.bq27500[BAT_P8].isBatteryGood();
+    chprintf(chp, "Battery good: %s\n", (status_p8? "yes" : "no"));
+    if (!status_p8) {
+      chprintf(chp, "-> Rerun test with (another) battery!\n");
+      status_p8 = true;
+    } else {
+      status1 = global.bq24103a[BAT_P8]->isCharging();
+      chprintf(chp, "status:%scharging\n", (status1? " " : " not "));
+      chprintf(chp, "%sabling charger...\n", (status1? "dis" : "en"));
+      global.bq24103a[BAT_P8]->enable(!status1);
+      BaseThread::sleep(MS2ST(1500));
+      status2 = global.bq24103a[BAT_P8]->isCharging();
+      chprintf(chp, "status:%scharging\n", (status2? " " : " not "));
+      chprintf(chp, "%sabling charger...\n", (!status1? "dis" : "en"));
+      global.bq24103a[BAT_P8]->enable(status1);
+      BaseThread::sleep(MS2ST(1500));
+      status3 = global.bq24103a[BAT_P8]->isCharging();
+      chprintf(chp, "status:%scharging\n", (status3? " " : "not "));
+      status_p8 = status2 != status1 && status3 == status1;
+      chprintf(chp, "->\t");
+      if (status_p8)
+        chprintf(chp, "OK");
+      else
+        chprintf(chp, "FAIL");
+      chprintf(chp, "\n");
+    }
+    chprintf(chp, "\n");
+    chprintf(chp, "->\tBQ24103A: ");
+    if (status_p7 && status_p8)
+      chprintf(chp, "OK");
+    else
+      chprintf(chp, "FAIL");
+    chprintf(chp, "\n");
+  }
+  chprintf(chp, "----------------------------------------\n");
+
+  // check Bluetooth (TODO: move this check to driver)
+  chprintf(chp, "\n");
+  chprintf(chp, "WT12-A-AI:\n");
+
+  chprintf(chp, "testing for MUX mode:\t");
+
+  if (global.wt12.bluetoothIsMuxMode()) {
+    chprintf(chp, "PASSED\n");
+  } else {
+    chprintf(chp, "FAILED -> setting MUX mode now\n");
+    /* initialise the WT-12 bluetooth chip on AMIRO (Please, run this processes once) */
+    global.wt12.bluetoothSendCommand("SET BT AUTH *");
+    global.wt12.bluetoothSendCommand("SET BT PAIR *");
+    global.wt12.bluetoothSendCommand("SET BT SSP 3 0");
+    global.wt12.bluetoothEnableMux();
+    global.wt12.bluetoothReset();
+  }
+
+  global.wt12.bluetoothSendCommand("TEMP");
+  global.wt12.bluetoothSendCommand("SET");
+
+  chprintf(chp, "----------------------------------------\n");
+
+  // check the buzzer
+  chprintf(chp, "\n");
+  chprintf(chp, "PKLCS1212E4001:\n");
+  chprintf(chp, "you should hear the buzzer for one second...\n");
+  pwmEnableChannel(&PWMD3, 1, 50);
+  BaseThread::sleep(MS2ST(1000));
+  pwmDisableChannel(&PWMD3, 1);
+  chprintf(chp, "----------------------------------------\n");
+
+  chprintf(chp, "CHECK: FINISH\n");
+#endif
+
+  return;
+}
+
+void shellRequestShutdown(BaseSequentialStream* chp, int __unused argc, char __unused *argv[]) {
+
+  chprintf(chp, "shellRequestShutdown\n");
+
+  /* if no argument was given, print some help text */
+  if (argc == 0 || strcmp(argv[0],"help") == 0) {
+    chprintf(chp, "\tUSAGE:\n");
+    chprintf(chp, "> shutdown <type>\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\ttype\n");
+    chprintf(chp, "The type of shutdown to perform.\n");
+    chprintf(chp, "Choose one of the following types:\n");
+    chprintf(chp, "  transportation - Ultra low-power mode with all wakeups disabled.\n");
+    chprintf(chp, "                   The robot can not be charged.\n");
+    chprintf(chp, "  deepsleep      - Ultra low-power mode with several wakeups enabled.\n");
+    chprintf(chp, "                   The robot can only be charged via the power plug.\n");
+    chprintf(chp, "  hibernate      - Medium low-power mode, but with full charging capabilities.\n");
+    chprintf(chp, "  restart        - Performs a system restart.\n");
+    chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
+    chprintf(chp, "\n");
+    return;
+  }
+
+  if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) {
+    shutdown_now = SHUTDOWN_TRANSPORTATION;
+    chprintf(chp, "shutdown to transportation mode initialized\n");
+  } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) {
+    shutdown_now = SHUTDOWN_DEEPSLEEP;
+    chprintf(chp, "shutdown to deepsleep mode initialized\n");
+  } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) {
+    shutdown_now = SHUTDOWN_HIBERNATE;
+    chprintf(chp, "shutdown to hibernate mode initialized\n");
+  } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) {
+    chprintf(chp, "restart initialized\n");
+    shutdown_now = SHUTDOWN_RESTART;
+  } else {
+    chprintf(chp, "ERROR: unknown argument!\n");
+    shutdown_now = SHUTDOWN_NONE;
+  }
+
+  return;
+}
+
+void shellRequestResetMemory(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestInitMemory\n");
+
+  msg_t res = global.memory.resetMemory();
+
+  if ( res != global.memory.OK)
+    chprintf(chp, "Memory Init: FAIL\n");
+  else
+    chprintf(chp, "Memory Init: OK\n");
+}
+
+void shellRequestGetBoardId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestGetBoardId\n");
+
+  uint8_t id = 0xFFu;
+  msg_t res = global.memory.getBoardId(&id);
+  if (res != global.memory.OK)
+    chprintf(chp, "Get Board ID: FAIL\n");
+  else
+    chprintf(chp, "Get Board ID: %u\n", id);
+}
+
+void shellRequestSetBoardId(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestSetBoardId\n");
+
+  if (argc == 0) {
+    chprintf(chp, "Usage: %s\n","set_board_id <idx>");
+  } else {
+    msg_t res = global.memory.setBoardId(atoi(argv[0]));
+    if (res != global.memory.OK)
+      chprintf(chp, "Set Board ID: FAIL\n");
+    else
+      chprintf(chp, "Set Board ID: OK\n");
+  }
+
+}
+
+void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) {
+  enum Type {HEX, U8, U16, U32, S8, S16, S32};
+
+  chprintf(chp, "shellRequestReadData\n");
+
+  if (argc < 2 || strcmp(argv[0],"help") == 0)
+  {
+    chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]");
+    chprintf(chp, "\n");
+    chprintf(chp, "\ttype\n");
+    chprintf(chp, "The data type as which to interpret the data.\n");
+    chprintf(chp, "Choose one of the following types:\n");
+    chprintf(chp, "  hex - one byte as hexadecimal value\n");
+    chprintf(chp, "  u8  - unsigned integer (8 bit)\n");
+    chprintf(chp, "  u16 - unsigned integer (16 bit)\n");
+    chprintf(chp, "  u32 - unsigned integer (32 bit)\n");
+    chprintf(chp, "  s8  - signed integer (8 bit)\n");
+    chprintf(chp, "  s16 - signed integer (16 bit)\n");
+    chprintf(chp, "  s32 - signed integer (32 bit)\n");
+    chprintf(chp, "\tstart\n");
+    chprintf(chp, "The first byte to read from the memory.\n");
+    chprintf(chp, "\tcount [default = 1]\n");
+    chprintf(chp, "The number of elements to read.\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\tNOTE\n");
+    chprintf(chp, "Type conversions of this function might fail.\n");
+    chprintf(chp, "If so, use type=hex and convert by hand.\n");
+    chprintf(chp, "\n");
+    return;
+  }
+
+  uint8_t type_size = 0;
+  Type type = HEX;
+  if (strcmp(argv[0],"hex") == 0) {
+    type_size = sizeof(unsigned char);
+    type = HEX;
+  } else if(strcmp(argv[0],"u8") == 0) {
+    type_size = sizeof(uint8_t);
+    type = U8;
+  } else if(strcmp(argv[0],"u16") == 0) {
+    type_size = sizeof(uint16_t);
+    type = U16;
+  } else if(strcmp(argv[0],"u32") == 0) {
+    type_size = sizeof(uint32_t);
+    type = U32;
+  } else if(strcmp(argv[0],"s8") == 0) {
+    type_size = sizeof(int8_t);
+    type = S8;
+  } else if(strcmp(argv[0],"s16") == 0) {
+    type_size = sizeof(int16_t);
+    type = S16;
+  } else if(strcmp(argv[0],"s32") == 0) {
+    type_size = sizeof(int32_t);
+    type = S32;
+  } else {
+    chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
+    return;
+  }
+
+  unsigned int start_byte = atoi(argv[1]);
+
+  unsigned int num_elements = 1;
+  if (argc >= 3) {
+    num_elements = atoi(argv[2]);
+  }
+
+  const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
+  uint8_t buffer[eeprom_size];
+  if (start_byte + (type_size * num_elements) > eeprom_size) {
+    num_elements = (eeprom_size - start_byte) / type_size;
+    chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
+  }
+
+  chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte);
+  uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements);
+
+  if (bytes_read != type_size*num_elements) {
+    chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);
+  }
+
+  for (unsigned int i = 0; i < num_elements; ++i)
+  {
+    switch (type)
+    {
+      case HEX:
+        chprintf(chp, "%02X ", buffer[i]);
+        break;
+      case U8:
+        chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
+        break;
+      case U16:
+        chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
+        break;
+      case U32:
+        chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
+        break;
+      case S8:
+        chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
+        break;
+      case S16:
+        chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
+        break;
+      case S32:
+        chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
+        break;
+      default:
+        break;
+    }
+  }
+  chprintf(chp, "\n");
+
+  return;
+}
+
+void shellRequestSetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestSetVcnlOffset\n");
+  if (argc != 2) {
+    chprintf(chp, "Usage: %s\n","set_vcnl <idx> <offset>");
+    return;
+  }
+
+  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
+  uint16_t vcnlOffset = static_cast<uint16_t>(atoi(argv[1]));
+
+  if (vcnlIdx >= global.vcnl4020.size()) {
+    chprintf((BaseSequentialStream *)&SD1, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
+    return;
+  }
+
+  msg_t res = global.memory.setVcnl4020Offset(vcnlOffset, vcnlIdx);
+  if (res != global.memory.OK) {
+    chprintf(chp, "Set Offset: FAIL\n");
+  } else {
+    chprintf(chp, "Set Offset: OK\n");
+    global.vcnl4020[vcnlIdx].setProximityOffset(vcnlOffset);
+  }
+}
+
+void shellRequestResetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
+  msg_t res = global.memory.OK;
+  for (uint8_t idx = 0; idx < 8; ++idx) {
+    msg_t r = global.memory.setVcnl4020Offset(0, idx);
+    if (r == global.memory.OK) {
+      global.vcnl4020[idx].setProximityOffset(0);
+    } else {
+      chprintf(chp, "Reset Offset %u: FAIL\n", idx);
+      res = r;
+    }
+  }
+
+  if (res == global.memory.OK) {
+    chprintf(chp, "Reset Offset: DONE\n");
+  }
+
+  return;
+}
+
+void shellRequestGetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestGetVcnlOffset\n");
+  if (argc != 1) {
+    chprintf(chp, "Usage: %s\n","get_vcnl_offset <idx>");
+    return;
+  }
+
+  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
+
+  if (vcnlIdx >= global.vcnl4020.size()) {
+    chprintf((BaseSequentialStream *)&SD1, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
+    return;
+  }
+
+  uint16_t vcnlOffset;
+  msg_t res = global.memory.getVcnl4020Offset(&vcnlOffset, vcnlIdx);
+  if (res != global.memory.OK) {
+    chprintf(chp, "Get Offset: FAIL\n");
+  } else {
+    chprintf(chp, "Get Offset: OK \t Offset=%d\n", vcnlOffset);
+  }
+}
+
+void shellRequestCheck(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestCheck\n");
+  boardPeripheryCheck(chp);
+
+}
+
+void shellRequestGetVcnl(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestGetVcnl\n");
+  // Print the sensor information
+  if (argc != 1) {
+    chprintf(chp, "Usage: %s\n","get_vcnl <rep>");
+    return;
+  }
+  for (int32_t rep = 0x00; rep < atoi(argv[0]); ++rep) {
+    for (uint8_t idx = 0x00; idx < global.vcnl4020.size(); idx++) {
+     chprintf(chp, "%d: Ambi %d\tProx raw %d\tProx scaled %d\n", idx, global.vcnl4020[idx].getAmbientLight(), global.vcnl4020[idx].getProximity(), global.vcnl4020[idx].getProximityScaledWoOffset());
+    }
+    chprintf(chp, "\n\n");
+    BaseThread::sleep(MS2ST(250));
+  }
+}
+
+void shellRequestCalib(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestCalib\n");
+  global.robot.calibrate();
+
+}
+
+void shellRequestGetRobotId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
+  chprintf(chp, "shellRequestGetRobotId\n");
+  chprintf(chp, "Robot ID: %u\n", global.robot.getRobotID());
+  if (global.robot.getRobotID() == 0) {
+    chprintf(chp, "Warning: The ID seems to be uninitialized. Is CAN communication working correctly?\n");
+  }
+}
+
+void shellRequestGetSystemLoad(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestGetSystemLoad\n");
+  uint8_t seconds = 1;
+  if (argc >= 1) {
+    seconds = atoi(argv[0]);
+  }
+  chprintf(chp, "measuring CPU load for %u %s...\n", seconds, (seconds>1)? "seconds" : "second");
+
+  const systime_t before = chThdGetTicks(chSysGetIdleThread());
+  BaseThread::sleep(S2ST(seconds));
+  const systime_t after = chThdGetTicks(chSysGetIdleThread());
+  const float usage = 1.0f - (float(after - before) / float(seconds * CH_FREQUENCY));
+
+  chprintf(chp, "CPU load: %3.2f%%\n", usage * 100);
+  const uint32_t memory_total = 0x1C000;
+  const uint32_t memory_load = memory_total - chCoreStatus();
+  chprintf(chp, "RAM load: %3.2f%% (%u / %u Byte)\n", float(memory_load)/float(memory_total) * 100, memory_load, memory_total);
+}
+
+void shellRequestSetCharging(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestSetCharging\n");
+  if (argc < 1) {
+    chprintf(chp, "Usage: %s\n","set_charging <enable>");
+    chprintf(chp, "\n");
+    chprintf(chp, "\tenable\n");
+    chprintf(chp, "Whether to enable (1) or to disable (0) the power path controller of the DiWheelDrive board.\n");
+    chprintf(chp, "\n");
+    return;
+  }
+
+  const bool enable = atoi(argv[0]);
+  chprintf(chp, "%s power path controller\n", enable ? "enabling" : "disabling");
+  global.robot.getPowerStatus().charging_flags.content.diwheeldrive_enable_power_path = enable;
+
+  return;
+}
+
+void shellRequestPrintVCNL(BaseSequentialStream *chp, int argc, char *argv[]) {
+  chprintf(chp, "shellRequestPrintVCNL\n");
+  if (argc < 1) {
+    chprintf(chp, "Warning: no arguments specified.\n");
+    chprintf(chp, "Using default values (prints one measurement).\n");
+    chprintf(chp, "Type 'print_vcnl help' for help.\n");
+    chprintf(chp, "----------------------------------------\n");
+  }
+
+  if (strcmp(argv[0],"help") == 0) {
+    chprintf(chp, "Usage: %s\n","print_vcnl [<seconds>] [<frequency>]");
+    chprintf(chp, "\n");
+    chprintf(chp, "\tseconds\n");
+    chprintf(chp, "Number of seconds to print the VCNL values. (default: 1)\n");
+    chprintf(chp, "\n");
+    chprintf(chp, "\tfrequency\n");
+    chprintf(chp, "Number prints per second. (default: 1)\n");
+    return;
+  }
+
+  const uint32_t seconds = (argc >= 1) ? atoi(argv[0]) : 1;
+  const uint32_t freq = (argc >= 2) ? atoi(argv[1]) : 1;
+
+  uint16_t ambient = 0;
+  uint16_t proximity = 0;
+
+  for (uint32_t loop = 0; loop < seconds*freq; ++loop) {
+    // Print proximity and ambilight values
+    for (uint8_t i = 0x00; i < global.vcnl4020.size(); ++i) {
+      ambient = global.vcnl4020[i].getAmbientLight();
+      proximity = global.vcnl4020[i].getProximity();
+      chprintf(chp, "#%d: ambient=0x%04X\tproximity=0x%04X\n", i, ambient, proximity);
+    }
+    chprintf(chp, "----------------------------------------\n");
+
+    BaseThread::sleep(US2ST(1000000 / freq));
+  }
+
+  return;
+}
+
+void shellSwitchBoardCmd(BaseSequentialStream *chp, int argc, char *argv[]) {
+  if (argc != 1) {
+    chprintf(chp, "Call with decimal numbers: shell_board <idx>\n");
+    return;
+  }
+  uint8_t boardIdx = static_cast<uint8_t>(atoi(argv[0]));
+
+  chprintf(chp, "shellSwitchBoardCmd\n");
+  global.sercanmux1.sendSwitchCmd(boardIdx);
+}
+
+void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *argv[]) {
+  // check the magic number
+  if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) {
+    chprintf(chp, "Bootloader version:  %u.%u.%u\n",
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (3*4))));
+    chprintf(chp, "Callback functions:\n");
+    if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2) {
+      chprintf(chp, "\thibernate:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (4*4))) ? "available" : "unsupported");
+      chprintf(chp, "\tdeepsleep:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (5*4))) ? "available" : "unsupported");
+      chprintf(chp, "\ttransportation: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (6*4))) ? "available" : "unsupported");
+      chprintf(chp, "\trestart:        %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (10*4))) ? "available" : "unsupported");
+    } else if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3) {
+      chprintf(chp, "\thibernate:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (4*4))) ? "available" : "unsupported");
+      chprintf(chp, "\tdeepsleep:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (5*4))) ? "available" : "unsupported");
+      chprintf(chp, "\ttransportation: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (6*4))) ? "available" : "unsupported");
+      chprintf(chp, "\trestart:        %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (7*4))) ? "available" : "unsupported");
+      chprintf(chp, "\thandle request: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (8*4))) ? "available" : "unsupported");
+    }
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader deprecated\n");
+  }
+
+  return;
+}
+
+static const ShellCommand commands[] = {
+  {"shutdown", shellRequestShutdown},
+  {"check", shellRequestCheck},
+  {"reset_memory", shellRequestResetMemory},
+  {"get_board_id", shellRequestGetBoardId},
+  {"set_board_id", shellRequestSetBoardId},
+  {"get_memory_data", shellRequestGetMemoryData},
+  {"get_vcnl", shellRequestGetVcnl},
+  {"calib_vcnl_offset", shellRequestCalib},
+  {"set_vcnl_offset", shellRequestSetVcnlOffset},
+  {"reset_vcnl_offset", shellRequestResetVcnlOffset},
+  {"get_vcnl_offset", shellRequestGetVcnlOffset},
+  {"get_robot_id", shellRequestGetRobotId},
+  {"get_system_load", shellRequestGetSystemLoad},
+  {"set_charging", shellRequestSetCharging},
+  {"print_vcnl", shellRequestPrintVCNL},
+  {"shell_board", shellSwitchBoardCmd},
+  {"get_bootloader_info", shellRequestGetBootloaderInfo},
+  {NULL, NULL}
+};
+
+static const ShellConfig shell_cfg1 = {
+  (BaseSequentialStream *) &global.sercanmux1,
+  commands
+};
+
+void charger_logic() {
+  /*
+   * if supply connected, activate charger, else
+   * deactivate charger
+   */
+  if (global.ltc4412.isPluggedIn()) {
+    boardChargerSetState(0x03u, 1), chprintf((BaseSequentialStream*) &SD1, "Charging.\n");
+  } else {
+    boardChargerSetState(0x03u, 0), chprintf((BaseSequentialStream*) &SD1, "Not charging.\n");
+  }
+}
+
+void init_powermonitor(INA219::Driver &ina219, const float shunt_resistance_O, const float max_expected_current_A, const uint16_t current_lsb_uA)
+{
+  INA219::CalibData calib_data;
+  INA219::InitData init_data;
+
+  calib_data.input.configuration.content.brng = INA219::Configuration::BRNG_16V;
+  calib_data.input.configuration.content.pg = INA219::Configuration::PGA_40mV;
+  calib_data.input.configuration.content.badc = INA219::Configuration::ADC_68100us;
+  calib_data.input.configuration.content.sadc = INA219::Configuration::ADC_68100us;
+  calib_data.input.configuration.content.mode = INA219::Configuration::MODE_ShuntBus_Continuous;
+  calib_data.input.shunt_resistance_O = shunt_resistance_O;
+  calib_data.input.max_expected_current_A = max_expected_current_A;
+  calib_data.input.current_lsb_uA = current_lsb_uA;
+  if (ina219.calibration(&calib_data) != BaseSensor<>::SUCCESS)
+    chprintf((BaseSequentialStream*)&SD1, "WARNING: calibration of INA219 failed.\n");
+
+  init_data.configuration.value = calib_data.input.configuration.value;
+  init_data.calibration = calib_data.output.calibration_value;
+  init_data.current_lsb_uA = calib_data.output.current_lsb_uA;
+  if (ina219.init(&init_data) != BaseSensor<>::SUCCESS)
+    chprintf((BaseSequentialStream*)&SD1, "WARNING: initialization of INA219 failed.\n");
+
+  if (calib_data.input.current_lsb_uA != init_data.current_lsb_uA)
+  {
+    chprintf((BaseSequentialStream*)&SD1, "NOTE: LSB for current measurement was limited when initializing INA219 (%u -> %u)", calib_data.input.current_lsb_uA, init_data.current_lsb_uA);
+  }
+
+  return;
+}
+
+/*
+ * Application entry point.
+ */
+int main(void) {
+
+  Thread *shelltp = NULL;
+
+  /*
+   * System initializations.
+   * - HAL initialization, this also initializes the configured device drivers
+   *   and performs the board-specific initializations.
+   * - Kernel initialization, the main() function becomes a thread and the
+   *   RTOS is active.
+   */
+  halInit();
+  System::init();
+
+  /*
+   * TODO: detect the reason why the system was started by reading the 1st backup register of the RTC.
+   *       To do that, it is probably required to extend the RTC LLD by the required methods.
+   *       For further details, how to interprete the data in the backu register, please have a look at the bootloader project.
+   */
+
+  /*
+   * Activates the serial driver 1 using the driver default configuration.
+   */
+  sdStart(&SD1, &global.sd1_config);
+
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+  chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n");
+  if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))),
+             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (3*4))));
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Bootloader deprecated\n");
+  }
+  chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n");
+  // make sure that the info text is completetly printed
+  BaseThread::sleep(10);
+
+//  boardWriteSystemPower(1);
+//  boardWriteIoPower(1);
+//  boardWriteWarmRestart(0);
+
+  global.robot.start(HIGHPRIO - 1);
+
+  pwmStart(&PWMD3, &global.pwm3_config);
+  pwmDisableChannel(&PWMD3, 1);
+
+  adcStart(&ADCD1, NULL);
+
+  extStart(&EXTD1, &extcfg);
+
+  boardClearI2CBus(GPIOB_GAUGE_SCL1);
+  boardClearI2CBus(GPIOB_GAUGE_SCL2);
+
+  global.HW_I2C1.start(&global.i2c1_config);
+  global.HW_I2C2.start(&global.i2c2_config);
+
+  global.memory.init();
+  uint8_t i = 0;
+  if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
+    chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
+  } else {
+    chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
+  }
+  chprintf((BaseSequentialStream*) &SD1, "\n");
+
+  shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
+
+  // initialize the power monitors
+  init_powermonitor(global.ina219[INA_VDD], 0.1f, 0.075f, 10);
+  init_powermonitor(global.ina219[INA_VIO18], 0.01f, 1.5f, 100);
+  init_powermonitor(global.ina219[INA_VIO33], 0.01f, 1.5f, 100);
+  init_powermonitor(global.ina219[INA_VIO42], 0.01f, 1.5f, 100);
+  init_powermonitor(global.ina219[INA_VIO50], 0.01f, 1.5f, 100);
+
+  // start the ADC watchdog
+  global.adc1_vsys.start(NORMALPRIO);
+
+  // start the ina threads
+  for (i = 0; i < global.ina219.size(); ++i)
+    global.ina219[i].start(NORMALPRIO);
+
+//  // calibrate the fuel gauges (TODO)
+//  BQ27500::CalibData bq27500_calib_data;
+//  bq27500_p7.calibration(&bq27500_calib_data);
+
+  // start the fuel gauge threads
+  for (i = 0; i < global.bq27500.size(); ++i)
+    global.bq27500[i].start(NORMALPRIO);
+
+  // start the proximity sensor threads
+  for (i = 0; i < global.vcnl4020.size(); ++i) {
+    uint16_t buffer;
+    global.memory.getVcnl4020Offset(&buffer,i);
+    global.vcnl4020[i].setProximityOffset(buffer);
+    global.vcnl4020[i].start(NORMALPRIO);
+  }
+
+  /* Start uart port connecting bluetooth chip */
+  global.wt12.bluetoothStart();
+
+  global.mpr121.configure(&global.mpr121_run_config);
+  global.mpr121.start(NORMALPRIO);
+
+  global.userThread.start(NORMALPRIO);
+
+  /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
+  palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_HIGH);
+
+  /* wait until all modules are done */
+  while (palReadPad(GPIOC, GPIOC_SYS_INT_N) == PAL_LOW) {
+    continue;
+  }
+
+  while (true) {
+
+    if (!shelltp)
+      shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
+    else if (chThdTerminated(shelltp)) {
+      chThdRelease(shelltp); /* Recovers memory of the previous shell. */
+      shelltp = NULL; /* Triggers spawning of a new shell.      */
+    }
+
+    // Let the LED just blink as an alive signal
+    boardWriteLed(1);
+    BaseThread::sleep(MS2ST(250));
+    boardWriteLed(0);
+    BaseThread::sleep(MS2ST(250));
+
+    /*
+     * Charger logic before shutdown logic,
+     * so we don't need to call it twice.
+     */
+    if (pathdc_change) {
+      pathdc_change = 0x00u;
+//      charger_logic();
+    }
+
+    /*
+     * Shutdown logic before user logic,
+     * so user does not get hopes up and
+     * prepares wheels to drive into the
+     * next wall or something like that.
+     */
+    if (shutdown_now != SHUTDOWN_NONE) {
+      if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER) {
+        chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
+        shutdown_now = SHUTDOWN_NONE;
+      } else {
+        uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR;
+        if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2) {
+          switch (shutdown_now) {
+            case SHUTDOWN_TRANSPORTATION:
+              blCallbackPtrAddr += 6 * 4;
+              break;
+            case SHUTDOWN_DEEPSLEEP:
+              blCallbackPtrAddr += 5 * 4;
+              break;
+            case SHUTDOWN_HIBERNATE:
+              blCallbackPtrAddr += 4 * 4;
+              break;
+            case SHUTDOWN_HANDLE_REQUEST:
+            case SHUTDOWN_RESTART:
+              blCallbackPtrAddr += 10 * 4;
+              break;
+            default:
+              blCallbackPtrAddr = 0;
+              break;
+          }
+        } else if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3) {
+          switch (shutdown_now) {
+            case SHUTDOWN_TRANSPORTATION:
+              blCallbackPtrAddr += 6 * 4;
+              break;
+            case SHUTDOWN_DEEPSLEEP:
+              blCallbackPtrAddr += 5 * 4;
+              break;
+            case SHUTDOWN_HIBERNATE:
+              blCallbackPtrAddr += 4 * 4;
+              break;
+            case SHUTDOWN_RESTART:
+              blCallbackPtrAddr += 7 * 4;
+              break;
+            case SHUTDOWN_HANDLE_REQUEST:
+              blCallbackPtrAddr += 8 * 4;
+              break;
+            default:
+              blCallbackPtrAddr = 0;
+              break;
+          }
+        }
+
+        void (*blCallback)(void) = NULL;
+        if (blCallbackPtrAddr) {
+          blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr));
+
+          if (!blCallback) {
+            chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
+            shutdown_now = SHUTDOWN_NONE;
+          } else {
+            chprintf((BaseSequentialStream*) &SD1, "initiating shutdown sequence...\n");
+            palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW);
+            palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW);
+
+            chprintf((BaseSequentialStream*) &SD1, "stopping all threads and periphery...");
+            systemStop();
+            chprintf((BaseSequentialStream*) &SD1, "\tdone\n");
+            BaseThread::sleep(MS2ST(10)); // sleep to print everything
+
+            blCallback();
+          }
+
+        } else {
+          chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
+          shutdown_now = SHUTDOWN_NONE;
+        }
+      }
+
+//      shutdown_now = 0x00u;
+//      if (palReadPad(GPIOC, GPIOC_PATH_DC))
+//        systemStop();
+//      else
+//        systemShutdown();
+//      //boardStop(0x00u, 0x00u);
+//      //chprintf((BaseSequentialStream*) &SD1, "Stop exit\n");
+    }
+
+  }
+
+  global.HW_I2C2.stop();
+  global.HW_I2C1.stop();
+
+  return 0;
+}
diff --git a/devices/PowerManagement/mcuconf.h b/devices/PowerManagement/mcuconf.h
new file mode 100644
index 0000000..e1dc7a8
--- /dev/null
+++ b/devices/PowerManagement/mcuconf.h
@@ -0,0 +1,273 @@
+/*
+ * STM32F4xx drivers configuration.
+ * The following settings override the default settings present in
+ * the various device driver implementation headers.
+ * Note that the settings for each driver only have effect if the whole
+ * driver is enabled in halconf.h.
+ *
+ * IRQ priorities:
+ * 15...0       Lowest...Highest.
+ *
+ * DMA priorities:
+ * 0...3        Lowest...Highest.
+ */
+
+#define STM32F4xx_MCUCONF
+
+/*
+ * HAL driver system settings.
+ */
+#define STM32_NO_INIT                       FALSE
+#define STM32_HSI_ENABLED                   TRUE
+#define STM32_LSI_ENABLED                   TRUE
+#define STM32_HSE_ENABLED                   TRUE
+#define STM32_LSE_ENABLED                   FALSE
+#define STM32_CLOCK48_REQUIRED              TRUE
+#define STM32_SW                            STM32_SW_PLL
+#define STM32_PLLSRC                        STM32_PLLSRC_HSE
+#define STM32_PLLM_VALUE                    8
+#define STM32_PLLN_VALUE                    336
+#define STM32_PLLP_VALUE                    2
+#define STM32_PLLQ_VALUE                    7
+#define STM32_HPRE                          STM32_HPRE_DIV1
+#define STM32_PPRE1                         STM32_PPRE1_DIV4
+#define STM32_PPRE2                         STM32_PPRE2_DIV2
+#define STM32_RTCSEL                        STM32_RTCSEL_LSI
+#define STM32_RTCPRE_VALUE                  8
+#define STM32_MCO1SEL                       STM32_MCO1SEL_HSI
+#define STM32_MCO1PRE                       STM32_MCO1PRE_DIV1
+#define STM32_MCO2SEL                       STM32_MCO2SEL_SYSCLK
+#define STM32_MCO2PRE                       STM32_MCO2PRE_DIV5
+#define STM32_I2SSRC                        STM32_I2SSRC_CKIN
+#define STM32_PLLI2SN_VALUE                 192
+#define STM32_PLLI2SR_VALUE                 5
+#define STM32_PVD_ENABLE                    FALSE
+#define STM32_PLS                           STM32_PLS_LEV0
+#define STM32_BKPRAM_ENABLE                 FALSE
+
+/*
+ * ADC driver system settings.
+ */
+#define STM32_ADC_ADCPRE                    ADC_CCR_ADCPRE_DIV4
+#define STM32_ADC_USE_ADC1                  TRUE
+#define STM32_ADC_USE_ADC2                  FALSE
+#define STM32_ADC_USE_ADC3                  FALSE
+#define STM32_ADC_ADC1_DMA_STREAM           STM32_DMA_STREAM_ID(2, 4)
+#define STM32_ADC_ADC2_DMA_STREAM           STM32_DMA_STREAM_ID(2, 2)
+#define STM32_ADC_ADC3_DMA_STREAM           STM32_DMA_STREAM_ID(2, 1)
+#define STM32_ADC_ADC1_DMA_PRIORITY         2
+#define STM32_ADC_ADC2_DMA_PRIORITY         2
+#define STM32_ADC_ADC3_DMA_PRIORITY         2
+#define STM32_ADC_IRQ_PRIORITY              6
+#define STM32_ADC_ADC1_DMA_IRQ_PRIORITY     6
+#define STM32_ADC_ADC2_DMA_IRQ_PRIORITY     6
+#define STM32_ADC_ADC3_DMA_IRQ_PRIORITY     6
+
+/*
+ * CAN driver system settings.
+ */
+#define STM32_CAN_USE_CAN1                  TRUE
+#define STM32_CAN_USE_CAN2                  FALSE
+#define STM32_CAN_CAN1_IRQ_PRIORITY         11
+#define STM32_CAN_CAN2_IRQ_PRIORITY         11
+
+/*
+ * EXT driver system settings.
+ */
+#define STM32_EXT_EXTI0_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI1_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI2_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI3_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI4_IRQ_PRIORITY        6
+#define STM32_EXT_EXTI5_9_IRQ_PRIORITY      6
+#define STM32_EXT_EXTI10_15_IRQ_PRIORITY    6
+#define STM32_EXT_EXTI16_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI17_IRQ_PRIORITY       15
+#define STM32_EXT_EXTI18_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI19_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI20_IRQ_PRIORITY       6
+#define STM32_EXT_EXTI21_IRQ_PRIORITY       15
+#define STM32_EXT_EXTI22_IRQ_PRIORITY       15
+
+/*
+ * GPT driver system settings.
+ */
+#define STM32_GPT_USE_TIM1                  FALSE
+#define STM32_GPT_USE_TIM2                  FALSE
+#define STM32_GPT_USE_TIM3                  FALSE
+#define STM32_GPT_USE_TIM4                  FALSE
+#define STM32_GPT_USE_TIM5                  FALSE
+#define STM32_GPT_USE_TIM6                  FALSE
+#define STM32_GPT_USE_TIM7                  FALSE
+#define STM32_GPT_USE_TIM8                  FALSE
+#define STM32_GPT_USE_TIM9                  FALSE
+#define STM32_GPT_USE_TIM11                 FALSE
+#define STM32_GPT_USE_TIM12                 FALSE
+#define STM32_GPT_USE_TIM14                 FALSE
+#define STM32_GPT_TIM1_IRQ_PRIORITY         7
+#define STM32_GPT_TIM2_IRQ_PRIORITY         7
+#define STM32_GPT_TIM3_IRQ_PRIORITY         7
+#define STM32_GPT_TIM4_IRQ_PRIORITY         7
+#define STM32_GPT_TIM5_IRQ_PRIORITY         7
+#define STM32_GPT_TIM6_IRQ_PRIORITY         7
+#define STM32_GPT_TIM7_IRQ_PRIORITY         7
+#define STM32_GPT_TIM8_IRQ_PRIORITY         7
+#define STM32_GPT_TIM9_IRQ_PRIORITY         7
+#define STM32_GPT_TIM11_IRQ_PRIORITY        7
+#define STM32_GPT_TIM12_IRQ_PRIORITY        7
+#define STM32_GPT_TIM14_IRQ_PRIORITY        7
+
+/*
+ * I2C driver system settings.
+ */
+#define STM32_I2C_USE_I2C1                  TRUE
+#define STM32_I2C_USE_I2C2                  TRUE
+#define STM32_I2C_USE_I2C3                  FALSE
+#define STM32_I2C_I2C1_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 0)
+#define STM32_I2C_I2C1_TX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 6)
+#define STM32_I2C_I2C2_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 2)
+#define STM32_I2C_I2C2_TX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 7)
+#define STM32_I2C_I2C3_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 2)
+#define STM32_I2C_I2C3_TX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 4)
+#define STM32_I2C_I2C1_IRQ_PRIORITY         5
+#define STM32_I2C_I2C2_IRQ_PRIORITY         5
+#define STM32_I2C_I2C3_IRQ_PRIORITY         5
+#define STM32_I2C_I2C1_DMA_PRIORITY         3
+#define STM32_I2C_I2C2_DMA_PRIORITY         3
+#define STM32_I2C_I2C3_DMA_PRIORITY         3
+#define STM32_I2C_I2C1_DMA_ERROR_HOOK()     chSysHalt()
+#define STM32_I2C_I2C2_DMA_ERROR_HOOK()     chSysHalt()
+#define STM32_I2C_I2C3_DMA_ERROR_HOOK()     chSysHalt()
+#define STM32_I2C_USE_DMA                   TRUE
+
+/*
+ * ICU driver system settings.
+ */
+#define STM32_ICU_USE_TIM1                  FALSE
+#define STM32_ICU_USE_TIM2                  FALSE
+#define STM32_ICU_USE_TIM3                  FALSE
+#define STM32_ICU_USE_TIM4                  FALSE
+#define STM32_ICU_USE_TIM5                  FALSE
+#define STM32_ICU_USE_TIM8                  FALSE
+#define STM32_ICU_USE_TIM9                  FALSE
+#define STM32_ICU_TIM1_IRQ_PRIORITY         7
+#define STM32_ICU_TIM2_IRQ_PRIORITY         7
+#define STM32_ICU_TIM3_IRQ_PRIORITY         7
+#define STM32_ICU_TIM4_IRQ_PRIORITY         7
+#define STM32_ICU_TIM5_IRQ_PRIORITY         7
+#define STM32_ICU_TIM8_IRQ_PRIORITY         7
+#define STM32_ICU_TIM9_IRQ_PRIORITY         7
+
+/*
+ * MAC driver system settings.
+ */
+#define STM32_MAC_TRANSMIT_BUFFERS          2
+#define STM32_MAC_RECEIVE_BUFFERS           4
+#define STM32_MAC_BUFFERS_SIZE              1522
+#define STM32_MAC_PHY_TIMEOUT               100
+#define STM32_MAC_ETH1_CHANGE_PHY_STATE     TRUE
+#define STM32_MAC_ETH1_IRQ_PRIORITY         13
+#define STM32_MAC_IP_CHECKSUM_OFFLOAD       0
+
+/*
+ * PWM driver system settings.
+ */
+#define STM32_PWM_USE_ADVANCED              FALSE
+#define STM32_PWM_USE_TIM1                  FALSE
+#define STM32_PWM_USE_TIM2                  FALSE
+#define STM32_PWM_USE_TIM3                  TRUE
+#define STM32_PWM_USE_TIM4                  FALSE
+#define STM32_PWM_USE_TIM5                  FALSE
+#define STM32_PWM_USE_TIM8                  FALSE
+#define STM32_PWM_USE_TIM9                  FALSE
+#define STM32_PWM_TIM1_IRQ_PRIORITY         7
+#define STM32_PWM_TIM2_IRQ_PRIORITY         7
+#define STM32_PWM_TIM3_IRQ_PRIORITY         7
+#define STM32_PWM_TIM4_IRQ_PRIORITY         7
+#define STM32_PWM_TIM5_IRQ_PRIORITY         7
+#define STM32_PWM_TIM8_IRQ_PRIORITY         7
+#define STM32_PWM_TIM9_IRQ_PRIORITY         7
+
+/*
+ * SERIAL driver system settings.
+ */
+#define STM32_SERIAL_USE_USART1             TRUE
+#define STM32_SERIAL_USE_USART2             FALSE
+#define STM32_SERIAL_USE_USART3             FALSE
+#define STM32_SERIAL_USE_UART4              FALSE
+#define STM32_SERIAL_USE_UART5              FALSE
+#define STM32_SERIAL_USE_USART6             FALSE
+#define STM32_SERIAL_USART1_PRIORITY        12
+#define STM32_SERIAL_USART2_PRIORITY        12
+#define STM32_SERIAL_USART3_PRIORITY        12
+#define STM32_SERIAL_UART4_PRIORITY         12
+#define STM32_SERIAL_UART5_PRIORITY         12
+#define STM32_SERIAL_USART6_PRIORITY        12
+
+/*
+ * SPI driver system settings.
+ */
+#define STM32_SPI_USE_SPI1                  TRUE
+#define STM32_SPI_USE_SPI2                  FALSE
+#define STM32_SPI_USE_SPI3                  FALSE
+#define STM32_SPI_SPI1_RX_DMA_STREAM        STM32_DMA_STREAM_ID(2, 0)
+#define STM32_SPI_SPI1_TX_DMA_STREAM        STM32_DMA_STREAM_ID(2, 3)
+#define STM32_SPI_SPI2_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 3)
+#define STM32_SPI_SPI2_TX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 4)
+#define STM32_SPI_SPI3_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 0)
+#define STM32_SPI_SPI3_TX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 7)
+#define STM32_SPI_SPI1_DMA_PRIORITY         1
+#define STM32_SPI_SPI2_DMA_PRIORITY         1
+#define STM32_SPI_SPI3_DMA_PRIORITY         1
+#define STM32_SPI_SPI1_IRQ_PRIORITY         10
+#define STM32_SPI_SPI2_IRQ_PRIORITY         10
+#define STM32_SPI_SPI3_IRQ_PRIORITY         10
+#define STM32_SPI_DMA_ERROR_HOOK(spip)      chSysHalt()
+
+/*
+ * UART driver system settings.
+ */
+#define STM32_UART_USE_USART1               FALSE
+#define STM32_UART_USE_USART2               TRUE
+#define STM32_UART_USE_USART3               TRUE
+#define STM32_UART_USE_UART4                FALSE
+#define STM32_UART_USE_UART5                FALSE
+#define STM32_UART_USE_USART6               FALSE
+#define STM32_UART_USART1_RX_DMA_STREAM     STM32_DMA_STREAM_ID(2, 5)
+#define STM32_UART_USART1_TX_DMA_STREAM     STM32_DMA_STREAM_ID(2, 7)
+#define STM32_UART_USART2_RX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 5)
+#define STM32_UART_USART2_TX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 6)
+#define STM32_UART_USART3_RX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 1)
+#define STM32_UART_USART3_TX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 3)
+#define STM32_UART_UART4_RX_DMA_STREAM      STM32_DMA_STREAM_ID(1, 2)
+#define STM32_UART_UART4_TX_DMA_STREAM      STM32_DMA_STREAM_ID(1, 4)
+#define STM32_UART_UART5_RX_DMA_STREAM      STM32_DMA_STREAM_ID(1, 0)
+#define STM32_UART_UART5_TX_DMA_STREAM      STM32_DMA_STREAM_ID(1, 7)
+#define STM32_UART_USART6_RX_DMA_STREAM     STM32_DMA_STREAM_ID(2, 2)
+#define STM32_UART_USART6_TX_DMA_STREAM     STM32_DMA_STREAM_ID(2, 7)
+#define STM32_UART_USART1_IRQ_PRIORITY      12
+#define STM32_UART_USART2_IRQ_PRIORITY      12
+#define STM32_UART_USART3_IRQ_PRIORITY      12
+#define STM32_UART_UART4_IRQ_PRIORITY       12
+#define STM32_UART_UART5_IRQ_PRIORITY       12
+#define STM32_UART_USART6_IRQ_PRIORITY      12
+#define STM32_UART_USART1_DMA_PRIORITY      0
+#define STM32_UART_USART2_DMA_PRIORITY      0
+#define STM32_UART_USART3_DMA_PRIORITY      0
+#define STM32_UART_UART4_DMA_PRIORITY       0
+#define STM32_UART_UART5_DMA_PRIORITY       0
+#define STM32_UART_USART6_DMA_PRIORITY      0
+#define STM32_UART_DMA_ERROR_HOOK(uartp)    chSysHalt()
+
+/*
+ * USB driver system settings.
+ */
+#define STM32_USB_USE_OTG1                  FALSE
+#define STM32_USB_USE_OTG2                  FALSE
+#define STM32_USB_OTG1_IRQ_PRIORITY         14
+#define STM32_USB_OTG2_IRQ_PRIORITY         14
+#define STM32_USB_OTG1_RX_FIFO_SIZE         512
+#define STM32_USB_OTG2_RX_FIFO_SIZE         1024
+#define STM32_USB_OTG_THREAD_PRIO           LOWPRIO
+#define STM32_USB_OTG_THREAD_STACK_SIZE     128
+#define STM32_USB_OTGFIFO_FILL_BASEPRI      0
diff --git a/devices/PowerManagement/userthread.cpp b/devices/PowerManagement/userthread.cpp
new file mode 100644
index 0000000..b7f3eca
--- /dev/null
+++ b/devices/PowerManagement/userthread.cpp
@@ -0,0 +1,169 @@
+#include "userthread.h"
+
+#include "global.hpp"
+#include <array>
+
+using namespace amiro;
+
+extern Global global;
+
+uint16_t touch;
+std::array<uint16_t, 8> proximity;
+std::array<float, 8> proxNormalized;
+
+bool running;
+
+uint16_t constexpr proxThresholdLow = 0x0000;
+uint16_t constexpr proxThresholdHigh = 0x2000;
+uint16_t constexpr proxRange = proxThresholdHigh - proxThresholdLow;
+
+std::array< std::array<float, 2>, 8> constexpr namMatrix = {
+    /*                              x     w_z */
+    std::array<float, 2>/* SSW */{ 0.00f,  0.00f},
+    std::array<float, 2>/* WSW */{ 0.25f, -0.25f},
+    std::array<float, 2>/* WNW */{-0.75f, -0.50f},
+    std::array<float, 2>/* NNW */{-0.75f, -1.00f},
+    std::array<float, 2>/* NNE */{-0.75f,  1.00f},
+    std::array<float, 2>/* ENE */{-0.75f,  0.50f},
+    std::array<float, 2>/* ESE */{ 0.25f,  0.25f},
+    std::array<float, 2>/* SSE */{ 0.00f,  0.00f}
+};
+uint32_t constexpr baseTranslation = 50e3; // 2cm/s
+uint32_t constexpr baseRotation = 1e6; // 1rad/s
+types::kinematic constexpr defaultKinematic = {
+    /*  x  [µm/s]   */ baseTranslation,
+    /*  y  [µm/s]   */ 0,
+    /*  z  [µm/s]   */ 0,
+    /* w_x [µrad/s] */ 0,
+    /* w_y [µrad/s] */ 0,
+    /* w_z [µrad/s] */ 0
+};
+
+
+
+inline uint8_t ProxId2LedId(const uint8_t proxId) {
+    return (proxId < 4) ? proxId+4 : proxId-4;
+}
+
+Color Prox2Color(const float prox) {
+  float p = 0.0f;
+  if (prox < 0.5f) {
+    p = 2.0f * prox;
+    return Color(0x00, p*0xFF, (1.0f-p)*0xFF);
+  } else {
+    p = 2.0f * (prox - 0.5f);
+    return Color(p*0xFF, (1.0f-p)*0xFF, 0x00);
+  }
+}
+
+UserThread::UserThread() :
+  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
+{
+}
+
+UserThread::~UserThread()
+{
+}
+
+msg_t
+UserThread::main()
+{
+    uint8_t sensor = 0;
+    float factor_x = 0.0f;
+    float factor_wz = 0.0f;
+    types::kinematic kinematic = defaultKinematic;
+
+    for (uint8_t led = 0; led < 8; ++led) {
+      global.robot.setLightColor(led, Color(Color::BLACK));
+    }
+    running = false;
+
+    while (!this->shouldTerminate())
+    {
+        /*
+         * read touch sensor values
+         */
+        touch = global.mpr121.getButtonStatus();
+
+        /*
+         * evaluate touch input
+         */
+        if (touch == 0x0F) {
+            if (running) {
+                // stop the robot
+                running = false;
+                kinematic = {0, 0, 0, 0, 0, 0};
+                global.robot.setTargetSpeed(kinematic);
+            } else {
+                // start the robot
+                running = true;
+            }
+
+            // set all LEDs to white for one second
+            for (uint8_t led = 0; led < 8; ++led) {
+                global.robot.setLightColor(led, Color(Color::WHITE));
+            }
+            this->sleep(MS2ST(1000));
+            for (uint8_t led = 0; led < 8; ++led) {
+                global.robot.setLightColor(led, Color(Color::BLACK));
+            }
+        }
+
+        if (running) {
+            /*
+             * read proximity values
+             */
+            for (sensor = 0; sensor < 8; ++sensor) {
+                proximity[sensor] = global.vcnl4020[sensor].getProximityScaledWoOffset();
+                //proxNormalized[sensor] += 2.0f * (proxNormalized[sensor] * (1.0f - proxNormalized[sensor])); // non linearity
+            }
+
+            /*
+             * normalize proximity values
+             */
+            for (sensor = 0; sensor < 8; ++sensor) {
+                register uint16_t prox = proximity[sensor];
+                // limit to high treshold
+                if (prox > proxThresholdHigh)
+                    prox = proxThresholdHigh;
+                // limit to low threshold
+                else if (prox < proxThresholdLow)
+                    prox = proxThresholdLow;
+                // apply low threshold
+                prox -= proxThresholdLow;
+                // normalize to [0, 1]
+                proxNormalized[sensor] = float(prox) / float(proxRange);
+            }
+
+            /*
+             * map the sensor values to the top LEDs
+             */
+            for (sensor = 0; sensor < 8; ++sensor) {
+                global.robot.setLightColor(ProxId2LedId(sensor), Prox2Color(proxNormalized[sensor]));
+            }
+
+            /*
+             * evaluate NAM
+             */
+            factor_x = 0.0f;
+            factor_wz = 0.0f;
+            for (sensor = 0; sensor < 8; ++sensor) {
+                factor_x += proxNormalized[sensor] * namMatrix[sensor][0];
+                factor_wz += proxNormalized[sensor] * namMatrix[sensor][1];
+            }
+
+            /*
+             * set motor commands
+             */
+            kinematic = defaultKinematic;
+            kinematic.x += (factor_x * baseTranslation) + 0.5f;
+            kinematic.w_z += (factor_wz * baseRotation) + 0.5f;
+            global.robot.setTargetSpeed(kinematic);
+        }
+
+        this->sleep(MS2ST(100));
+    }
+
+  return RDY_OK;
+}
+
diff --git a/devices/PowerManagement/userthread.h b/devices/PowerManagement/userthread.h
new file mode 100644
index 0000000..ac056b1
--- /dev/null
+++ b/devices/PowerManagement/userthread.h
@@ -0,0 +1,21 @@
+#ifndef AMIRO_USERTHREAD_H_
+#define AMIRO_USERTHREAD_H_
+
+#include <ch.hpp>
+#include <amiroosconf.h>
+
+namespace amiro {
+
+class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
+{
+public:
+  explicit UserThread();
+
+  virtual ~UserThread();
+
+  virtual msg_t main();
+};
+
+} // end of namespace amiro
+
+#endif // AMIRO_USERTHREAD_H_
diff --git a/devices/flash.mk b/devices/flash.mk
new file mode 100644
index 0000000..728523f
--- /dev/null
+++ b/devices/flash.mk
@@ -0,0 +1,44 @@
+DEVICES := DiWheelDrive LightRing PowerManagement
+
+DiWheelDrive := 1
+PowerManagement := 2
+LightRing := 3
+
+SERIALBOOT ?= SerialBoot
+ifeq ($(OS),Windows_NT)
+	SERIALBOOT_PORT ?= COM4
+else
+	SERIALBOOT_PORT ?= /dev/ttyUSB0
+#	SERIALBOOT_PORT ?= /dev/rfcomm0
+endif
+SERIALBOOT_BAUDRATE ?= 115200
+#SERIALBOOT_BAUDRATE ?= 921600
+
+#SERIALBOOT_BT_ADDR ?= 00:00:00:00:00:00
+SERIALBOOT_BT_ADDR ?= 00:07:80:44:23:F9
+
+
+ifdef PROJECT
+	DEVICES :=
+	FLASHFILE = build/$(PROJECT).srec
+	FLASHTRGS = $(FLASHFILE)
+	FLASHARGS =  -T$($(PROJECT)) $(FLASHFILE)
+	FLASHARGSDIRECT = $(FLASHFILE)
+else
+	FLASHTRGS = $(foreach dev,$(DEVICES),$(dev)/build/$(dev).srec)
+	FLASHARGS = $(foreach dev,$(DEVICES),-T$($(dev)) $(dev)/build/$(dev).srec)
+	FLASHARGSDIRECT = $(foreach dev,$(DEVICES),$(dev)/build/$(dev).srec)
+endif
+
+flash: $(DEVICES) $(FLASHTRGS)
+	@echo Flashing
+	$(SERIALBOOT) -d$(SERIALBOOT_PORT) -b$(SERIALBOOT_BAUDRATE) $(FLASHARGS)
+
+flashdirect: $(DEVICES) $(FLASHTRGS)
+	@echo Flashing
+	$(SERIALBOOT) -d$(SERIALBOOT_PORT) -b$(SERIALBOOT_BAUDRATE) $(FLASHARGSDIRECT)
+
+flashbt: $(DEVICES) $(FLASHTRGS)
+	@echo Flashing over Bluetooth to $(SERIALBOOT_BT_ADDR)
+	$(SERIALBOOT) -a$(SERIALBOOT_BT_ADDR) $(FLASHARGS)
+
diff --git a/hal/hal.mk b/hal/hal.mk
new file mode 100644
index 0000000..266c9c0
--- /dev/null
+++ b/hal/hal.mk
@@ -0,0 +1,6 @@
+# List of all the ChibiOS/RT HAL files, there is no need to remove the files
+# from this list, you can disable parts of the kernel by editing halconf.h.
+HALSRC += ${AMIRO}/hal/src/qei.c 
+
+# Required include directories
+HALINC += ${AMIRO}/hal/include
diff --git a/hal/include/qei.h b/hal/include/qei.h
new file mode 100644
index 0000000..8f7dc44
--- /dev/null
+++ b/hal/include/qei.h
@@ -0,0 +1,120 @@
+#ifndef _QEI_H_
+#define _QEI_H_
+
+#if HAL_USE_QEI || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Driver state machine possible states.
+ */
+typedef enum {
+  QEI_UNINIT = 0,                   /**< Not initialized.                   */
+  QEI_STOP = 1,                     /**< Stopped.                           */
+  QEI_READY = 2,                    /**< Ready.                             */
+  QEI_ACTIVE = 4,                   /**< Active.                            */
+} qeistate_t;
+
+/**
+ * @brief   Type of a structure representing an QEI driver.
+ */
+typedef struct QEIDriver QEIDriver;
+
+#include "qei_lld.h"
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @name    Macro Functions
+ * @{
+ */
+/**
+ * @brief   Enables the quadrature encoder.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @iclass
+ */
+#define qeiEnableI(qeip) qei_lld_enable(qeip)
+
+/**
+ * @brief   Disables the quadrature encoder.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @iclass
+ */
+#define qeiDisableI(qeip) qei_lld_disable(qeip)
+
+/**
+ * @brief   Returns the direction of the last transition.
+ * @details The direction is defined as boolean and is
+ *          calculated at each transition on any input.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ * @return              The request direction.
+ * @retval FALSE        Position counted up.
+ * @retval TRUE         Position counted down.
+ * @iclass
+ */
+#define qeiGetDirectionI(qeip) qei_lld_get_direction(qeip)
+
+/**
+ * @brief   Returns the position of the encoder.
+ * @details The position is defined as number of pulses since last reset.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ * @return              The number of pulses.
+ *
+ * @iclass
+ */
+#define qeiGetPositionI(qeip) qei_lld_get_position(qeip)
+
+/**
+ * @brief   Returns the range of the encoder.
+ * @details The range is defined as number of maximum pulse count.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ * @return              The number of pulses.
+ *
+ * @iclass
+ */
+#define qeiGetRangeI(qeip) qei_lld_get_range(qeip)
+/** @} */
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void qeiInit(void);
+  void qeiObjectInit(QEIDriver *qeip);
+  void qeiStart(QEIDriver *qeip, const QEIConfig *config);
+  void qeiStop(QEIDriver *qeip);
+  void qeiEnable(QEIDriver *qeip);
+  void qeiDisable(QEIDriver *qeip);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* HAL_USE_QEI */
+
+#endif /* _QEI_H_ */
diff --git a/hal/platforms/STM32/platform.mk b/hal/platforms/STM32/platform.mk
new file mode 100644
index 0000000..ef40af2
--- /dev/null
+++ b/hal/platforms/STM32/platform.mk
@@ -0,0 +1,5 @@
+# List of all the STM32F1xx platform files.
+PLATFORMSRC += ${AMIRO}/hal/platforms/STM32/qei_lld.c
+
+# Required include directories
+PLATFORMINC += ${AMIRO}/hal/platforms/STM32
diff --git a/hal/platforms/STM32/qei_lld.c b/hal/platforms/STM32/qei_lld.c
new file mode 100644
index 0000000..b27d79e
--- /dev/null
+++ b/hal/platforms/STM32/qei_lld.c
@@ -0,0 +1,275 @@
+#include "ch.h"
+#include "hal.h"
+
+#include "qei.h"
+
+#if HAL_USE_QEI || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   QEID1 driver identifier.
+ * @note    The driver QEID1 allocates the complex timer TIM1 when enabled.
+ */
+#if STM32_QEI_USE_TIM1 || defined(__DOXYGEN__)
+QEIDriver QEID1;
+#endif
+
+/**
+ * @brief   QEID2 driver identifier.
+ * @note    The driver QEID1 allocates the timer TIM2 when enabled.
+ */
+#if STM32_QEI_USE_TIM2 || defined(__DOXYGEN__)
+QEIDriver QEID2;
+#endif
+
+/**
+ * @brief   QEID3 driver identifier.
+ * @note    The driver QEID1 allocates the timer TIM3 when enabled.
+ */
+#if STM32_QEI_USE_TIM3 || defined(__DOXYGEN__)
+QEIDriver QEID3;
+#endif
+
+/**
+ * @brief   QEID4 driver identifier.
+ * @note    The driver QEID4 allocates the timer TIM4 when enabled.
+ */
+#if STM32_QEI_USE_TIM4 || defined(__DOXYGEN__)
+QEIDriver QEID4;
+#endif
+
+/**
+ * @brief   QEID5 driver identifier.
+ * @note    The driver QEID5 allocates the timer TIM5 when enabled.
+ */
+#if STM32_QEI_USE_TIM5 || defined(__DOXYGEN__)
+QEIDriver QEID5;
+#endif
+
+/**
+ * @brief   QEID8 driver identifier.
+ * @note    The driver QEID8 allocates the timer TIM8 when enabled.
+ */
+#if STM32_QEI_USE_TIM8 || defined(__DOXYGEN__)
+QEIDriver QEID8;
+#endif
+
+/*===========================================================================*/
+/* Driver local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver interrupt handlers.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Low level QEI driver initialization.
+ *
+ * @notapi
+ */
+void qei_lld_init(void) {
+
+#if STM32_QEI_USE_TIM1
+  /* Driver initialization.*/
+  qeiObjectInit(&QEID1);
+  QEID1.tim = STM32_TIM1;
+#endif
+
+#if STM32_QEI_USE_TIM2
+  /* Driver initialization.*/
+  qeiObjectInit(&QEID2);
+  QEID2.tim = STM32_TIM2;
+#endif
+
+#if STM32_QEI_USE_TIM3
+  /* Driver initialization.*/
+  qeiObjectInit(&QEID3);
+  QEID3.tim = STM32_TIM3;
+#endif
+
+#if STM32_QEI_USE_TIM4
+  /* Driver initialization.*/
+  qeiObjectInit(&QEID4);
+  QEID4.tim = STM32_TIM4;
+#endif
+
+#if STM32_QEI_USE_TIM5
+  /* Driver initialization.*/
+  qeiObjectInit(&QEID5);
+  QEID5.tim = STM32_TIM5;
+#endif
+
+#if STM32_QEI_USE_TIM8
+  /* Driver initialization.*/
+  qeiObjectInit(&QEID8);
+  QEID8.tim = STM32_TIM8;
+#endif
+}
+
+/**
+ * @brief   Configures and activates the QEI peripheral.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @notapi
+ */
+void qei_lld_start(QEIDriver *qeip) {
+  uint32_t arr, ccer;
+
+  if (qeip->state == QEI_STOP) {
+    /* Clock activation and timer reset.*/
+#if STM32_QEI_USE_TIM1
+    if (&QEID1 == qeip) {
+      rccEnableTIM1(FALSE);
+      rccResetTIM1();
+    }
+#endif
+#if STM32_QEI_USE_TIM2
+    if (&QEID2 == qeip) {
+      rccEnableTIM2(FALSE);
+      rccResetTIM2();
+    }
+#endif
+#if STM32_QEI_USE_TIM3
+    if (&QEID3 == qeip) {
+      rccEnableTIM3(FALSE);
+      rccResetTIM3();
+    }
+#endif
+#if STM32_QEI_USE_TIM4
+    if (&QEID4 == qeip) {
+      rccEnableTIM4(FALSE);
+      rccResetTIM4();
+    }
+#endif
+
+#if STM32_QEI_USE_TIM5
+    if (&QEID5 == qeip) {
+      rccEnableTIM5(FALSE);
+      rccResetTIM5();
+    }
+#endif
+#if STM32_QEI_USE_TIM8
+    if (&QEID8 == qeip) {
+      rccEnableTIM8(FALSE);
+      rccResetTIM8();
+    }
+#endif
+  }
+  else {
+    /* Driver re-configuration scenario, it must be stopped first.*/
+    qeip->tim->CR1    = 0;                  /* Timer disabled.              */
+    qeip->tim->DIER   = 0;                  /* All IRQs disabled.           */
+    qeip->tim->SR     = 0;                  /* Clear eventual pending IRQs. */
+    qeip->tim->CCR[0] = 0;                  /* Comparator 1 disabled.       */
+    qeip->tim->CCR[1] = 0;                  /* Comparator 2 disabled.       */
+    qeip->tim->CNT    = 0;                  /* Counter reset to zero.       */
+  }
+
+  /* Timer configuration.*/
+  qeip->tim->PSC  = 0;
+  arr = qeip->config->range - 1;
+  chDbgAssert((arr <= 0xFFFF), "qei_lld_start(), #1", "invalid range");
+  qeip->tim->ARR  = arr & 0xFFFF;
+
+  /* CCMR1_CC1S = 01 - CH1 Input on TI1.
+     CCMR1_CC2S = 01 - CH2 Input on TI2.*/
+  qeip->tim->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0;
+
+  ccer = 0;
+  if (qeip->config->channels[0].mode == QEI_INPUT_INVERTED)
+    ccer |= TIM_CCER_CC1P;
+  if (qeip->config->channels[1].mode == QEI_INPUT_INVERTED)
+    ccer |= TIM_CCER_CC2P;
+  qeip->tim->CCER = ccer;
+
+  if (qeip->config->mode == QEI_COUNT_CH1)
+    qeip->tim->SMCR  = TIM_SMCR_SMS_1;
+  else if (qeip->config->mode == QEI_COUNT_CH2)
+    qeip->tim->SMCR  = TIM_SMCR_SMS_0;
+  else
+    qeip->tim->SMCR  = TIM_SMCR_SMS_0 | TIM_SMCR_SMS_1;
+}
+
+/**
+ * @brief   Deactivates the QEI peripheral.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @notapi
+ */
+void qei_lld_stop(QEIDriver *qeip) {
+
+  if (qeip->state == QEI_READY) {
+    /* Clock deactivation.*/
+    qeip->tim->CR1  = 0;                    /* Timer disabled.              */
+
+#if STM32_QEI_USE_TIM1
+    if (&QEID1 == qeip) {
+      rccDisableTIM1(FALSE);
+    }
+#endif
+#if STM32_QEI_USE_TIM2
+    if (&QEID2 == qeip) {
+      rccDisableTIM2(FALSE);
+    }
+#endif
+#if STM32_QEI_USE_TIM3
+    if (&QEID3 == qeip) {
+      rccDisableTIM3(FALSE);
+    }
+#endif
+#if STM32_QEI_USE_TIM4
+    if (&QEID4 == qeip) {
+      rccDisableTIM4(FALSE);
+    }
+#endif
+#if STM32_QEI_USE_TIM5
+    if (&QEID5 == qeip) {
+      rccDisableTIM5(FALSE);
+    }
+#endif
+  }
+#if STM32_QEI_USE_TIM8
+    if (&QEID8 == qeip) {
+      rccDisableTIM8(FALSE);
+    }
+#endif
+}
+
+/**
+ * @brief   Enables the quadrature encoder.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @notapi
+ */
+void qei_lld_enable(QEIDriver *qeip) {
+
+  qeip->tim->CR1  = TIM_CR1_CEN;
+}
+
+/**
+ * @brief   Disables the quadrature encoder.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @notapi
+ */
+void qei_lld_disable(QEIDriver *qeip) {
+
+  qeip->tim->CR1  = 0;
+}
+
+#endif /* HAL_USE_QEI */
diff --git a/hal/platforms/STM32/qei_lld.h b/hal/platforms/STM32/qei_lld.h
new file mode 100644
index 0000000..3a594d4
--- /dev/null
+++ b/hal/platforms/STM32/qei_lld.h
@@ -0,0 +1,274 @@
+#ifndef _QEI_LLD_H_
+#define _QEI_LLD_H_
+
+#include "stm32_tim.h"
+
+#if HAL_USE_QEI || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+/**
+ * @brief   Number of input channels per QEI driver.
+ */
+#define QEI_CHANNELS                            2
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Configuration options
+ * @{
+ */
+/**
+ * @brief   QEID1 driver enable switch.
+ * @details If set to @p TRUE the support for QEID1 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_QEI_USE_TIM1) || defined(__DOXYGEN__)
+#define STM32_QEI_USE_TIM1                  TRUE
+#endif
+
+/**
+ * @brief   QEID2 driver enable switch.
+ * @details If set to @p TRUE the support for QEID2 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_QEI_USE_TIM2) || defined(__DOXYGEN__)
+#define STM32_QEI_USE_TIM2                  TRUE
+#endif
+
+/**
+ * @brief   QEID3 driver enable switch.
+ * @details If set to @p TRUE the support for QEID3 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_QEI_USE_TIM3) || defined(__DOXYGEN__)
+#define STM32_QEI_USE_TIM3                  TRUE
+#endif
+
+/**
+ * @brief   QEID4 driver enable switch.
+ * @details If set to @p TRUE the support for QEID4 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_QEI_USE_TIM4) || defined(__DOXYGEN__)
+#define STM32_QEI_USE_TIM4                  TRUE
+#endif
+
+/**
+ * @brief   QEID5 driver enable switch.
+ * @details If set to @p TRUE the support for QEID5 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_QEI_USE_TIM5) || defined(__DOXYGEN__)
+#define STM32_QEI_USE_TIM5                  TRUE
+#endif
+
+/**
+ * @brief   QEID8 driver enable switch.
+ * @details If set to @p TRUE the support for QEID8 is included.
+ * @note    The default is @p TRUE.
+ */
+#if !defined(STM32_QEI_USE_TIM8) || defined(__DOXYGEN__)
+#define STM32_QEI_USE_TIM8                  TRUE
+#endif
+/** @} */
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if STM32_QEI_USE_TIM1 && !STM32_HAS_TIM1
+#error "TIM1 not present in the selected device"
+#endif
+
+#if STM32_QEI_USE_TIM2 && !STM32_HAS_TIM2
+#error "TIM2 not present in the selected device"
+#endif
+
+#if STM32_QEI_USE_TIM3 && !STM32_HAS_TIM3
+#error "TIM3 not present in the selected device"
+#endif
+
+#if STM32_QEI_USE_TIM4 && !STM32_HAS_TIM4
+#error "TIM4 not present in the selected device"
+#endif
+
+#if STM32_QEI_USE_TIM5 && !STM32_HAS_TIM5
+#error "TIM5 not present in the selected device"
+#endif
+
+#if STM32_QEI_USE_TIM8 && !STM32_HAS_TIM8
+#error "TIM8 not present in the selected device"
+#endif
+
+#if !STM32_QEI_USE_TIM1 && !STM32_QEI_USE_TIM2 &&                           \
+    !STM32_QEI_USE_TIM3 && !STM32_QEI_USE_TIM4 &&                           \
+    !STM32_QEI_USE_TIM5 && !STM32_QEI_USE_TIM8
+#error "QEI driver activated but no TIM peripheral assigned"
+#endif
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @brief QEI driver mode.
+ */
+typedef enum {
+  QEI_COUNT_BOTH = 0,
+  QEI_COUNT_CH1 = 1,
+  QEI_COUNT_CH2 = 2,
+} qeimode_t;
+
+/**
+ * @brief QEI input mode.
+ */
+typedef enum {
+  QEI_INPUT_NONINVERTED = 0, /**< Input channel noninverted.*/
+  QEI_INPUT_INVERTED = 1, /**< Input channel inverted.*/
+} qeiinputmode_t;
+
+/**
+ * @brief   QEI count type.
+ */
+typedef uint32_t qeicnt_t;
+
+/**
+ * @brief   Driver channel configuration structure.
+ */
+typedef struct {
+  /**
+   * @brief Channel input logic.
+   */
+  qeiinputmode_t                 mode;
+  /* End of the mandatory fields.*/
+} QEIChannelConfig;
+
+/**
+ * @brief   Driver configuration structure.
+ */
+typedef struct {
+  /**
+   * @brief   Driver mode.
+   */
+  qeimode_t                 mode;
+  /**
+   * @brief   Channels configurations.
+   */
+  QEIChannelConfig          channels[QEI_CHANNELS];
+  /**
+   * @brief   Range in pulses.
+   */
+  qeicnt_t                  range;
+  /* End of the mandatory fields.*/
+} QEIConfig;
+
+/**
+ * @brief   Structure representing an QEI driver.
+ */
+struct QEIDriver {
+  /**
+   * @brief Driver state.
+   */
+  qeistate_t                state;
+  /**
+   * @brief Current configuration data.
+   */
+  const QEIConfig           *config;
+#if defined(QEI_DRIVER_EXT_FIELDS)
+  QEI_DRIVER_EXT_FIELDS
+#endif
+  /* End of the mandatory fields.*/
+  /**
+   * @brief Pointer to the TIMx registers block.
+   */
+  stm32_tim_t               *tim;
+};
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/**
+ * @brief   Returns the direction of the last transition.
+ * @details The direction is defined as boolean and is
+ *          calculated at each transition on any input.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ * @return              The request direction.
+ * @retval FALSE        Position counted up.
+ * @retval TRUE         Position counted down.
+ *
+ * @iclass
+ */
+#define qei_lld_get_direction(qeip) !!((qeip)->tim->CR1 & TIM_CR1_DIR)
+
+/**
+ * @brief   Returns the position of the encoder.
+ * @details The position is defined as number of pulses since last reset.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ * @return              The number of pulses.
+ *
+ * @iclass
+ */
+#define qei_lld_get_position(qeip) ((qeip)->tim->CNT)
+
+/**
+ * @brief   Returns the range of the encoder.
+ * @details The range is defined as number of maximum pulse count.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ * @return              The number of pulses.
+ *
+ * @iclass
+ */
+#define qei_lld_get_range(qeip) ((qeip)->tim->ARR + 1)
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#if STM32_QEI_USE_TIM1 && !defined(__DOXYGEN__)
+extern QEIDriver QEID1;
+#endif
+
+#if STM32_QEI_USE_TIM2 && !defined(__DOXYGEN__)
+extern QEIDriver QEID2;
+#endif
+
+#if STM32_QEI_USE_TIM3 && !defined(__DOXYGEN__)
+extern QEIDriver QEID3;
+#endif
+
+#if STM32_QEI_USE_TIM4 && !defined(__DOXYGEN__)
+extern QEIDriver QEID4;
+#endif
+
+#if STM32_QEI_USE_TIM5 && !defined(__DOXYGEN__)
+extern QEIDriver QEID5;
+#endif
+
+#if STM32_QEI_USE_TIM8 && !defined(__DOXYGEN__)
+extern QEIDriver QEID8;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void qei_lld_init(void);
+  void qei_lld_start(QEIDriver *qeip);
+  void qei_lld_stop(QEIDriver *qeip);
+  void qei_lld_enable(QEIDriver *qeip);
+  void qei_lld_disable(QEIDriver *qeip);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* HAL_USE_QEI */
+
+#endif /* _QEI_LLD_H_ */
diff --git a/hal/src/qei.c b/hal/src/qei.c
new file mode 100644
index 0000000..9ffa934
--- /dev/null
+++ b/hal/src/qei.c
@@ -0,0 +1,130 @@
+#include "ch.h"
+#include "hal.h"
+#include "qei.h"
+
+#if HAL_USE_QEI || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   QEI Driver initialization.
+ * @note    This function is implicitly invoked by @p halInit(), there is
+ *          no need to explicitly initialize the driver.
+ *
+ * @init
+ */
+void qeiInit(void) {
+
+  qei_lld_init();
+}
+
+/**
+ * @brief   Initializes the standard part of a @p QEIDriver structure.
+ *
+ * @param[out] qeip     pointer to the @p QEIDriver object
+ *
+ * @init
+ */
+void qeiObjectInit(QEIDriver *qeip) {
+
+  qeip->state  = QEI_STOP;
+  qeip->config = NULL;
+}
+
+/**
+ * @brief   Configures and activates the QEI peripheral.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ * @param[in] config    pointer to the @p QEIConfig object
+ *
+ * @api
+ */
+void qeiStart(QEIDriver *qeip, const QEIConfig *config) {
+
+  chDbgCheck((qeip != NULL) && (config != NULL), "qeiStart");
+
+  chSysLock();
+  chDbgAssert((qeip->state == QEI_STOP) || (qeip->state == QEI_READY),
+              "qeiStart(), #1", "invalid state");
+  qeip->config = config;
+  qei_lld_start(qeip);
+  qeip->state = QEI_READY;
+  chSysUnlock();
+}
+
+/**
+ * @brief   Deactivates the QEI peripheral.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @api
+ */
+void qeiStop(QEIDriver *qeip) {
+
+  chDbgCheck(qeip != NULL, "qeiStop");
+
+  chSysLock();
+  chDbgAssert((qeip->state == QEI_STOP) || (qeip->state == QEI_READY),
+              "qeiStop(), #1", "invalid state");
+  qei_lld_stop(qeip);
+  qeip->state = QEI_STOP;
+  chSysUnlock();
+}
+
+/**
+ * @brief   Enables the quadrature encoder.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @api
+ */
+void qeiEnable(QEIDriver *qeip) {
+
+  chDbgCheck(qeip != NULL, "qeiEnable");
+
+  chSysLock();
+  chDbgAssert(qeip->state == QEI_READY, "qeiEnable(), #1", "invalid state");
+  qei_lld_enable(qeip);
+  qeip->state = QEI_ACTIVE;
+  chSysUnlock();
+}
+
+/**
+ * @brief   Disables the quadrature encoder.
+ *
+ * @param[in] qeip      pointer to the @p QEIDriver object
+ *
+ * @api
+ */
+void qeiDisable(QEIDriver *qeip) {
+
+  chDbgCheck(qeip != NULL, "qeiDisable");
+
+  chSysLock();
+  chDbgAssert((qeip->state == QEI_READY) || (qeip->state == QEI_ACTIVE),
+              "qeiDisable(), #1", "invalid state");
+  qei_lld_disable(qeip);
+  qeip->state = QEI_READY;
+  chSysUnlock();
+}
+
+#endif /* HAL_USE_QEI */
+
diff --git a/ide/qtcreator/README.txt b/ide/qtcreator/README.txt
new file mode 100644
index 0000000..3df7781
--- /dev/null
+++ b/ide/qtcreator/README.txt
@@ -0,0 +1,67 @@
+SETTING UP AN AMIRO-OS PROJECT FOR QT-CREATOR
+=============================================
+
+1 - Creating the Project Files
+------------------------------
+Run the "setup.sh" script in this directory and follow the instructions.
+  > ./setup.sh
+
+
+
+!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+! THAT'S IT for the basic setup.                                                     !
+! The next steps are only necessary, if you want to integrate 'make' into QtCreator, !
+! or if QtCreator crashes when opening the projects.                                 !
+!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+
+
+2 - Setup the AMiRo build environment for QtCreator
+---------------------------------------------------
+Launch the QtCreator application, click on
+  Tools -> Options
+and select "Build & Run".
+
+Under the tab "Compilers" add the arm-none-eabi-g++ compiler as another GCC.
+
+Under the tab "Debuggers" add the arm-none-eabi-gdb debugger.
+
+Under the tab "Kits" manually create a new kit:
+  Name: AMiRo
+  Device type: Desktop
+  Sysroot: [gcc-arm-none-eabi include path]
+  Compiler: [the compiler added above]
+  Environment: SERIALBOOT=[absolute path to the SerialBoot binary]
+  Debugger: [the debugger added above]
+  QT version: none
+With everything else empty or default.
+If there is a warning sign shown right beside the new kit, check the setup for errors.
+
+
+
+3 - Open the Project(s) with QtCreator
+--------------------------------------
+For convenience, the absolute path to your device (e.g. /home/juser/amiro-os/devices/PowerManagement)
+will be referenced in the following by "MOD_PATH".
+
+In the QtCreator application, click on
+  File -> Open File or Project
+and open the according *.creator file.
+
+Select the project in the "Projects" view and click on the "Projects" icon on the very left.
+Switch to the "Build & Run" tab.
+
+Change the kit to "AMiRo" (right above the "Build" and "Run" buttons).
+
+Add a new build configuration and call it "make".
+  Build directory: MOD_PATH
+  Build Steps: make --directory=MOD_PATH all
+  Clean Steps: make --directory=MOD_PATH clean
+Optionally you can add "-j" to the build setp in order to parallelize the compilation process.
+
+Add another new build configuration by cloning the new "make" configuration and call t "make flash".
+Replace "all" at the end of the Build Step with "flash"
+
+Remove the "Default" build configuration.
+
+Repeat this step for reach further project.
diff --git a/ide/qtcreator/setup.sh b/ide/qtcreator/setup.sh
new file mode 100755
index 0000000..cd2bac9
--- /dev/null
+++ b/ide/qtcreator/setup.sh
@@ -0,0 +1,484 @@
+#!/bin/bash
+
+############################
+###   HELPER FUNCTIONS   ###
+############################
+
+function absoluteToRelativePath {
+  # both $1 and $2 are absolute paths beginning with /
+  # stores the relative path from $1/START_PATH to $2/TARGET_PATH in $3/RESULT_PATH or echoes it
+
+  START_PATH=$1
+  TARGET_PATH=$2
+
+  # check whether one of both paths ends with / and remove it for now
+  ENDS_WITH_SLASH=false
+  while [ "${START_PATH%"/"}" != "$START_PATH" ]; do
+    START_PATH=${START_PATH::-1}
+    ENDS_WITH_SLASH=true
+  done
+  while [ "${TARGET_PATH%"/"}" != "$TARGET_PATH" ]; do
+    TARGET_PATH=${TARGET_PATH::-1}
+    ENDS_WITH_SLASH=true
+  done
+
+  # initialize the common prefix and the relative path from START_PATH to TARGET_PATH
+  COMMON_PREFIX=$START_PATH
+  RELATIVE_PATH=""
+  while [ "${TARGET_PATH#$COMMON_PREFIX}" == "$TARGET_PATH" ]; do
+    # while the COMMOM_PREFIX is no substring of TARGET_PATH, reduce it
+    COMMON_PREFIX="$(dirname $COMMON_PREFIX)"
+
+    # thereby record the relative path
+    if [ -z $RELATIVE_PATH ]; then
+      RELATIVE_PATH=".."
+    else
+      RELATIVE_PATH="../$RELATIVE_PATH"
+    fi
+  done
+
+  # if the reltiva path is empty, set it as the current directory
+  if [ -z $RELATIVE_PATH ]; then
+    RELATIVE_PATH="."
+  fi
+
+  if [ $COMMON_PREFIX == "/" ]; then
+    # special case if only root is the common path
+    RELATIVE_PATH="$RELATIVE_PATH/"
+  fi
+
+  # compute the unique part of the target path
+  UNIQUE_POSTFIX="${TARGET_PATH#$COMMON_PREFIX}"
+
+  # append the unique postfix to the relative path
+  if [ -n $RELATIVE_PATH ] && [ -n $UNIQUE_POSTFIX ]; then
+    RELATIVE_PATH="$RELATIVE_PATH$UNIQUE_POSTFIX"
+  elif [ -n $UNIQUE_POSTFIX ]; then
+    # remove the slash
+    RELATIVE_PATH="${UNIQUE_POSTFIX:1}"
+  fi
+
+  # remove a prepending slash if any
+  if [ "${RELATIVE_PATH#"/"}" != "$RELATIVE_PATH" ]; then
+    RELATIVE_PATH=${RELATIVE_PATH:1}
+  fi
+
+  # if at least one input ended with a slash, add it to the result as well
+  if [ $ENDS_WITH_SLASH == true ]; then
+    RELATIVE_PATH="${RELATIVE_PATH}/"
+  fi
+
+  # return the result
+  if [ $# -gt 2 ]; then
+    eval $3="$RELATIVE_PATH"
+  else
+    echo $RELATIVE_PATH
+  fi
+
+  # clean up
+  unset START_PATH
+  unset TARGET_PATH
+  unset ENDS_WITH_SLASH
+  unset COMMON_PREFIX
+  unset RELATIVE_PATH
+  unset UNIQUE_POSTFIX
+}
+
+
+function cleanupVariables {
+
+  unset HELP_FLAG
+  unset CLEAN_FLAG
+  unset WIPE_FLAG
+  unset CHOS
+  unset LR
+  unset PM
+  unset DWD
+  unset ARG_LIST
+
+  unset USER_DIR
+  unset SCRIPT_DIR
+
+  unset GCC_ARM_NONE_EABI_INCLUDE_PATH
+  unset PROJECT_ROOT_PATH
+  unset AMIROOS_ROOT_PATH
+  unset CHIBIOS_ROOT_PATH
+
+  unset CHIBIOS_PROJECT_PREFIX
+  unset LIGHTRING_PROJECT_PREFIX
+  unset POWERMANAGEMENT_PROJECT_PREFIX
+  unset DIWHEELDRIVE_PROJECT_PREFIX
+
+  unset PROJECT_ROOT_TO_CHIBIOS
+  unset PROJECT_ROOT_TO_AMIROOS
+}
+
+
+#################
+###   INTRO   ###
+#################
+
+echo "-------------------------------------------------------------------------------"
+echo ""
+echo "Setup to create QtCreator projects"
+echo "=================================="
+echo ""
+
+
+
+##########################
+###   INITIALIZATION   ###
+##########################
+
+# ignore case when comparing strings
+shopt -s nocasematch
+
+# detect what exactly should be done
+HELP_FLAG="HELP"
+CLEAN_FLAG="CLEAN"
+WIPE_FLAG="WIPE"
+CHIBIOS_FLAG="CHOS"
+LIGHTRING_FLAG="LR"
+POWERMANAGEMENT_FLAG="PM"
+DIWHEELDRIVE_FLAG="DWD"
+
+# start with an empty array
+ARG_LIST=()
+if [ $# == 0 ]; then
+  # if no argument was specified, 'help' is considered as default
+  ARG_LIST+=($HELP_FLAG)
+else
+  for ARG in "$@"; do
+    case ${ARG,,} in
+      "help"|"-help"|"--help")
+        ARG_LIST+=($HELP_FLAG)
+        ;;
+      "clean")
+        ARG_LIST+=($CLEAN_FLAG)
+        ;;
+      "wipe")
+        ARG_LIST+=($WIPE_FLAG)
+        ;;
+      "all")
+        ARG_LIST+=($LIGHTRING_FLAG $POWERMANAGEMENT_FLAG $DIWHEELDRIVE_FLAG)
+        ;;
+      "ChibiOS")
+        ArrayContainsElement "${ARG_LIST[@]}" "$CHIBIOS_FLAG"
+        if [ $? != 1 ]; then
+          ARG_LIST+=($CHIBIOS_FLAG)
+        fi
+        ;;
+      "LightRing")
+        ArrayContainsElement "${ARG_LIST[@]}" "$LIGHTRING_FLAG"
+        if [ $? != 1 ]; then
+          ARG_LIST+=($LIGHTRING_FLAG)
+        fi
+        ;;
+      "PowerManagement")
+        ArrayContainsElement "${ARG_LIST[@]}" "$POWERMANAGEMENT_FLAG"
+        if [ $? != 1 ]; then
+          ARG_LIST+=($POWERMANAGEMENT_FLAG)
+        fi
+        ;;
+      "DiWheelDrive")
+        ArrayContainsElement "{$ARG_LIST[@]}" "DIWHEELDRIVE_FLAG"
+        if [ $? != 1 ]; then
+          ARG_LIST+=($DIWHEELDRIVE_FLAG)
+        fi
+        ;;
+    esac
+  done
+fi
+
+
+
+######################
+###   PRINT HELP   ###
+######################
+
+if [[ "${ARG_LIST[@]}" == *"$HELP_FLAG"* ]]; then
+    echo -e "> HELP"
+    echo -e ""
+    echo -e "\tThe following commands are available:"
+    echo -e ""
+    echo -e "\t  help            - Prints this help text."
+    echo -e "\t  clean           - Deletes all files created by this script."
+    echo -e "\t  wipe            - Deletes the .user files, created by QtCreator."
+    echo -e "\t  ChibiOS         - Creates a project for ChibiOS."
+    echo -e "\t  LightRing       - Creates a project for the LightRing module."
+    echo -e "\t  PowerManagement - Creates a project for the PowerManagement module."
+    echo -e "\t  DiWheelDrive    - Creates a project for the DiWheelDrive module."
+    echo -e "\t  all             - Creates all three projects (no ChibiOS project)."
+    echo -e ""
+    echo -e "\tAny of these commands can be combined, e.g."
+    echo -e "\t  $> ./setup.sh PowerManagement ChibiOS"
+    echo -e "\twill create two projects."
+    echo -e ""
+    echo -e "-------------------------------------------------------------------------------"
+
+    cleanupVariables
+    exit
+fi
+
+
+
+#########################
+###   CONFIGURATION   ###
+#########################
+
+USER_DIR=${PWD}
+cd $(dirname ${BASH_SOURCE[0]})
+SCRIPT_DIR=${PWD}
+cd $USER_DIR
+
+### the include path for GCC specific headers
+GCC_ARM_NONE_EABI_INCLUDE_PATH="/opt/gcc-arm-none-eabi-4_8-2014q1/arm-none-eabi/include"
+read -p "gcc_arm_none_eabi include path:     " -i "$GCC_ARM_NONE_EABI_INCLUDE_PATH" -e GCC_ARM_NONE_EABI_INCLUDE_PATH
+# make absolute
+if [ "/$GCC_ARM_NONE_EABI_INCLUDE_PATH" != "$GCC_ARM_NONE_EABI_INCLUDE_PATH" ]; then
+  cd $GCC_ARM_NONE_EABI_INCLUDE_PATH
+  GCC_ARM_NONE_EABI_INCLUDE_PATH=${PWD}
+  cd $USER_DIR
+fi
+# append a slash if required
+if [ "${GCC_ARM_NONE_EABI_INCLUDE_PATH%"/"}" == "$GCC_ARM_NONE_EABI_INCLUDE_PATH" ]; then
+  GCC_ARM_NONE_EABI_INCLUDE_PATH="${GCC_ARM_NONE_EABI_INCLUDE_PATH}/"
+fi
+
+### the relative path where all project files shall be generated
+cd "$SCRIPT_DIR/../../"
+PROJECT_ROOT_PATH=${PWD}
+cd $USER_DIR
+read -p "path where to create project files: " -i "$PROJECT_ROOT_PATH" -e PROJECT_ROOT_PATH
+# make absolute
+if [ "/$PROJECT_ROOT_PATH" != "$PROJECT_ROOT_PATH" ]; then
+  cd $PROJECT_ROOT_PATH
+  PROJECT_ROOT_PATH=${PWD}
+  cd $USER_DIR
+fi
+# append a slash if required
+if [ "${PROJECT_ROOT_PATH%"/"}" == "$PROJECT_ROOT_PATH" ]; then
+  PROJECT_ROOT_PATH="${PROJECT_ROOT_PATH}/"
+fi
+
+# the relative path to the AMiRo-OS root directory
+cd "$SCRIPT_DIR/../../"
+AMIROOS_ROOT_PATH=${PWD}
+cd $USER_DIR
+read -p "AMiRo-OS root path:                 " -i "$AMIROOS_ROOT_PATH" -e AMIROOS_ROOT_PATH
+# make absolute
+if [ "/$AMIROOS_ROOT_PATH" != "$AMIROOS_ROOT_PATH" ]; then
+  cd $AMIROOS_ROOT_PATH
+  AMIROOS_ROOT_PATH=${PWD}
+  cd $USER_DIR
+fi
+# append a slash if required
+if [ "${AMIROOS_ROOT_PATH%"/"}" == "$AMIROOS_ROOT_PATH" ]; then
+  AMIROOS_ROOT_PATH="${AMIROOS_ROOT_PATH}/"
+fi
+
+# the relative path to the ChibiOS root directory
+cd "${AMIROOS_ROOT_PATH}../ChibiOS/"
+CHIBIOS_ROOT_PATH=${PWD}
+cd $USER_DIR
+read -p "ChibiOS root path:                  " -i "$CHIBIOS_ROOT_PATH" -e CHIBIOS_ROOT_PATH
+# make absolute
+if [ "/$CHIBIOS_ROOT_PATH" != "$CHIBIOS_ROOT_PATH" ]; then
+  cd $CHIBIOS_ROOT_PATH
+  CHIBIOS_ROOT_PATH=${PWD}
+  cd $USER_DIR
+fi
+# append a slash if required
+if [ "${CHIBIOS_ROOT_PATH%"/"}" == "$CHIBIOS_ROOT_PATH" ]; then
+  CHIBIOS_ROOT_PATH="${CHIBIOS_ROOT_PATH}/"
+fi
+
+# the prefix names for the projects to be generated
+CHIBIOS_PROJECT_PREFIX="ChibiOS"
+LIGHTRING_PROJECT_PREFIX="LightRing"
+POWERMANAGEMENT_PROJECT_PREFIX="PowerManagement"
+DIWHEELDRIVE_PROJECT_PREFIX="DiWheelDrive"
+
+echo ""
+
+
+
+#############################
+###   ENVIRONMENT SETUP   ###
+#############################
+
+# generate the relative path from the project root to ChibiOS
+absoluteToRelativePath "$PROJECT_ROOT_PATH" "$CHIBIOS_ROOT_PATH" PROJECT_ROOT_TO_CHIBIOS
+# generate the relative path from the project root to AMiRo-OS
+absoluteToRelativePath "$PROJECT_ROOT_PATH" "$AMIROOS_ROOT_PATH" PROJECT_ROOT_TO_AMIROOS
+
+# move back to the project root directory
+cd $PROJECT_ROOT_PATH
+
+
+
+######################
+###   CLEAN STEP   ###
+######################
+
+# clean the project files
+if [[ "${ARG_LIST[@]}" == *"$CLEAN_FLAG"* ]]; then
+  echo -n "removing existing project files..."
+
+  # remove all files
+  rm -f ${CHIBIOS_PROJECT_PREFIX}.includes
+  rm -f ${CHIBIOS_PROJECT_PREFIX}.files
+  rm -f ${CHIBIOS_PROJECT_PREFIX}.config
+  rm -f ${CHIBIOS_PROJECT_PREFIX}.creator
+
+  rm -f ${LIGHTRING_PROJECT_PREFIX}.includes
+  rm -f ${LIGHTRING_PROJECT_PREFIX}.files
+  rm -f ${LIGHTRING_PROJECT_PREFIX}.config
+  rm -f ${LIGHTRING_PROJECT_PREFIX}.creator
+
+  rm -f ${POWERMANAGEMENT_PROJECT_PREFIX}.includes
+  rm -f ${POWERMANAGEMENT_PROJECT_PREFIX}.files
+  rm -f ${POWERMANAGEMENT_PROJECT_PREFIX}.config
+  rm -f ${POWERMANAGEMENT_PROJECT_PREFIX}.creator
+
+  rm -f ${DIWHEELDRIVE_PROJECT_PREFIX}.includes
+  rm -f ${DIWHEELDRIVE_PROJECT_PREFIX}.files
+  rm -f ${DIWHEELDRIVE_PROJECT_PREFIX}.config
+  rm -f ${DIWHEELDRIVE_PROJECT_PREFIX}.creator
+
+  echo -e "\tdone"
+fi
+
+# wipe even the user specific files (created by QtCreator)
+if [[ "${ARG_LIST[@]}" == *"$WIPE_FLAG"* ]]; then
+  echo -n "removing existing user specific project files..."
+
+  # remove all user files
+  rm -f ${CHIBIOS_PROJECT_PREFIX}.creator.user
+  rm -f ${LIGHTRING_PROJECT_PREFIX}.creator.user
+  rm -f ${POWERMANAGEMENT_PROJECT_PREFIX}.creator.user
+  rm -f ${DIWHEELDRIVE_PROJECT_PREFIX}.creator.user
+
+  echo -e "\tdone"
+fi
+
+
+
+#########################
+###   CHIBIOS SETUP   ###
+#########################
+
+if [[ "${ARG_LIST[@]}" == *"$CHIBIOS_FLAG"* ]]; then
+  echo -n "creating project files for ${CHIBIOS_PROJECT_PREFIX}..."
+
+  # generate a file that contains all subdirectories as includes (but ignore hidden and documentation directories)
+  find $PROJECT_ROOT_TO_CHIBIOS -type d | grep -v /[.][^.] | grep -v /doc/ > ${CHIBIOS_PROJECT_PREFIX}.includes
+  # generate a file that specifies all files
+  find $PROJECT_ROOT_TO_CHIBIOS -type f -iname "*.c" -o -iname "*.cpp" -o -iname "*.h" -o -iname "*.hpp" -o -iname "*.s" | grep -v /[.][^.] | grep -v /doc/ > ${CHIBIOS_PROJECT_PREFIX}.files
+  # generate a default project configuration file if none exists so far
+  if [ ! -f ${CHIBIOS_PROJECT_PREFIX}.config ]; then
+    echo -e "// Add predefined macros for your project here. For example:\n// #define YOUR_CONFIGURATION belongs here\n" > ${CHIBIOS_PROJECT_PREFIX}.config
+  fi
+  # generate a default .creator file if none exists so far
+  if [ ! -f ${CHIBIOS_PROJECT_PREFIX}.creator ]; then
+    echo -e "[general]\n" > ${CHIBIOS_PROJECT_PREFIX}.creator
+  fi
+
+  echo -e "\tdone"
+fi
+
+
+
+###########################
+###   LIGHTRING SETUP   ###
+###########################
+
+if [[ "${ARG_LIST[@]}" == *"$LIGHTRING_FLAG"* ]]; then
+  echo -n "creating project files for ${LIGHTRING_PROJECT_PREFIX}..."
+
+  # generate a file that contains all subdirectories as includes (but ignore hidden and documentation directories)
+  echo $GCC_ARM_NONE_EABI_INCLUDE_PATH > ${LIGHTRING_PROJECT_PREFIX}.includes
+  find $PROJECT_ROOT_TO_AMIROOS -type d | grep -v /[.][^.] | grep -v /doc/ | grep "devices\|include\|util\|boards\|components\|hal" | grep -iv "PowerManagement" | grep -iv "DiWheelDrive" >> ${LIGHTRING_PROJECT_PREFIX}.includes
+  find $PROJECT_ROOT_TO_CHIBIOS -type d | grep -v /[.][^.] | grep -v /doc/ | grep /os/ >> ${LIGHTRING_PROJECT_PREFIX}.includes
+  # generate a file that specifies all files
+  find $PROJECT_ROOT_TO_AMIROOS -type f -iname "*.c" -o -iname "*.cpp" -o -iname "*.h" -o -iname "*.hpp" -o -iname "*.s" -o -iname "*.tpp" | grep -v /[.][^.] | grep -v /doc/ | grep -iv "PowerManagement" | grep -iv "DiWheelDrive" > ${LIGHTRING_PROJECT_PREFIX}.files
+  find $PROJECT_ROOT_TO_CHIBIOS -type f -iname "*.c" -o -iname "*.cpp" -o -iname "*.h" -o -iname "*.hpp" -o -iname "*.s" | grep -v /[.][^.] | grep -v /doc/ | grep /os/ >> ${LIGHTRING_PROJECT_PREFIX}.files
+  # generate a default project configuration file if none exists so far
+  if [ ! -f ${LIGHTRING_PROJECT_PREFIX}.config ]; then
+    echo -e "// Add predefined macros for your project here. For example:\n//#define YOUR_CONFIGURATION belongs here\n" > ${LIGHTRING_PROJECT_PREFIX}.config
+  fi
+  # generate a default .creator file if none exists so far
+  if [ ! -f ${LIGHTRING_PROJECT_PREFIX}.creator ]; then
+    echo -e "[general]\n" > ${LIGHTRING_PROJECT_PREFIX}.creator
+  fi
+
+  echo -e "\tdone"
+fi
+
+
+
+#################################
+###   POWERMANAGEMENT SETUP   ###
+#################################
+
+if [[ "${ARG_LIST[@]}" == *"$POWERMANAGEMENT_FLAG"* ]]; then
+  echo -n "creating project files for ${POWERMANAGEMENT_PROJECT_PREFIX}..."
+
+  # generate a file that contains all subdirectories as includes (but ignore hidden and documentation directories)
+  echo $GCC_ARM_NONE_EABI_INCLUDE_PATH > ${POWERMANAGEMENT_PROJECT_PREFIX}.includes
+  find $PROJECT_ROOT_TO_AMIROOS -type d | grep -v /[.][^.] | grep -v /doc/ | grep "devices\|include\|util\|boards\|components\|hal" | grep -iv "LightRing" | grep -iv "DiWheelDrive" >> ${POWERMANAGEMENT_PROJECT_PREFIX}.includes
+  find $PROJECT_ROOT_TO_CHIBIOS -type d | grep -v /[.][^.] | grep -v /doc/ >> ${POWERMANAGEMENT_PROJECT_PREFIX}.includes
+  # generate a file that specifies all files
+  find $PROJECT_ROOT_TO_AMIROOS -type f -iname "*.c" -o -iname "*.cpp" -o -iname "*.h" -o -iname "*.hpp" -o -iname "*.s" -o -iname "*.tpp" | grep -v /[.][^.] | grep -v /doc/ | grep -iv "LightRing" | grep -iv "DiWheelDrive" > ${POWERMANAGEMENT_PROJECT_PREFIX}.files
+  find $PROJECT_ROOT_TO_CHIBIOS -type f -iname "*.c" -o -iname "*.cpp" -o -iname "*.h" -o -iname "*.hpp" -o -iname "*.s" | grep -v /[.][^.] | grep -v /doc/ | grep /os/ >> ${POWERMANAGEMENT_PROJECT_PREFIX}.files
+  # generate a default project configuration file if none exists so far
+  if [ ! -f ${POWERMANAGEMENT_PROJECT_PREFIX}.config ]; then
+    echo -e "// Add predefined macros for your project here. For example:\n//#define YOUR_CONFIGURATION belongs here\n" > ${POWERMANAGEMENT_PROJECT_PREFIX}.config
+  fi
+  # generate a default .creator file if none exists so far
+  if [ ! -f ${POWERMANAGEMENT_PROJECT_PREFIX}.creator ]; then
+    echo -e "[general]\n" > ${POWERMANAGEMENT_PROJECT_PREFIX}.creator
+  fi
+
+  echo -e "\tdone"
+fi
+
+
+
+##############################
+###   DIWHEELDRIVE SETUP   ###
+##############################
+
+if [[ "${ARG_LIST[@]}" == *"$DIWHEELDRIVE_FLAG"* ]]; then
+  echo -n "creating project files for ${DIWHEELDRIVE_PROJECT_PREFIX}..."
+
+  # generate a file that contains all subdirectories as includes (but ignore hidden and documentation directories)
+  echo $GCC_ARM_NONE_EABI_INCLUDE_PATH > ${DIWHEELDRIVE_PROJECT_PREFIX}.includes
+  find $PROJECT_ROOT_TO_AMIROOS -type d | grep -v /[.][^.] | grep -v /doc/ | grep "devices\|include\|util\|boards\|components\|hal" | grep -iv "LightRing" | grep -iv "PowerManagement" >> ${DIWHEELDRIVE_PROJECT_PREFIX}.includes
+  find $PROJECT_ROOT_TO_CHIBIOS -type d | grep -v /[.][^.] | grep -v /doc/ >> ${DIWHEELDRIVE_PROJECT_PREFIX}.includes
+  # generate a file that specifies all files
+  find $PROJECT_ROOT_TO_AMIROOS -type f -iname "*.c" -o -iname "*.cpp" -o -iname "*.h" -o -iname "*.hpp" -o -iname "*.s" -o -iname "*.tpp" | grep -v /[.][^.] | grep -v /doc/ | grep -iv "LightRing" | grep -iv "PowerManagement" > ${DIWHEELDRIVE_PROJECT_PREFIX}.files
+  find $PROJECT_ROOT_TO_CHIBIOS -type f -iname "*.c" -o -iname "*.cpp" -o -iname "*.h" -o -iname "*.hpp" -o -iname "*.s" | grep -v /[.][^.] | grep -v /doc/ | grep /os/ >> ${DIWHEELDRIVE_PROJECT_PREFIX}.files
+  # generate a default project configuration file if none exists so far
+  if [ ! -f ${DIWHEELDRIVE_PROJECT_PREFIX}.config ]; then
+    echo -e "// Add predefined macros for your project here. For example:\n//#define YOUR_CONFIGURATION belongs here\n" > ${DIWHEELDRIVE_PROJECT_PREFIX}.config
+  fi
+  # generate a default .creator file if none exists so far
+  if [ ! -f ${DIWHEELDRIVE_PROJECT_PREFIX}.creator ]; then
+    echo -e "[general]\n" > ${DIWHEELDRIVE_PROJECT_PREFIX}.creator
+  fi
+
+  echo -e "\tdone"
+fi
+
+
+
+#################
+###   OUTRO   ###
+#################
+
+cleanupVariables
+
+echo ""
+echo "-------------------------------------------------------------------------------"
+
diff --git a/include/Matrix.h b/include/Matrix.h
new file mode 100644
index 0000000..818e98c
--- /dev/null
+++ b/include/Matrix.h
@@ -0,0 +1,174 @@
+#ifndef MATRIX_H_
+#define MATRIX_H_
+
+#include <algorithm> // std::fill, std::copy
+
+namespace Matrix {
+
+  typedef unsigned int uint;
+
+  /**
+    * Multiplication of two matrices Z=X*Y
+    *
+    * @param *X Pointer to first element of first matrix
+    * @param Xrows Rows of X
+    * @param Xcols Cols of X
+    * @param *Y Pointer to first element of second matrix
+    * @param Yrows Rows of Y
+    * @param Ycols Cols of Y
+    * @param *Z Pointer to first element of result matrix (needs to be init with 0)
+    * @param Zrows Rows of Z
+    * @param Zcols Cols of Z
+    */
+  template <typename T>
+  void XdotY(T* X,uint Xrows,uint Xcols,T* Y,uint Yrows, uint Ycols,T* Z, uint Zrows,uint Zcols) {
+
+    // Check if the multiplication will work
+//       if (Zrows != Xrows || Zcols != Ycols || Xcols != Yrows) {
+//   return 1;
+//       }
+
+    // Do the multiplication
+    // Z_{i,k} = sum_{n} X_{i,n} * Y_{n,k}
+    for (uint i = 0; i < Zrows; i++) {
+      for (uint k = 0; k < Zcols; k++) {
+  for (uint n = 0; n < Xcols; n++) {
+    Z[i*Zcols+k] += X[i*Xcols+n] * Y[n*Ycols+k];
+  }
+      }
+    }
+//       return 0;
+  };
+
+  /**
+    * Multiplication of two matrices Z=X*Y^{T},
+    * while Y will be transposed during calculation
+    *
+    * @param *X Pointer to first element of first matrix
+    * @param Xrows Rows of X
+    * @param Xcols Cols of X
+    * @param *Y Pointer to first element of second matrix
+    * @param Yrows Rows of Y
+    * @param Ycols Cols of Y
+    * @param *Z Pointer to first element of result matrix (needs to be init with 0)
+    * @param Zrows Rows of Z
+    * @param Zcols Cols of Z
+    */
+  template <typename T>
+  void XdotYtrans(T* X,uint Xrows,uint Xcols,T* Y,uint Yrows, uint Ycols,T* Z, uint Zrows,uint Zcols) {
+
+    // Check if the multiplication will work
+//       if (Zrows != Xrows || Zcols != Yrows || Xcols != Ycols) {
+//   return 1;
+//       }
+
+    // Do the multiplication
+    // Z_{i,k} = sum_{n} X_{i,n} * Y_{k,n}
+    for (uint i = 0; i < Zrows; i++) {
+      for (uint k = 0; k < Zcols; k++) {
+  for (uint n = 0; n < Xcols; n++) {
+    Z[i*Zcols+k] += X[i*Xcols+n] * Y[n+k*Ycols];
+  }
+      }
+    }
+//       return 0;
+  };
+
+/**
+    * Multiplication of two matrices Z=X^{T}*Y,
+    * while X will be transposed during calculation
+    *
+    * @param *X Pointer to first element of first of matrix
+    * @param Xrows Rows of X
+    * @param Xcols Cols of X
+    * @param *Y Pointer to first element of second matrix
+    * @param Yrows Rows of Y
+    * @param Ycols Cols of Y
+    * @param *Z Pointer to first element of result matrix (needs to be init with 0)
+    * @param Zrows Rows of Z
+    * @param Zcols Cols of Z
+    */
+  template <typename T>
+  void XtransDotY(T* X, uint Xrows,uint Xcols,T* Y,uint Yrows, uint Ycols,T* Z, uint Zrows,uint Zcols) {
+
+    // Check if the multiplication will work
+//       if (Zrows != Xrows || Zcols != Yrows || Xcols != Ycols) {
+//   return 1;
+//       }
+
+    // Do the multiplication
+    // Z_{i,k} = sum_{n} X_{n,i} * Y_{k,n}
+    for (uint i = 0; i < Zrows; i++) {
+      for (uint k = 0; k < Zcols; k++) {
+        for (uint n = 0; n < Xcols; n++) {
+          Z[i*Zcols+k] += X[n*Xcols+i] * Y[n+k*Ycols];
+        }
+      }
+    }
+//       return 0;
+  };
+
+    /**
+    * Addition of two matrices Z=X+Y,
+    *
+    * @param *X Pointer to first element of first matrix
+    * @param Xrows Rows of X
+    * @param Xcols Cols of X
+    * @param *Y Pointer to first element of second matrix
+    * @param Yrows Rows of Y
+    * @param Ycols Cols of Y
+    * @param *Z Pointer to first element of result matrix (needs to be init with 0)
+    * @param Zrows Rows of Z
+    * @param Zcols Cols of Z
+    */
+  template <typename T>
+  void XplusY(T* X,uint Xrows,uint Xcols,T* Y,uint Yrows, uint Ycols,T* Z, uint Zrows,uint Zcols) {
+
+    // Check if the multiplication will work
+//       if (Zrows != Xrows || Xrows != Yrows || Zcols != Ycols || Ycols != Xcols {
+//   return 1;
+//       }
+
+    // Do the summation
+  for (uint idx = 0; idx < Xrows*Xcols; idx++) {
+      Z[idx] += X[idx] + Y[idx];
+    }
+//       return 0;
+  };
+
+  /**
+    * Initializing matrix with any element
+    *
+    * @param *X Pointer to first element of first matrix
+    * @param Xrows Rows of X
+    * @param Xcols Cols of X
+    * @param value value of all elements
+    */
+  template <typename T>
+  void init(T* X, uint Xrows, uint Xcols, T value) {
+    std::fill (X,X+Xrows*Xcols,value);
+  };
+
+  /**
+    * Copy a matrix Y=X
+    *
+    * @param *X Pointer to first element of source matrix
+    * @param Xrows Rows of X
+    * @param Xcols Cols of X
+    * @param *Y Pointer to first element of result matrix
+    * @param Yrows Rows of Y
+    * @param Ycols Cols of Y
+    *
+    */
+  template <typename T>
+  void copy(T* X, uint Xrows, uint Xcols, T* Y, uint Yrows, uint Ycols) {
+    // if (Xrows != Yrows || Xcols != Ycols ) {
+//         return 1;
+//       }
+    std::copy (X,X+Xrows*Xcols,Y);
+//       return 0;
+  };
+
+}
+
+#endif /* MATRIX_ */
diff --git a/include/Types.h b/include/Types.h
new file mode 100644
index 0000000..999c706
--- /dev/null
+++ b/include/Types.h
@@ -0,0 +1,107 @@
+#ifndef TYPES_H_
+#define TYPES_H_
+
+/*! \brief Types to use if you work with the AMiRo platform
+ *
+ *  This header contains types which has to be used
+ *  if they suit the problem.
+ *  All physical constants (therefore all values with a
+ *  physical unit) are implicitly in µ iff the variable
+ *  is of type integer, unless it is explicitly named in
+ *  the variable.
+ *  All physical constants (therefore all values with a
+ *  physical unit) are implicitly without prefix (e.g. µ)
+ *  iff the variable is of type float, unless it is
+ *  explicitly named in the variable.
+ *  For the types, the SI prefix is used, iff the variable
+ *  is of type float and therefor in SI units.
+ */
+
+namespace types {
+
+  /**
+   * A structure to represent the position and orientation of the robot
+   */
+  struct position {
+    /*@{*/
+    int32_t x;   /**< the x coordinate in µm */
+    int32_t y;   /**< the y coordinate in µm */
+    int32_t z;   /**< the z coordinate in µm */
+    int32_t f_x; /**< the f_x orientation in µrad */
+    int32_t f_y; /**< the f_y orientation in µrad */
+    int32_t f_z; /**< the f_z orientation in µrad */
+    /*@}*/
+  };
+
+  /**
+   * A structure to represent the position and orientation of the robot
+   */
+  struct positionSI {
+    /*@{*/
+    float x;   /**< the x coordinate in m */
+    float y;   /**< the y coordinate in m */
+    float z;   /**< the z coordinate in m */
+    float f_x; /**< the f_x coordinate in rad */
+    float f_y; /**< the f_y coordinate in rad */
+    float f_z; /**< the f_z coordinate in rad */
+    /*@}*/
+  };
+
+  /**
+   * A structure to represent the kinematics of the robot
+   */
+  struct kinematic {
+    /*@{*/
+    int32_t x;   /**< the x velocity in µm/s */
+    int32_t y;   /**< the y velocity in µm/s */
+    int32_t z;   /**< the z velocity in µm/s */
+    int32_t w_x; /**< the w_x angular velocity in µrad/s */
+    int32_t w_y; /**< the w_y angular velocity in µrad/s */
+    int32_t w_z; /**< the w_z angular velocity in µrad/s */
+    /*@}*/
+  };
+
+  /**
+   * A structure to represent the kinematics of the robot
+   */
+  struct kinematicSI {
+    /*@{*/
+    float x;   /**< the x velovity in m/s */
+    float y;   /**< the y velocity in m/s */
+    float z;   /**< the z velocity in m/s */
+    float f_x; /**< the f_x angular velocity in rad/s */
+    float f_y; /**< the f_y angular velocity in rad/s */
+    float f_z; /**< the f_z angular velocity in rad/s */
+    /*@}*/
+  };
+
+  /**
+   * A structure to represent the power status of the robot
+   */
+  struct power_status {
+    /**
+     * @brief A union to encode the current charging status
+     */
+    union ChargingState {
+      uint8_t value = 0;
+      struct {
+        uint8_t powermanagement_plugged_in      : 1;  /**< status flag, whether the PowerManagement board is plugged in (physical plug detected) */
+        uint8_t powermanagement_charging        : 1;  /**< status flag, whether the batteries are currently charged via the PowerManagement board */
+        uint8_t diwheeldrive_enable_power_path  : 1;  /**< a flag to enable/disable charging via the DiWheelDrive board */
+        uint8_t diwheeldrive_charging           : 1;  /**< status flag, whether the batteries are currently charged via the DiWheelDrive board */
+        uint8_t vsys_higher_than_9V             : 1;  /**< status flag, whether vsys is currently higher than 9V */
+        uint8_t rsvd                            : 3;
+      } content;
+    };
+
+    /*@{*/
+    ChargingState charging_flags; /**< charging status */
+    uint8_t state_of_charge;      /**< state of charge of the batteries in percent */
+    uint16_t minutes_remaining;   /**< remaining time until the batteries are fully charged/discharged in minutes*/
+    uint16_t power_consumption;   /**< current power consumption in mW */
+    /*@}*/
+  };
+
+}
+
+#endif /* TYPES_ */
diff --git a/include/amiro/BaseSensor.hpp b/include/amiro/BaseSensor.hpp
new file mode 100644
index 0000000..850c9ef
--- /dev/null
+++ b/include/amiro/BaseSensor.hpp
@@ -0,0 +1,113 @@
+#ifndef AMIRO_BASE_SENSOR_H_
+#define AMIRO_BASE_SENSOR_H_
+
+/* ABOUT THE USED PREPROCESSOR DIRECTIVES IN THIS FILE
+ * AMIRO_NCALIBRATION:
+ *  If defined, the calibration() function will be ignored for compilation.
+ *  Thus, all inheriting classes must use this directive, too.
+ * AMIRO_NSELFTEST:
+ *  If defined, the selftest() function will be ignored for compilation.
+ *  Thus, all inheriting classes must use this directive, too.
+ */
+
+namespace amiro {
+
+  /**
+   * This is an interface class, which should be used as a blueprint for all drivers of the AMiRo.
+   */
+  template<typename Init_t = void, typename Calib_t = void>
+  class BaseSensor
+  {
+  public:
+    BaseSensor() {}
+    virtual ~BaseSensor() {}
+
+    /**
+     * Some error codes that should be returned by the implementations of the virtual functions of this class.
+     */
+    enum ErrorCodes {
+      SUCCESS = 0x00u, OK = 0x00u,
+      ERROR = 0x80u, FAIL = 0x80u,
+      NOT_IMPLEMENTED = 0xFFu
+    };
+
+    /**
+     * Initializes the hardware (e.g. sets register values).
+     * This function is optional.
+     *
+     * @param[in,out] driver specific initialization data (should be used for arguments and results); optional
+     * @return an error code
+     */
+    virtual inline msg_t init(Init_t* = NULL)
+    {
+      return NOT_IMPLEMENTED;
+    }
+
+    /**
+     * Reads the most important values from the hardware and stores them in local buffers.
+     *
+     * @return an error code
+     */
+    virtual msg_t update() = 0;
+
+    /**
+     * If required, wakes the hardware from power saving mode into an active state.
+     * (Re)Initializes all local buffers with valid values, thus the update() function should be called.
+     *
+     * @return an error code
+     */
+    virtual msg_t wakeup() = 0;
+
+    /**
+     * Puts the hardware in a power saving mode to reduce energy consumption.
+     * This function is optional.
+     * @return an error code
+     */
+    virtual msg_t hibernate()
+    {
+      return NOT_IMPLEMENTED;
+    }
+
+#ifndef AMIRO_NCALIBRATION
+    /**
+     * Runs a calibration of the hardware.
+     * This function is optional.
+     *
+     * @param[in,out] driver specific calibration data (should be used for arguments and results); optional
+     *
+     * @return an error code
+     */
+    virtual msg_t calibration(Calib_t* = NULL)
+    {
+      return NOT_IMPLEMENTED;
+    }
+#else
+    static uint8_t calibration(Calib_t* = NULL)
+    {
+      return NOT_IMPLEMENTED;
+    }
+#endif
+
+#ifndef AMIRO_NSELFTEST
+    /**
+     * Runs a self-test on the hardware and/or communication.
+     * This function is optional but highly recommended.
+     *
+     * @return an error code
+     */
+    virtual msg_t selftest()
+    {
+      return NOT_IMPLEMENTED;
+    }
+#else
+    static msg_t selftest()
+    {
+      return NOT_IMPLEMENTED;
+    }
+#endif
+
+  };
+
+}
+
+#endif /* AMIRO_BASE_SENSOR_H_ */
diff --git a/include/amiro/Color.h b/include/amiro/Color.h
new file mode 100644
index 0000000..aab533c
--- /dev/null
+++ b/include/amiro/Color.h
@@ -0,0 +1,181 @@
+#ifndef AMIRO_COLOR_H_
+#define AMIRO_COLOR_H_
+
+namespace amiro {
+
+  class Color {
+  public:
+    enum GlobalColor {
+      ALICEBLUE,
+      ANTIQUEWHITE,
+      AQUA,
+      AQUAMARINE,
+      AZURE,
+      BEIGE,
+      BISQUE,
+      BLACK,
+      BLANCHEDALMOND,
+      BLUE,
+      BLUEVIOLET,
+      BROWN,
+      BURLYWOOD,
+      CADETBLUE,
+      CHARTREUSE,
+      CHOCOLATE,
+      CORAL,
+      CORNFLOWERBLUE,
+      CORNSILK,
+      CRIMSON,
+      CYAN,
+      DARKBLUE,
+      DARKCYAN,
+      DARKGOLDENROD,
+      DARKGRAY,
+      DARKGREEN,
+      DARKGREY,
+      DARKKHAKI,
+      DARKMAGENTA,
+      DARKOLIVEGREEN,
+      DARKORANGE,
+      DARKORCHID,
+      DARKRED,
+      DARKSALMON,
+      DARKSEAGREEN,
+      DARKSLATEBLUE,
+      DARKSLATEGRAY,
+      DARKSLATEGREY,
+      DARKTURQUOISE,
+      DARKVIOLET,
+      DEEPPINK,
+      DEEPSKYBLUE,
+      DIMGRAY,
+      DIMGREY,
+      DODGERBLUE,
+      FIREBRICK,
+      FLORALWHITE,
+      FORESTGREEN,
+      FUCHSIA,
+      GAINSBORO,
+      GHOSTWHITE,
+      GOLD,
+      GOLDENROD,
+      GRAY,
+      GREY,
+      GREEN,
+      GREENYELLOW,
+      HONEYDEW,
+      HOTPINK,
+      INDIANRED,
+      INDIGO,
+      IVORY,
+      KHAKI,
+      LAVENDER,
+      LAVENDERBLUSH,
+      LAWNGREEN,
+      LEMONCHIFFON,
+      LIGHTBLUE,
+      LIGHTCORAL,
+      LIGHTCYAN,
+      LIGHTGOLDENRODYELLOW,
+      LIGHTGRAY,
+      LIGHTGREEN,
+      LIGHTGREY,
+      LIGHTPINK,
+      LIGHTSALMON,
+      LIGHTSEAGREEN,
+      LIGHTSKYBLUE,
+      LIGHTSLATEGRAY,
+      LIGHTSLATEGREY,
+      LIGHTSTEELBLUE,
+      LIGHTYELLOW,
+      LIME,
+      LIMEGREEN,
+      LINEN,
+      MAGENTA,
+      MAROON,
+      MEDIUMAQUAMARINE,
+      MEDIUMBLUE,
+      MEDIUMORCHID,
+      MEDIUMPURPLE,
+      MEDIUMSEAGREEN,
+      MEDIUMSLATEBLUE,
+      MEDIUMSPRINGGREEN,
+      MEDIUMTURQUOISE,
+      MEDIUMVIOLETRED,
+      MIDNIGHTBLUE,
+      MINTCREAM,
+      MISTYROSE,
+      MOCCASIN,
+      NAVAJOWHITE,
+      NAVY,
+      OLDLACE,
+      OLIVE,
+      OLIVEDRAB,
+      ORANGE,
+      ORANGERED,
+      ORCHID,
+      PALEGOLDENROD,
+      PALEGREEN,
+      PALETURQUOISE,
+      PALEVIOLETRED,
+      PAPAYAWHIP,
+      PEACHPUFF,
+      PERU,
+      PINK,
+      PLUM,
+      POWDERBLUE,
+      PURPLE,
+      RED,
+      ROSYBROWN,
+      ROYALBLUE,
+      SADDLEBROWN,
+      SALMON,
+      SANDYBROWN,
+      SEAGREEN,
+      SEASHELL,
+      SIENNA,
+      SILVER,
+      SKYBLUE,
+      SLATEBLUE,
+      SLATEGRAY,
+      SLATEGREY,
+      SNOW,
+      SPRINGGREEN,
+      STEELBLUE,
+      TAN,
+      TEAL,
+      THISTLE,
+      TOMATO,
+      TURQUOISE,
+      VIOLET,
+      WHEAT,
+      WHITE,
+      WHITESMOKE,
+      YELLOW,
+      YELLOWGREEN
+    };
+
+    Color();
+    Color(const Color& color);
+    Color(int r, int g, int b);
+    Color(GlobalColor color);
+    Color darker(int factor);
+    void getRedGreenBlue(int* r, int* g, int* b);
+    int getRed();
+    int getGreen();
+    int getBlue();
+    Color lighter(int factor);
+    void setRedGreenBlue(int r, int g, int b);
+
+    Color& operator=(Color const& color);
+    Color& operator=(GlobalColor color);
+
+  private:
+    unsigned char red;
+    unsigned char green;
+    unsigned char blue;
+  };
+
+}
+
+#endif /* AMIRO_COLOR_H_ */
diff --git a/include/amiro/Constants.h b/include/amiro/Constants.h
new file mode 100644
index 0000000..0c38fd7
--- /dev/null
+++ b/include/amiro/Constants.h
@@ -0,0 +1,234 @@
+#ifndef AMIRO_CONSTANTS_H_
+#define AMIRO_CONSTANTS_H_
+
+/*! \brief Constants regarding the AMiRo platform
+ *
+ *  This header contains constant variables
+ *  regarding the AMiRo platform, which means that
+ *  these values do not change during runtime.
+ *  Constants are e.g. physical ones like seconds per minute
+ *  or geometrical ones like the circumference of wheel.
+ *  All physical constants (therefore all values with a
+ *  physical unit) are implicitly in µ iff the variable
+ *  is of type integer, unless it is explicitly named in
+ *  the variable.
+ *  All physical constants (therefore all values with a
+ *  physical unit) are implicitly without prefix (e.g. µ)
+ *  iff the variable is of type float, unless it is
+ *  explicitly named in the variable. The SI prefix is
+ *  used, iff the variable is of type float and therefor
+ *  in SI units.
+ */
+
+#include <math.h>
+#include <stdint.h>
+
+/* CAN_* defines start */
+
+/** \brief Controller Area Network specific defines
+ *
+ * These CAN_* defines are used in ControllerAreaNetworkRx.h
+ * and ControllerAreaNetworkTx.h
+ */
+
+/* CAN_* defines end */
+
+namespace amiro {
+
+namespace CAN {
+
+  const uint32_t UPDATE_PERIOD_MSEC        = MS2ST(125);
+
+  const uint32_t PERIODIC_TIMER_ID         = 1;
+  const uint32_t RECEIVED_ID               = 2;
+
+  const uint32_t BOARD_ID_SHIFT            = 0x00u;
+  const uint32_t BOARD_ID_MASK             = 0x07u;
+  const uint32_t DEVICE_ID_SHIFT           = 0x03u;
+  const uint32_t DEVICE_ID_MASK            = 0xFFu;
+  const uint32_t INDEX_ID_SHIFT            = 0x03u;
+  const uint32_t INDEX_ID_MASK             = 0x07u;
+
+  const uint32_t DI_WHEEL_DRIVE_ID         = 1;
+  const uint32_t POWER_MANAGEMENT_ID       = 2;
+  const uint32_t LIGHT_RING_ID             = 3;
+  const uint32_t COGNITION                 = 4;
+
+
+  const uint32_t PROXIMITY_FLOOR_ID        = 0x51;
+  const uint32_t ODOMETRY_ID               = 0x50;
+  const uint32_t BRIGHTNESS_ID             = 0x40;
+  inline constexpr uint32_t COLOR_ID(uint32_t index)             {return 0x38 | ((index) & 0x7);}
+  inline constexpr uint32_t PROXIMITY_RING_ID(uint32_t index)    {return 0x30 | ((index) & 0x7);}
+  const uint32_t SET_KINEMATIC_CONST_ID    = 0x22;
+  const uint32_t TARGET_POSITION_ID        = 0x21;
+  const uint32_t ACTUAL_SPEED_ID           = 0x20;
+  const uint32_t SET_ODOMETRY_ID           = 0x12;
+  const uint32_t TARGET_RPM_ID             = 0x11;
+  const uint32_t TARGET_SPEED_ID           = 0x10;
+  const uint32_t POWER_STATUS_ID           = 0x60;
+  const uint32_t ROBOT_ID                  = 0x48;
+  inline constexpr uint32_t SHELL_QUERY_ID(uint8_t index)        {return 0x70 | ((index) & 0x7);}
+  inline constexpr uint32_t SHELL_REPLY_ID(uint8_t index)        {return 0x78 | ((index) & 0x7);}
+  const uint32_t BROADCAST_SHUTDOWN        = 0x80u;
+
+  const uint32_t CALIBRATE_PROXIMITY_FLOOR = 0x81u;
+  const uint32_t CALIBRATE_PROXIMITY_RING  = 0x82u;
+
+  const uint32_t SHUTDOWN_MAGIC            = 0xAA55u;
+}
+
+namespace constants {
+
+  /** \brief Amount of seconds per minute */
+  const int32_t secondsPerMinute = 60;
+
+  /** \brief Amount of minutes per hour */
+  const int32_t minutesPerHour = 60;
+
+  /** \brief Amount of milliseconds per second */
+  const int32_t millisecondsPerSecond = 1000;
+
+  /* Several definitions of PI */
+  constexpr float    PI   = float(M_PI);                /**< PI approximated with single precision floating point */
+  constexpr uint32_t PIe9 = (M_PI * 1000000000) + 0.5f; /**< PI approximated with 32-bit integer and multiplied by factor 1e9 */
+  constexpr uint32_t PIe6 = (M_PI * 1000000) + 0.5f;    /**< PI approximated with 32-bit integer and multiplied by factor 1e6 */
+  constexpr uint16_t PIe3 = (M_PI * 1000) + 0.5f;       /**< PI approximated with 16-bit integer and multiplied by factor 1e3 */
+  constexpr uint16_t PIe2 = (M_PI * 100) + 0.5f;        /**< PI approximated with 16-bit integer and multiplied by factor 1e2 */
+  constexpr uint8_t  PIe1 = (M_PI * 10) + 0.5f;         /**< PI approximated with 8-bit integer and multiplied by factor 1e1 */
+  constexpr uint8_t  PIe0 = (M_PI * 1) + 0.5f;          /**< PI approximated with 8-bit integer and multiplied by factor 1e0 */
+
+namespace LightRing {
+
+  /** \brief Index of the top LEDs
+   *
+   * Top view of the AMiRo top LEDs and their indices:
+   *   _______
+   *  / 7 F 0 \
+   * |6       1|
+   * |5       2|
+   *  \_4_B_3_/
+   */
+  enum ledIndex : uint8_t {
+    LED_BL = 4, LED_BACK_LEFT = 4, LED_SSW = 4, LED_SOUTH_SOUTHWEST = 4,
+    LED_LB = 5, LED_LEFT_BACK = 5, LED_WSW = 5, LED_WEST_SOUTHWEST = 5,
+    LED_LF = 6, LED_LEFT_FRONT = 6, LED_WNW = 6, LED_WEST_NORTHWEST = 6,
+    LED_FL = 7, LED_FRONT_LEFT = 7, LED_NNW = 7, LED_NORTH_NORTHWEST = 7,
+    LED_FR = 0, LED_FRONT_RIGHT = 0, LED_NNE = 0, LED_NORTH_NORTHEAST = 0,
+    LED_RF = 1, LED_RIGHT_FRONT = 1, LED_ENE = 1, LED_EAST_NORTHEAST = 1,
+    LED_RB = 2, LED_RIGHT_BACK = 2, LED_ESE = 2, LED_EAST_SOUTHEAST = 2,
+    LED_BR = 3, LED_BACK_RIGHT = 3, LED_SSE = 3, LED_SOUTH_SOUTHEAST = 3
+  };
+}
+
+namespace DiWheelDrive {
+
+  /** \brief Distance between wheels in meter */
+  const float wheelBaseDistanceSI = 0.069f;
+
+  /** \brief Distance between wheels in micrometer */
+  const int32_t wheelBaseDistance = wheelBaseDistanceSI * 1e6;
+
+  /** \brief Wheel diameter in meter */
+  const float wheelDiameterSI = 0.05571f;
+
+  /** \brief Wheel diameter */
+  const int32_t wheelDiameter = wheelDiameterSI * 1e6;
+
+  /** \brief Wheel circumference in meter */
+  const float wheelCircumferenceSI = M_PI * wheelDiameterSI;
+
+  /** \brief Wheel circumference in micrometer */
+  const int32_t wheelCircumference = wheelCircumferenceSI * 1e6;
+
+  /** \brief Wheel error in meter (topview left:0, right:1) */
+  const float wheelErrorSI[2] = {0.1, 0.1};
+
+  /** \brief Wheel error in meter (topview left:0, right:1) */
+  const int32_t wheelError[2] = {(int32_t) (wheelErrorSI[0] * 1e6), (int32_t) (wheelErrorSI[1] * 1e6)};
+
+  /** \brief Motor increments per revolution
+   *
+   *  The increments are produced by 2 channels á 16
+   *  pulses per revolution with respect to the rising
+   *  and falling signal => 2*2*16 pulses/revolution.
+   *  The gearbox is 22:1 => 2*2*16*22 pulses/revolution
+   */
+  const int32_t incrementsPerRevolution = 2 * 2 * 16 * 22;
+
+  /** \brief Index of the proximity sensors
+   *
+   * Bottom view of the AMiRo sensors and their indices (F:Front, B:Back):
+   *  _____
+   * / 0F3 \
+   * |1   2|
+   * \__B__/
+   */
+  enum proximitySensorIdx : uint8_t {
+    PROX_WL = 2, PROX_LW = 2, PROX_WHEEL_LEFT = 2, PROX_LEFT_WHEEL = 2,
+    PROX_FL = 3, PROX_LF = 3, PROX_FRONT_LEFT = 3, PROX_LEFT_FRONT = 3,
+    PROX_FR = 0, PROX_RF = 0, PROX_FRONT_RIGHT = 0, PROX_RIGHT_FRONT = 0,
+    PROX_WR = 1, PROX_RW = 1, PROX_WHEEL_RIGHT = 1, PROX_RIGHT_WHEEL = 1,
+  };
+
+  /** \brief Index of the wheels
+   *
+   * Top view of the AMiRo wheels and their indices (F:Front, B:Back):
+   *   ____
+   * /| F |\
+   * |0   1|
+   * \|_B_|/
+   */
+  enum wheelIdx : uint8_t {
+    WHEEL_L = 0, WHEEL_LEFT = 0, LEFT_WHEEL = 0,
+    WHEEL_R = 1, WHEEL_RIGHT = 1, RIGHT_WHEEL = 1,
+  };
+}
+
+namespace PowerManagement {
+
+  /** \brief Index of the proximity sensors
+   *
+   * Top view of the AMiRo sensors and their indices:
+   *   _______
+   *  / 3 F 4 \
+   * |2       5|
+   * |1       6|
+   *  \_0_B_7_/
+   */
+  enum proximitySensorIdx : uint8_t {
+    PROX_BL = 0, PROX_BACK_LEFT = 0, PROX_SSW = 0, PROX_SOUTH_SOUTHWEST = 0,
+    PROX_LB = 1, PROX_LEFT_BACK = 1, PROX_WSW = 1, PROX_WEST_SOUTHWEST = 1,
+    PROX_LF = 2, PROX_LEFT_FRONT = 2, PROX_WNW = 2, PROX_WEST_NORTHWEST = 2,
+    PROX_FL = 3, PROX_FRONT_LEFT = 3, PROX_NNW = 3, PROX_NORTH_NORTHWEST = 3,
+    PROX_FR = 4, PROX_FRONT_RIGHT = 4, PROX_NNE = 4, PROX_NORTH_NORTHEAST = 4,
+    PROX_RF = 5, PROX_RIGHT_FRONT = 5, PROX_ENE = 5, PROX_EAST_NORTHEAST = 5,
+    PROX_RB = 6, PROX_RIGHT_BACK = 6, PROX_ESE = 6, PROX_EAST_SOUTHEAST = 6,
+    PROX_BR = 7, PROX_BACK_RIGHT = 7, PROX_SSE = 7, PROX_SOUTH_SOUTHEAST = 7
+  };
+
+  /** \brief Index of the batteries.
+   *
+   * The port names are printed on the PCB.
+   */
+  enum batteryPortIdx : uint8_t {
+    BAT_P7 = 0, BAT_A = 0,
+    BAT_P8 = 1, BAT_B = 1
+  };
+
+  /** \brief Index of the power monitors.
+   */
+  enum powerMonitorIdx : uint8_t {
+    INA_VDD = 0,
+    INA_VIO18 = 1,
+    INA_VIO33 = 2,
+    INA_VIO42 = 3,
+    INA_VIO50 = 4
+  };
+}
+
+}
+
+}
+
+#endif /* AMIRO_CONSTANTS_H_ */
diff --git a/include/amiro/ControllerAreaNetworkRx.h b/include/amiro/ControllerAreaNetworkRx.h
new file mode 100644
index 0000000..a2388e2
--- /dev/null
+++ b/include/amiro/ControllerAreaNetworkRx.h
@@ -0,0 +1,58 @@
+#ifndef AMIRO_CONTROLLER_AREA_NETWORK_RX_H_
+#define AMIRO_CONTROLLER_AREA_NETWORK_RX_H_
+
+#include <evtimer.h>
+#include <amiro/Color.h>
+#include <Types.h>  // ::kinematic
+
+#include <amiro/Constants.h>  // CAN::* macros
+
+namespace amiro {
+
+  class ControllerAreaNetworkRx : public chibios_rt::BaseStaticThread<128> {
+  public:
+    ControllerAreaNetworkRx(CANDriver *can, const uint8_t boardId);
+    virtual ~ControllerAreaNetworkRx() = 0;
+
+    uint16_t getProximityRingValue(int index);
+    uint16_t getProximityFloorValue(int index);
+    void getActualSpeed(types::kinematic &targetSpeed);
+    types::position getOdometry();
+    types::power_status& getPowerStatus();
+    uint8_t getRobotID();
+
+    void calibrateProximityRingValues();
+    void calibrateProximityFloorValues();
+
+    int rxCmdShell(CANRxFrame *frame);
+
+  protected:
+    virtual msg_t main();
+    virtual msg_t receiveMessage(CANRxFrame *frame);
+
+    int decodeBoardId(CANRxFrame *frame);
+    int decodeDeviceId(CANRxFrame *frame);
+    int decodeIndexId(CANRxFrame *frame);
+
+    int boardId;
+    uint16_t proximityRingValue[8];
+    int actualSpeed[2];
+    uint16_t proximityFloorValue[4];
+    types::position robotPosition;
+    types::power_status powerStatus;
+    uint8_t robotId;
+    chibios_rt::EvtListener rxFullCanEvtListener;
+    chibios_rt::EvtSource *rxFullCanEvtSource;
+
+  private:
+    msg_t receiveSensorVal(CANRxFrame *frame);
+
+    CANDriver *canDriver;
+    CANConfig canConfig;
+
+  };
+
+}
+
+#endif /* AMIRO_CONTROLLER_AREA_NETWORK_RX_H_ */
+
diff --git a/include/amiro/ControllerAreaNetworkTx.h b/include/amiro/ControllerAreaNetworkTx.h
new file mode 100644
index 0000000..bc576b4
--- /dev/null
+++ b/include/amiro/ControllerAreaNetworkTx.h
@@ -0,0 +1,106 @@
+#ifndef AMIRO_CONTROLLER_AREA_NETWORK_TX_H_
+#define AMIRO_CONTROLLER_AREA_NETWORK_TX_H_
+
+#include <evtimer.h>
+#include <amiro/Color.h>
+#include <Types.h>  // ::kinematic
+
+#include <amiro/Constants.h>  // CAN::* macros
+
+namespace amiro {
+
+  class ControllerAreaNetworkTx : public chibios_rt::BaseStaticThread<128> {
+  public:
+    ControllerAreaNetworkTx(CANDriver *can, const uint8_t boardId);
+    virtual ~ControllerAreaNetworkTx() = 0;
+
+    /**
+     * \brief Setting the brightness of the light ring
+     *
+     * @param brightness Brightness level 0 .. 200
+     */
+    void setLightBrightness(int brightness);
+
+    /**
+     * \brief Setting the color of the LED with the given index
+     *
+     * @param index Index of the LED
+     * @param color Color of the LED
+     */
+    void setLightColor(int index, Color color);
+
+    /**
+     * \brief Setting the desired speed in as kinematic struct
+     *
+     * @param targetSpeed Desired speed of the robot
+     */
+    void setTargetSpeed(types::kinematic &targetSpeed);
+
+    /**
+     * \brief Setting the desired speed in µ rounds per minute
+     *
+     * @param leftURpm Desired speed of the left wheel
+     * @param rightURpm Desired speed of the right wheel
+     */
+    void setTargetSpeed(int32_t leftURpm, int32_t rightURpm);
+
+    /**
+     * \brief Setting odometry for the Euler-Collatz algorithm on the DiWheelDrive board
+     *
+     * \notice Only bits 31..8 of robotPosition.x and robotPosition.y will be sent,
+     * i.e. values are divided by 1024 before being sent.
+     * Only bits 23..8 of orientation vector f_z be sent, i.e.
+     * f_z is devided by 1024 and clamped to int16_t.
+     * To avoid buffer overflow, only values between between [0, 2 * pi * 1e6] are valid.
+     * This is due to size constraints of CAN message frames.
+     *
+     * @param robotPosition Desired robot position.
+     */
+    void setOdometry(types::position robotPosition);
+
+    /**
+     * \brief Setting target position of current robot
+     *        local frame
+     *
+     * @param targetPosition Desired robot position
+     * @param targetTime Desired time reaching this position
+     */
+    void setTargetPosition(types::position &targetPosition, uint32_t targetTime);
+
+    /**
+     * \brief Setting correction ratios of the differential kinematic
+     *
+     * @param Ed Diameter ratio
+     * @param Eb Base width ratio
+     */
+    void setKinematicConstants(float Ed, float Eb);
+
+    void txQueryShell(uint8_t toBoardId, char *textdata, uint16_t size);
+    void txReplyShell(uint8_t toBoardId, char *textdata, uint16_t size);
+
+    void broadcastShutdown();
+
+  protected:
+    virtual msg_t main();
+    virtual msg_t updateSensorVal();
+    void transmitMessage(CANTxFrame *frame);
+    virtual void periodicBroadcast() = 0;
+
+    void encodeBoardId(CANTxFrame *frame, int board);
+    void encodeDeviceId(CANTxFrame *frame, int device);
+    void encodeIndexId(CANTxFrame *frame, int index);
+
+    int boardId;
+    chibios_rt::EvtListener eventTimerEvtListener;
+    chibios_rt::EvtSource *eventTimerEvtSource;
+
+  private:
+    EvTimer evtimer;
+    CANDriver *canDriver;
+    CANConfig canConfig;
+
+  };
+
+}
+
+#endif /* AMIRO_CONTROLLER_AREA_NETWORK_TX_H_ */
diff --git a/include/amiro/DistControl.h b/include/amiro/DistControl.h
new file mode 100644
index 0000000..97f37e4
--- /dev/null
+++ b/include/amiro/DistControl.h
@@ -0,0 +1,91 @@
+#ifndef AMIRO_MOTOR_DISTCONTROL_H_
+#define AMIRO_MOTOR_DISTCONTROL_H_
+
+#include <amiro/MotorIncrements.h>
+#include <amiro/MotorControl.h>
+#include <amiro/Constants.h>
+#include <Types.h>
+
+#include <chprintf.h>
+
+namespace amiro {
+
+  class DistControl : public chibios_rt::BaseStaticThread<256> {
+  public:
+    /**
+     * Constructor
+     *
+     * @param mc object for retrieving the motors
+     * @param mi object for retrieving the motor increments of the qei
+     */
+    DistControl(MotorControl* mc, MotorIncrements* mi);
+
+    /**
+     * Get the current target distance in um.
+     *
+     * @return target distance in um
+     */
+    int getCurrentTargetDist(void);
+
+    /**
+     * Get the current target angle in urad.
+     *
+     * @return target distance in urad
+     */
+    int getCurrentTargetAngle(void);
+
+    /**
+     * Sets the target position.
+     *
+     * @param distance Distance to drive in um
+     * @param angle Angle to turn in urad
+     * @param time Given time to drive in ms
+     */
+    void setTargetPosition(int32_t distance, int32_t angle, uint16_t time);
+
+    /**
+     * Gives if the distance controller is active.
+     *
+     * @return true if active, false if inactive
+     */
+    bool isActive(void);
+
+    /**
+     * Deactivates the controller.
+     * The motor velocities won't be set in any way!
+     */
+    void deactivateController(void);
+
+
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+    /**
+     * Calculates the velocities by using the error distance and error angle.
+     */
+    void calcVelocities(void);
+
+    MotorControl* motorControl;
+    MotorIncrements* motorIncrements;
+    bool controllerActive, drivingForward, turningLeft, newVelocities;
+    const uint32_t period;
+    int32_t increment[2];
+    float incrementDifference[2];
+    float actualDistance[2]; // m
+    int32_t fullDistance[2]; // um
+    int32_t realDistance; // um
+    int32_t realAngle; // urad
+    int32_t targetDistance; // um
+    int32_t targetAngle; // urad
+    int32_t errorDistance; // um
+    int32_t errorAngle; // urad
+    uint32_t restTime; // us
+    types::kinematic maxVelocity;
+    types::kinematic targetVelocity;
+    types::kinematic minVelocity;
+  };
+}
+
+#endif /* AMIRO_MOTOR_DISTCONTROL_H_ */
diff --git a/include/amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp b/include/amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp
new file mode 100644
index 0000000..20ed4e0
--- /dev/null
+++ b/include/amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp
@@ -0,0 +1,111 @@
+#ifndef AMIRO_FSIODIWHEELDRIVE_H_
+#define AMIRO_FSIODIWHEELDRIVE_H_
+
+/*! \brief memory layout for the eeproms
+ *
+ * This header defines the memory layout of the
+ * at24c01 on the CiWheelDrive board.
+ */
+
+#include <amiro/eeprom/at24.hpp>
+#include <amiro/FileSystemInputOutput/FileSystemInputOutputBase.hpp>
+
+// using namespace chibios_rt;
+
+namespace amiro {
+namespace fileSystemIo {
+
+class FSIODiWheelDrive : public FileSystemIoBase {
+  private:
+
+    /** \brief Layout for FSIODiWheelDrive with BSMV 1 and bsmv 1*/
+    struct DiWheelDrive_1_1 {
+      uint8_t  reserved_0x00_0x14[20];
+      uint16_t vcnl4020offset[4];       // U [12, 13, 15, 16]
+      uint8_t  generalPurpose[96];      // 1Cx_7Cx
+      uint8_t  reserved_0x7D_0x80[4];
+    };
+
+    /** \brief Layout for FSIODiWheelDrive with BSMV 1 and bsmv 2*/
+    struct DiWheelDrive_1_2 {
+      uint8_t  reserved_0x00_0x14[28];
+      float    Ed;
+      float    Eb;
+      uint8_t  generalPurpose[88];      // 24x_7Cx
+      uint8_t  reserved_0x7D_0x80[4];
+    };
+
+    /** \brief Layout for FSIODiWheelDrive with BSMV 1 and bsmv 3*/
+    struct DiWheelDrive_1_3 {
+      uint8_t  reserved_0x00_0x14[36];
+      float wheelfactor;
+      float igain;
+      int pgain;
+      float dgain;
+      uint8_t  generalPurpose[72];      // 24x_7Cx
+      uint8_t  reserved_0x7D_0x80[4];
+    };
+
+  public:
+    FSIODiWheelDrive(AT24 &at24c01, uint8_t BSMV, uint8_t bsmv, uint8_t HMV, uint8_t hmv)
+      : FileSystemIoBase(at24c01, BSMV, bsmv, HMV, hmv) {}
+
+    /**
+     * \brief Read the VCNL4020 offset from memory
+     * @param buffer Content to write in from the memory
+     * @param idx Index of the VCNL4020 sensor
+     * @return FSIO return types
+     */
+    msg_t getVcnl4020Offset (uint16_t *buffer, uint8_t idx);
+
+    /**
+     * \brief Write new data to the VCNL4020 offset
+     * @param buffer Content to write to the memory
+     * @param idx Index of the VCNL4020 sensor
+     * @return FSIO return types
+     */
+    msg_t setVcnl4020Offset (uint16_t buffer, uint8_t idx);
+
+    /**
+     * \brief Read the Ed ratio from memory
+     * @return FSIO return types
+     */
+    msg_t getEd (float *buffer);
+
+    /**
+     * \brief Write new data to the Ed ratio value
+     * @param buffer Content to write to the memory
+     * @return FSIO return types
+     */
+    msg_t setEd (float buffer);
+
+    /**
+     * \brief Read the Eb ratio from memory
+     * @return FSIO return types
+     */
+    msg_t getEb (float *buffer);
+
+    /**
+     * \brief Write new data to the Eb ratio value
+     * @param buffer Content to write to the memory
+     * @return FSIO return types
+     */
+    msg_t setEb (float buffer);
+
+    msg_t setwheelfactor (float buffer);
+    msg_t getWheelFactor (float *buffer);
+
+    msg_t setiGain (float buffer);
+    msg_t getiGain (float *buffer);
+
+    msg_t setdGain (float buffer);
+    msg_t getdGain (float *buffer);
+
+    msg_t setpGain (int buffer);
+    msg_t getpGain (int *buffer);
+};
+
+}
+}
+
+#endif /* AMIRO_FSIODIWHEELDRIVE_H_ */
diff --git a/include/amiro/FileSystemInputOutput/FSIOLightRing.hpp b/include/amiro/FileSystemInputOutput/FSIOLightRing.hpp
new file mode 100644
index 0000000..3be1edf
--- /dev/null
+++ b/include/amiro/FileSystemInputOutput/FSIOLightRing.hpp
@@ -0,0 +1,36 @@
+#ifndef AMIRO_FSIOLIGHTRING_H_
+#define AMIRO_FSIOLIGHTRING_H_
+
+/*! \brief memory layout for the eeproms
+ *
+ * This header defines the memory layout of the
+ * at24c01 on the LightRing board.
+ */
+
+#include <amiro/eeprom/at24.hpp>
+#include <amiro/FileSystemInputOutput/FileSystemInputOutputBase.hpp>
+
+namespace amiro {
+namespace fileSystemIo {
+
+class FSIOLightRing : public FileSystemIoBase {
+  private:
+
+    /** \brief Layout for FSIOLightRing with BSMV 1 and bsmv 1*/
+    struct LightRing_1_1 {
+      uint8_t  reserved_0x00_0x14[20];
+      uint8_t  generalPurpose[104];      // 15x_7Cx
+      uint8_t  reserved_0x7D_0x80[4];
+    };
+
+  public:
+    FSIOLightRing(AT24 &at24c01, uint8_t BSMV, uint8_t bsmv, uint8_t HMV, uint8_t hmv)
+      : FileSystemIoBase(at24c01, BSMV, bsmv, HMV, hmv) {
+
+    }
+};
+
+}
+}
+
+#endif /* AMIRO_FSIOLIGHTRING_H_ */
diff --git a/include/amiro/FileSystemInputOutput/FSIOPowerManagement.hpp b/include/amiro/FileSystemInputOutput/FSIOPowerManagement.hpp
new file mode 100644
index 0000000..440079a
--- /dev/null
+++ b/include/amiro/FileSystemInputOutput/FSIOPowerManagement.hpp
@@ -0,0 +1,53 @@
+#ifndef AMIRO_FSIOPOWERMANAGEMENT_H_
+#define AMIRO_FSIOPOWERMANAGEMENT_H_
+
+/*! \brief memory layout for the eeproms
+ *
+ * This header defines the memory layout of the
+ * at24c01 on the PowerManagement board.
+ */
+
+#include <amiro/eeprom/at24.hpp>
+#include <amiro/FileSystemInputOutput/FileSystemInputOutputBase.hpp>
+
+using namespace chibios_rt;
+
+namespace amiro {
+namespace fileSystemIo {
+
+class FSIOPowerManagement : public FileSystemIoBase {
+  private:
+
+    /** \brief Layout for FSIOPowerManagement with BSMV 1 and bsmv 1*/
+    struct PowerManagement_1_1 {
+      uint8_t  reserved_0x00_0x14[20];
+      uint16_t vcnl4020offset[8];        // U1.Proximity [11, 21, 31, 41, 12, 22, 32, 42]
+      uint8_t  generalPurpose[88];       // 24x_7Cx
+      uint8_t  reserved_0x7D_0x80[4];
+    };
+
+  public:
+    FSIOPowerManagement(AT24 &at24c01, uint8_t BSMV, uint8_t bsmv, uint8_t HMV, uint8_t hmv)
+      : FileSystemIoBase(at24c01, BSMV, bsmv, HMV, hmv) {}
+
+    /**
+     * \brief Read the VCNL4020 offset from memory
+     * @param buffer Content to write in from the memory
+     * @param idx Index of the VCNL4020 sensor
+     * @return FSIO return types
+     */
+    msg_t getVcnl4020Offset (uint16_t *buffer, uint8_t idx);
+
+    /**
+     * \brief Write new data to the VCNL4020 offset
+     * @param buffer Content to write to the memory
+     * @param idx Index of the VCNL4020 sensor
+     * @return FSIO return types
+     */
+    msg_t setVcnl4020Offset (uint16_t buffer, uint8_t idx);
+};
+
+}
+}
+
+#endif /* AMIRO_FSIOPOWERMANAGEMENT_H_ */
diff --git a/include/amiro/FileSystemInputOutput/FileSystemInputOutputBase.hpp b/include/amiro/FileSystemInputOutput/FileSystemInputOutputBase.hpp
new file mode 100644
index 0000000..a156228
--- /dev/null
+++ b/include/amiro/FileSystemInputOutput/FileSystemInputOutputBase.hpp
@@ -0,0 +1,125 @@
+#ifndef AMIRO_FILESYSTEMIO_H_
+#define AMIRO_FILESYSTEMIO_H_
+
+/*! \brief memory layouts for the eeproms
+ *
+ * This header defines the memory layout.
+ * Every memory layout starts with a preamble
+ * of 3 byte. The first one is a magic byte
+ * for checking purpose. The second one declares
+ * the byte structure major version (BSMV) and the third
+ * the byte structure minor version (bsmv) of the subsequently
+ * bytes in the memory. With different major versions, the
+ * layout of the memory may totaly change, so one should
+ * not read from such a different layout if the version
+ * differs from the expected one. With greater minor
+ * versions the old memory layout may not change, but
+ * new elements emerge.
+ */
+
+#include <amiro/eeprom/at24.hpp>
+
+namespace amiro {
+namespace fileSystemIo {
+
+class FileSystemIoBase {
+
+  public:
+    AT24 &at24c01;
+    const uint8_t BSMV;
+    const uint8_t bsmv;
+    const uint8_t HMV;
+    const uint8_t hmv;
+    static constexpr uint8_t magicByteValue() { return 0xAAu;}
+
+    // TODO implement CRC check at 0x7D to 0x80
+    /** \brief Global memory structure */
+    struct preamble_1_1 {
+      uint8_t magicByte;
+      uint8_t byteStructureMajorVersion;
+      uint8_t byteStructureMinorVersion;
+      uint8_t hardwareMajorVersion;
+      uint8_t hardwareMinorVersion;
+      uint8_t reserved_0x05_0x14[15];
+    };
+
+    struct preamble_1_2 {
+      uint8_t reserved_0x00_0x04[5];
+      uint8_t boardId;
+      uint8_t reserved_0x06_0x14[14];
+    };
+
+    /** \brief FSIO return types */
+    enum{
+      OK                    = 0x00u,
+      NOT_IMPLEMENTED       = 0x01u,
+      WRONG_BSMV            = 0x02u,
+      WRONG_bsmv            = 0x04u,
+      IO_ERROR              = 0x08u,
+      WRONG_MAGICBYTE_VALUE = 0x10u,
+      WRONG_INDEX           = 0x20u,
+      WRONG_HMV             = 0x40u,
+      WRONG_hmv             = 0x80u,
+    };
+
+  public:
+
+    FileSystemIoBase(AT24 &at24c01, uint8_t BSMV, uint8_t bsmv, uint8_t HMV, uint8_t hmv)
+      : at24c01(at24c01),
+        BSMV(BSMV),
+        bsmv(bsmv),
+        HMV(HMV),
+        hmv(hmv) {
+     }
+
+    /**
+     * \brief Checks the values stored in the memory and compares the set version numbers and the version numbers found.
+     *        If the magic byte is incorrect WRONG_MAGICBYTE_VALUE is returned.
+     *        If the BMSVs are not equal WRONG_BSMV is returned.
+     *        If the bmsv found in the memory is smaller, the missing elements are initialized with default values and WRONG_bsmv is returned.
+     *        If the bmsv found in the memory is equal or greater OK is returned.
+     *        If the HMVs are not equal WRONG_HMV is returned.
+     *        If the mmvs are not equal WRONG_hmv is returned.
+     * @return FSIO return types
+     */
+    msg_t init();
+
+    /**
+     * \brief Check the memory structure
+     * @return FSIO return types
+     */
+    msg_t getCheck();
+
+    /**
+     * @brief Initializes the memory of the given preamble with default values.
+     * @param[in] BSMV The Byte Structure Major Version to initialize.
+     * @param[in] bsvm The Byte Structure Minor Version to initialize.
+     * @param[in] strict Specifies whether all (true) or only new (false) values are written to the memory.
+     * @return FSIO return types
+     */
+    msg_t initPreamble(const uint8_t BSMV, const uint8_t bsmv, const bool strict = true);
+
+    /**
+     * \brief Initializes the memory with the given structure
+     * @return FSIO return types
+     */
+    msg_t resetMemory();
+
+    /**
+     * \brief Writes the board id to the eeprom
+     * @return FSIO return types
+     */
+    msg_t setBoardId (uint8_t buffer);
+
+    /**
+     * \brief Reads the board id from the eeprom
+     * @return FSIO return types
+     */
+    msg_t getBoardId (uint8_t *buffer);
+
+};
+
+}
+}
+
+#endif /* AMIRO_FILESYSTEMIO_H_ */
diff --git a/include/amiro/Lidar.h b/include/amiro/Lidar.h
new file mode 100644
index 0000000..c11895f
--- /dev/null
+++ b/include/amiro/Lidar.h
@@ -0,0 +1,261 @@
+#ifndef LIDAR_H_
+#define LIDAR_H_
+
+// Top view
+/*                                                 __________
+//                                               /     |     \
+//                                              /      O      \
+//                                             /  H O K U Y O  \
+//                                             |                |
+//                                             |                |
+//                                              \    U R G     /
+// scannedData[NUMBER_OF_STEPS] = END_STEP       \/          \/   STARTING_STEP = scannedData[0]
+//                                                \__________/
+*/
+
+#include <string.h> // memcpy
+#include <stdlib.h>
+
+#include <amiro/bus/spi/HWSPIDriver.hpp>
+#include <amiro/Constants.h> // CAN::LIGHT_RING_ID
+#include <ch.hpp>
+#include <hal.h>
+#include <chprintf.h>
+#include <evtimer.h>
+
+// DO CONFIGURATION HERE : START
+// Important: SD_SPEED_PREFIX + SD_SPEED has to have 6 characters: e.g. 019200 or 115200
+#define SD_SPEED_PREFIX   // Prefix < 0     | 0     |        |        |        |        >
+#define SD_SPEED 250000 // Baudrate < 19200 | 57600 | 115200 | 250000 | 500000 | 750000 >
+#define UPDATE_LIDAR_PERIOD_MSEC MS2ST(1000)  // Periodic sensor value fetch in ms
+// Scan acquire message : START
+// Uncomment the specific character decoding to make it available
+#define USE_2CHAR_CODE
+// #define USE_3CHAR_CODE  // TODO Not implemented yet
+#define DATA_ACQ_2CHAR_CODE "MS"
+#define DATA_ACQ_3CHAR_CODE "MD"
+// Important: STARTING_STEP_PREFIX + STARTING_STEP has to have 4 characters: e.g. 0005 0044 0320
+#define STARTING_STEP_PREFIX 00
+#define STARTING_STEP 44
+// Important: END_STEP_PREFIX + END_STEP has to have 4 characters: e.g. 0005 0725 0320
+#define END_STEP_PREFIX 0
+#define END_STEP 725
+#define CLUSTER_COUNT 00
+#define SCAN_INTERVALL 0
+#define NUMBER_OF_INTERVALL 01
+// Scan acquire message : END
+// DO CONFIGURATION HERE : END
+
+// Internal calculations and definitions
+#if defined(USE_2CHAR_CODE) && defined(USE_3CHAR_CODE)
+#  error("USE_2CHAR_CODE and USE_3CHAR_CODE are defined. Only one of them is allowed!")
+#elif !defined(USE_2CHAR_CODE) && !defined(USE_3CHAR_CODE)
+#  error("USE_2CHAR_CODE and USE_3CHAR_CODE are not defined. Define one of them!")
+#elif defined(USE_2CHAR_CODE)
+#  define DATA_ACQ_CODE DATA_ACQ_2CHAR_CODE
+#  define NUMBER_OF_CHARACTERS (END_STEP - STARTING_STEP + 1) * 2
+#elif defined(USE_3CHAR_CODE)
+#  define DATA_ACQ_CODE DATA_ACQ_3CHAR_CODE
+#  define NUMBER_OF_CHARACTERS (END_STEP - STARTING_STEP + 1) * 3
+#endif
+#define NUMBER_OF_STEPS (END_STEP - STARTING_STEP + 1)
+
+#define STR_HELPER(x) #x
+#define STR(x) STR_HELPER(x)
+
+#define LF "\n"
+
+namespace amiro {
+
+  class Lidar : public chibios_rt::BaseStaticThread<256> {
+  public:
+    /**
+    * Possible setups for the LIDAR: POWER_ONLY (The LIDAR will be powererd, but no serial IO is performed [Need for USB IO]); SERIAL_IO (Power and serial communication via lidar driver)
+    */
+    enum SETUP {POWER_ONLY, SERIAL_IO};
+
+    /**
+    * Writes the last scan into the given array
+    *
+    * @param scanData Array of size NUMBER_OF_STEPS which will be override
+    *                 with the scanned data
+    *
+    * @return True if scanData holds the current data, False if no data have been copied
+    */
+    bool_t getScan(uint16_t (&scanData)[NUMBER_OF_STEPS]);
+
+    /**
+    * Returns the value NUMBER_OF_STEPS which is the amount of datapoints per scan
+    *
+    * @return Amount of data in one scan
+    */
+    uint16_t getNumberOfValues();
+
+  protected:
+    virtual msg_t main();
+
+    /**
+    * Do a scan and update the datapoints in scanData
+    */
+    msg_t updateSensorVal();
+
+    /**
+    * Print received data until "\n\n" was received
+    */
+    void printData();
+
+    /**
+    * Receive data until "\n\n" and compare it to the given string
+    * Note: If the command did not work, use printData() instead
+    *       and compare the reply with chapter "9. Response to Invalid Commands"
+    *
+    * @param compareString String to compare with
+    *
+    * @return True if string was equal, false if not
+    */
+    bool_t checkDataString(const char compareString[]);
+    /**
+    *  Send the command which let the LIDAR send its details
+    */
+    void printDetails();
+
+    /**
+    *  Send the command which let the LIDAR send its specifications
+    */
+    void printSpecification();
+
+    /**
+    *  Send the command which let the LIDAR send its information
+    */
+    void printInformation();
+
+    /**
+    *  Tells you, if the sensor is ready for work
+    *
+    *  @return True for operability, False if not
+    */
+    inline bool_t getIsReady();
+
+    /**
+    *  Do the two character encoding after the manual
+    *
+    *  @param char1 High character
+    *  @param char2 Low character
+    *
+    *  @return Encoded data
+    */
+    inline uint16_t twoCharacterEncoding(uint8_t &char1, uint8_t &char2);
+
+    /**
+    *  Blocking function which writes the received character to the given variable
+    *
+    *  @param data Container for the variable
+    *  @param timeoutMs Timeout in millisecond
+    *
+    *  @return True if data was received, False if timed out (NOTE timeoutMs should by much smaller than UPDATE_LIDAR_PERIOD_MSEC)
+    */
+    bool_t getData(uint8_t &data, uint32_t timeoutMs);
+
+    /**
+    * Container for the scanned data
+    *
+    * @brief The "+1" character is for holding the last sum which is transmitted
+    *        by the LIDAR. It is not used, but it makes the algorithm much easier
+    *        we we do not care about it
+    */
+    static uint8_t scannedData[NUMBER_OF_CHARACTERS + 1];
+
+    /**
+    *  Flushes the input queue of the SD2 serial device
+    */
+    inline void flushSD2InputQueue();
+
+    /**
+    * Possible states for the receiver state machine
+    */
+    enum STATE {SEND_SCAN_CMD, DATA_VALID_CHECK, DATA_START_CHECK, DATA_RECORD, DATA_DECODE, DATA_SHOW, FAIL, FINISH};
+
+    /**
+    * Current state of the machine
+    */
+    STATE step = SEND_SCAN_CMD;
+
+    /**
+    * This is the status which comes after the command echo "\n99b\n"
+    */
+    const uint8_t validScanTag[5] =  {10,57,57,98,10};
+
+    /**
+    * Index for the received status
+    */
+    uint8_t checkStatusIdx = 0;
+
+    /**
+    * Index for the received status
+    */
+
+    /**
+    * Hold the board id
+    */
+    uint8_t boardId;
+
+    /**
+    * Hold the setup configuration for the lidar
+    */
+    SETUP setup;
+
+    /**
+    * Holds the current input
+    */
+    uint8_t newInput = 0xFF;
+
+    /**
+    * Holds the last input
+    */
+    uint8_t lastInput = 0xFF;
+
+    /**
+    * Counter for the received characters of the data points
+    */
+    uint16_t dataIdx = 0;
+
+    /**
+    * Temporary variable which hold the amount of receved bytes
+    */
+    uint32_t dataCounter = 0;
+
+    /**
+    * Holds the information, if scannedData holds the last sensor scan
+    */
+    bool_t isReady = false;
+
+    /**
+    * Holds the information, if the datachunk was not transmitted completely
+    */
+    bool_t isFail = false;
+
+  private:
+
+    EvTimer evtimer;
+
+  public:
+  /**
+    * Constructor
+    *
+    * @param boardId Board ID which can be found in ControllerAreaNetwork.h.
+    *                It is used to setup the LIDAR for exactly this board
+    */
+    Lidar(const uint8_t boardId, Lidar::SETUP setup);
+
+    /**
+    * Constructor
+    *
+    * Disable the LIDAR
+    * TODO Why "virtual ~Lidar() = 0;"
+    */
+    ~Lidar();
+
+  };
+
+}
+#endif /* LIDAR_H_ */
diff --git a/include/amiro/MotorControl.h b/include/amiro/MotorControl.h
new file mode 100644
index 0000000..0f809b9
--- /dev/null
+++ b/include/amiro/MotorControl.h
@@ -0,0 +1,273 @@
+#ifndef AMIRO_MOTOR_CONTROL_H_
+#define AMIRO_MOTOR_CONTROL_H_
+
+#include <amiro/MotorIncrements.h>
+#include <amiro/Constants.h>
+#include <Types.h>
+#include <amiro/FileSystemInputOutput/FSIODiWheelDrive.hpp>
+
+namespace amiro {
+
+  class MotorControl : public chibios_rt::BaseStaticThread<512> {
+  public:
+    /**
+     * Constructor
+     *
+     * @param pwm pulse width modulation driver (pwmd)
+     *            Can be any free PWMDx in 'ChibiOS-RT/os/hal/platforms/STM32/TIMv1/pwm_lld.h'
+     * @param mi object for retrieving the motor increments of the qei
+     * @param port GPIO port for motor control (should be the macro 'GPIOB')
+     * @param pad GPIO command for motor control (should be the macro 'GPIOB_POWER_EN' for enable)
+     * @param memory Memory interface to load/store parameters
+     */
+    MotorControl(PWMDriver* pwm, MotorIncrements* mi, GPIO_TypeDef* port, int pad, fileSystemIo::FSIODiWheelDrive *memory);
+
+    /**
+     * Get the current speed of the left wheel in rounds/min
+     *
+     * @return speed of left wheel in rounds/min
+     */
+    int getCurrentRPMLeft();
+
+    /**
+     * Get the current speed of the right wheel in rounds/min
+     *
+     * @return speed of right wheel in rounds/min
+     */
+    int getCurrentRPMRight();
+
+    chibios_rt::EvtSource* getEventSource();
+
+    /**
+     * Sets the target velocity
+     *
+     * @param targetVelocity Kartesian kinematic vector
+     */
+    void setTargetSpeed(const types::kinematic &targetVelocity);
+
+    /**
+     * Sets the target velocity in µ rounds per minute for every wheel
+     *
+     * @param targetURpmLeft Rounds per minute in µ1/min of the left wheel
+     * @param targetURpmLeft Rounds per minute in µ1/min of the right wheel
+     */
+    void setTargetRPM(int32_t targetURpmLeft, int32_t targetURpmRight);
+
+    /**
+     * Get the current velocitiy as a kinematic struct
+     *
+     * @return Current velocity
+     */
+    types::kinematic getCurrentVelocity();
+
+    /**
+     * Sets the correction factor for the wheel diameter.
+     * The factor Ed is the ratio of the wheel diameters
+     * <Ed = wheelDiameterRight / wheelDiameterLeft> as
+     * introduced in eq. 3 by J. Borenstein (Correction of
+     * Systematic Odometry Errors in Mobile Robots). This
+     * function calculates then the correction factors for every
+     * wheel by eq. 17a and 17b.
+     *
+     * @param Ed Wheel diameter ratio
+     * @param storeEbToMemory Do override Ed in the memory with the given value
+     * @return Return value of the memory interface
+     */
+    msg_t setWheelDiameterCorrectionFactor(float Ed = 1.0f, bool_t storeEdToMemory = false);
+
+    /**
+     * Sets the correction factor for the wheel base width.
+     * The factor Eb is the ratio of the actual and nominal
+     * base width <Eb = bActual / bNominal> as introduced
+     * in eq. 3 by J. Borenstein (Correction of
+     * Systematic Odometry Errors in Mobile Robots). This
+     * function calculates then the actual base width of the
+     * robot using eq. 4.
+     *
+     * @param Eb Base width ratio
+     * @param storeEbToMemory Do override Eb in the memory with the given value
+     * @return Return value of the memory interface
+     */
+    msg_t setActualWheelBaseDistance(float Eb = 1.0f, bool_t storeEbToMemory = false);
+
+    /**
+     * Calculate the current increments of both wheels and
+     * update oldIncrement and incrementsDifference,
+     * which is the corrected difference between the current increments
+     * and the old ones.
+     * The corrected difference is the original difference between the old
+     * increments and new increments, multiplied with the wheelDiameterCorrectionFactor.
+     * The incremennt source is given by motorIncrements.
+     *
+     * @param motorIncrements Increment source
+     * @param oldIncrement Old increments, which are updated
+     * @param incrementDifference Corrected difference between old and current increments
+     * @return Return value of the memory interface
+     */
+    static void updateIncrements(MotorIncrements* motorIncrements, int32_t (&oldIncrement)[2], float (&incrementDifference)[2]);
+
+    /**
+     * Calculate the current speed of both wheels and
+     * updates actualSpeed.
+     *
+     * @param incrementDifference Difference between old and current increments
+     * @param actualSpeed Actual speed of both wheels
+     * @param Update period
+     */
+    static void updateSpeed(const float (&incrementDifference)[2], int32_t (&actualSpeed)[2], const uint32_t period);
+
+    /**
+     * Calculate the current driven distance of both wheels and
+     * updates actualDistance.
+     *
+     * @param incrementDifference Difference between old and current increments
+     * @param actualDistance Actual distance driven
+     */
+    static void updateDistance(const float (&incrementDifference)[2], float (&actualDistance)[2]);
+
+    /**
+     * Prints Control Gains
+     */
+    void printGains();
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+
+    /**
+     * Calculate the velocitiy in the robot frame
+     * and saves it to this->currentVelocity
+     */
+    void calcVelocity();
+
+    /**
+     * PID Controller that works directly on the forward velocity v of the robots center and its
+     * angular velocity w around its z axis. The methods setLeftWheelSpeed() and setRightWheelSpeed()
+     * are used to set the final pwm values.
+     */
+    void PIDController();
+
+    /**
+     * Deletes the oldest entry in lastVelocitiesV[] and lastVelocitiesW[], pushes the other values up and saves the currentVelocity.x(w_z) in the last entry.
+     */
+    void updateDerivativeLastVelocities();
+
+
+    ////////////////////////////////////////////////
+    /////////////////Calibration////////////////////
+    ////////////////////////////////////////////////
+    /**
+     * Finds a prefactor for the stronger motor to match its power level to the weaker motor.
+     */
+    void calibrate();
+
+    /**
+     * Finds the P, I and D gains for the PID Controller using the Nichols-Ziegler Method.
+     */
+    void calibrateZiegler();
+
+    /**
+     * Counts the number of sign changes from the last 30 velocities.
+     */
+    int getNumberofSignChanges();
+
+    /**
+     * Update the past lastVelocitiesV array.
+     */
+    void updatePastVelocities();
+
+
+    /**
+     * Sets wheel speed according to the output of the PID Controller
+     */
+    void setLeftWheelSpeed(int diffv, int diffw);
+    /**
+     * Sets wheel speed according to the output of the PID Controller
+     */
+    void setRightWheelSpeed(int diffv, int diffw);
+
+    int getLeftWheelSpeed();
+    int getRightWheelSpeed();
+
+
+    /**
+     * Write the duty cicle from this->pwmPercentage
+     * to the PWM driver
+     */
+    void writePulseWidthModulation();
+
+    /**
+     * Control logic to save space in main loop
+     */
+    void controllerAndCalibrationLogic();
+
+
+
+    PWMDriver* pwmDriver;
+    PWMConfig pwmConfig;
+    MotorIncrements* motorIncrements;
+    GPIO_TypeDef *powerEnablePort;
+    const int powerEnablePad;
+    chibios_rt::EvtSource eventSource;
+    //const uint32_t period;
+    uint32_t period;
+    fileSystemIo::FSIODiWheelDrive *memory;
+    int32_t actualSpeed[2];
+    float actualDistance[2];
+    float errorSum[2];
+    int32_t increment[2];
+    float incrementDifference[2];
+    int pwmPercentage[2];
+    types::kinematic targetVelocity;
+    types::kinematic currentVelocity;
+    types::kinematic lastVelocity;
+    float errorSumDiff;
+    bool newTargetVelocities;
+    int delay = 0;
+    float Ed;
+    float Eb;
+
+    int pGain = 1000;
+
+    float iGain = 0.08;
+    int antiWindupV = 7100000;
+    int antiWindupW = 200000000;
+    int accumulatedErrorV =0; // accumulated velocity error
+    int accumulatedErrorW =0; // accumulated omega error
+
+    float dGain =0.01;
+    int lastVelocitiesV[6];
+    int lastVelocitiesW[6];
+
+    //motor calibration variables
+    int numberOfLastVelocitiesV = 30;
+    int lastVelocitiesVBig[30];
+    int rightWValues[3];
+    int leftWValues[3];
+    float motorCalibrationFactor =1.0;
+    //ziegler calibration variables
+    int zieglerHelp= 1;
+    int zieglerHelp2 = 0;
+    bool ziegler= true;
+    bool ziegler2 = true;
+    float zieglerPeriod;
+    int zieglerCalibrationTime = 0;
+    int wheelCalibrationTime = 0;
+    bool startedZieglerCalibration = false;
+    bool startedWheelCalibration = false;
+    bool motorCalibration = true;
+
+
+
+
+  public:
+    bool isCalibrating = false;
+    static float wheelDiameterCorrectionFactor[2];
+    static float actualWheelBaseDistanceSI;
+
+
+  };
+}
+
+#endif /* AMIRO_MOTOR_CONTROL_H_ */
diff --git a/include/amiro/MotorIncrements.h b/include/amiro/MotorIncrements.h
new file mode 100644
index 0000000..ef73f1c
--- /dev/null
+++ b/include/amiro/MotorIncrements.h
@@ -0,0 +1,45 @@
+#ifndef AMIRO_MOTOR_INCREMENTS_H_
+#define AMIRO_MOTOR_INCREMENTS_H_
+
+namespace amiro {
+
+  class MotorIncrements {
+
+    public:
+    /**
+     * Constructor
+     * This only creates the config. To start the qei driver, one needs to call
+     * qeiInit() after halInit() in the main process first. Then run this->start().
+     *
+     * @param qei0 quadrature encoder for left wheel from 'amiro-os/hal/include/qei.h'
+     * @param qei0 quadrature encoder for right wheel from 'amiro-os/hal/include/qei.h'
+     */
+    MotorIncrements(QEIDriver* qei0, QEIDriver* qei1);
+
+    /**
+     * Get the encoder position
+     *
+     * @return position of the encode
+     */
+    int qeiGetPosition(int idxQei);
+
+    /**
+     * Get the range of the qei config
+     *
+     * @return value of the range of the qei
+     */
+    int getQeiConfigRange();
+
+    /**
+     * Starts the qei driver (First run qeiInit() in the main process)
+     */
+    void start();
+
+  private:
+    QEIDriver* qeiDriver[2];
+    QEIConfig qeiConfig;
+  };
+
+}
+
+#endif /* AMIRO_MOTOR_INCREMENTS_H_ */
diff --git a/include/amiro/Odometry.h b/include/amiro/Odometry.h
new file mode 100644
index 0000000..c80cd50
--- /dev/null
+++ b/include/amiro/Odometry.h
@@ -0,0 +1,95 @@
+#ifndef AMIRO_ODOMETRY_H_
+#define AMIRO_ODOMETRY_H_
+
+#include <amiro/MotorControl.h>
+
+#include <Types.h> // types::position
+
+namespace amiro {
+
+  class Odometry : public chibios_rt::BaseStaticThread<512> {
+  public:
+    /**
+     * Constructor
+     *
+     * @param mi object for retrieving the motor increments of the qei
+     */
+    Odometry(MotorIncrements* mi);
+
+    /**
+     * Set the position of the roboter
+     *
+     * @param pX Position x in meter
+     * @param pY Position y in meter
+     * @param pPhi Orientation phi in radiant
+     */
+    void setPosition(float pX, float pY, float pPhi);
+
+    /**
+     * Reset the position to [0,0,0]
+     */
+    void resetPosition();
+
+    /**
+     * Set the position of the roboter
+     *
+     * @param *Cp
+     */
+    void setError(float* Cp);
+
+    /**
+     * Reset the error to [0,0,0;0,0,0;0,0,0]
+     */
+    void resetError();
+
+    /**
+     * Updates the wheelBaseDistance variable,
+     * because it may change after an calibration.
+     */
+    void updateWheelBaseDistance();
+
+    /**
+     * Get the current position
+     * @return position
+     */
+    types::position getPosition();
+
+    chibios_rt::EvtSource* getEventSource();
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+
+    /**
+     * Calculate the actual distance of both wheels
+     */
+    void updateDistance();
+
+    /**
+     * This is th PID controller for controling
+     * the speed. It gets the new duty cicle for the pwm
+     * and saves it to this->pwmPercentage to reach the
+     * target speed this->targetSpeed
+     */
+    void updateOdometry();
+
+    MotorIncrements* motorIncrements;
+    chibios_rt::EvtSource eventSource;
+    const unsigned int period;
+    int incrementsPerRevolution;
+    int updatesPerMinute;
+    float wheelCircumference; // wheel circumference in meter
+    float wheelBaseDistanceSI; // Base width in meter
+    float distance[2]; // Moved distance per wheel in meter left:0 right:1
+    float wheelError[2]; // error for left:0 and right:1 wheel
+    float pX, pY; // Position in meter
+    float pPhi; // Orientation in Rad
+    float Cp3x3[9];  // Covariance (position error) (3x3 matrix)
+    int32_t increment[2]; // Absolute number of difference since last update
+    float incrementDifference[2]; // Difference between old and current absolute increments
+  };
+
+}
+
+#endif /* AMIRO_ODOMETRY_ */
diff --git a/include/amiro/accel/lis331dlh.hpp b/include/amiro/accel/lis331dlh.hpp
new file mode 100644
index 0000000..2604d34
--- /dev/null
+++ b/include/amiro/accel/lis331dlh.hpp
@@ -0,0 +1,275 @@
+#ifndef LIS331DLH_HPP_
+#define LIS331DLH_HPP_
+
+#include <ch.hpp>
+
+namespace amiro {
+
+  class HWSPIDriver;
+
+  class LIS331DLH : public chibios_rt::BaseStaticThread<256> {
+
+  public:
+
+  struct InterruptConfig {
+
+    uint8_t config;
+    uint8_t ths;
+    uint8_t duration;
+
+  } __attribute__((packed));
+
+  struct LIS331DLHConfig {
+    uint8_t         ctrl1;
+    uint8_t         ctrl2;
+    uint8_t         ctrl3;
+    uint8_t         ctrl4;
+    uint8_t         ctrl5;
+    InterruptConfig int1_cfg;
+    InterruptConfig int2_cfg;
+  };
+
+  enum {
+    AOI_OR_INT  = 0x00u,
+    AOI_6MOVE   = 0x40u,
+    AOI_AND_INT = 0x80u,
+    AOI_6POS    = 0xC0u,
+    ZHIE        = 0x20u,
+    ZLIE        = 0x10u,
+    YHIE        = 0x08u,
+    YLIE        = 0x04u,
+    XHIE        = 0x02u,
+    XLIE        = 0x01u,
+  };
+
+  enum {
+    IA          = 0x40u,
+    ZH          = 0x20u,
+    ZL          = 0x10u,
+    YH          = 0x08u,
+    YL          = 0x04u,
+    XH          = 0x02u,
+    XL          = 0x01u,
+  };
+
+  enum {
+    THS_MASK      = 0x7Fu,
+  };
+
+  enum {
+    DURATION_MASK = 0x7Fu,
+  };
+
+  enum {
+    PM_OFF          = 0x00u,
+    PM_ODR          = 0x20u,
+    PM_0_5_HZ       = 0x40u,
+    PM_1_HZ         = 0x60u,
+    PM_2_HZ         = 0x80u,
+    PM_5_HZ         = 0xA0u,
+    PM_10_HZ        = 0xC0u,
+    DR_50HZ_37LP    = 0x00u,
+    DR_100HZ_74LP   = 0x08u,
+    DR_400HZ_292LP  = 0x10u,
+    DR_1000HZ_780LP = 0x18u,
+    ZEN             = 0x04u,
+    YEN             = 0x02u,
+    XEN             = 0x01u,
+  };
+
+  enum {
+    BOOT_REBOOT  = 0x80u,
+    HPM_NORMAL0  = 0x00u,
+    HPM_REF      = 0x20u,
+    HPM_NORMAL1  = 0x40u,
+    FDS_FILTER   = 0x10u,
+    HPEN_INT2    = 0x08u,
+    HPEN_INT1    = 0x04u,
+    HPCF_8       = 0x00u,
+    HPCF_16      = 0x01u,
+    HPCF_32      = 0x02u,
+    HPCF_64      = 0x03u,
+  };
+
+  enum {
+    INT_HIGH     = 0x00u,
+    INT_LOW      = 0x80u,
+    PUSH_PULL    = 0x00u,
+    OPEN_DRAIN   = 0x40u,
+    LIR2_LATCH   = 0x20u,
+    I2_CFG_I2    = 0x00u,
+    I2_CFG_I1_I2 = 0x08u,
+    I2_CFG_DRY   = 0x10u,
+    I2_CFG_BOOT  = 0x18u,
+    LIR1_LATCH   = 0x04u,
+    I1_CFG_I1    = 0x00u,
+    I1_CFG_I1_I2 = 0x01u,
+    I1_CFG_DRY   = 0x02u,
+    I1_CFG_BOOT  = 0x03u,
+  };
+
+  enum {
+    BDU_CONT = 0x00u,
+    BDU_STOP = 0x80u,
+    BLE_LE   = 0x00u,
+    BLE_BE   = 0x40u,
+    FS_2G    = 0x00u,
+    FS_4G    = 0x10u,
+    FS_8G    = 0x30u,
+    STSIGN_POS = 0x00u,
+    STSIGN_NEG = 0x08u,
+    ST_DISABLE = 0x00u,
+    ST_ENABLE  = 0x02u,
+    SIM_4WI    = 0x00u,
+    SIM_3WI    = 0x01u,
+  };
+
+  enum {
+    SLEEP_TO_WAKE_OFF = 0x00u,
+    SLEEP_TO_WAKE_ON  = 0x03u,
+  };
+
+  /**
+   * Return types of getCheck()
+   */
+  enum {
+    CHECK_OK   = 0x00,
+    CHECK_FAIL = 0x01,
+  };
+
+  /**
+  * Top view of the AMiRo with charger in the back (F:Front, B:Back).
+  * Z is pointing into the ground (Apply right-hand-rule):
+  *    ___________
+  *   /    F      \
+  *  /     X       \
+  * |      |        |
+  * |      Z---Y    |
+  * |               |
+  *  \             /
+  *   \____B______/
+  */
+  enum {
+    AXIS_X   = 0x00u,
+    AXIS_Y   = 0x01u,
+    AXIS_Z   = 0x02u,
+  };
+
+  //private:
+
+  struct registers {
+    uint8_t  reserved_0x00_0x0E[0x0Fu];
+    uint8_t  who_am_i;
+    uint8_t  reserved_0x10_0x1F[0x10u];
+    uint8_t  ctrl_reg1;
+    uint8_t  ctrl_reg2;
+    uint8_t  ctrl_reg3;
+    uint8_t  ctrl_reg4;
+    uint8_t  ctrl_reg5;
+    uint8_t  hp_filter_reset;
+    uint8_t  reference;
+    uint8_t  status_reg;
+    uint16_t out_x; /* LE */
+    uint16_t out_y; /* LE */
+    uint16_t out_z; /* LE */
+    uint8_t  reserved_0x2E_0x2F[0x02u];
+    uint8_t  int1_cfg;
+    uint8_t  int1_src;
+    uint8_t  int1_ths;
+    uint8_t  int1_duration;
+    uint8_t  int2_cfg;
+    uint8_t  int2_src;
+    uint8_t  int2_ths;
+    uint8_t  int2_duration;
+    uint8_t  reserved_0x38_0x3F[0x08u];
+  } __attribute__((packed));
+
+  enum {
+    LIS331DLH_ID = 0x32u
+  };
+
+  enum {
+    ZYXOR = 0x80,
+    ZOR   = 0x40,
+    YOR   = 0x20,
+    XOR   = 0x10,
+    ZYXDA = 0x08,
+    ZDA   = 0x04,
+    YDA   = 0x02,
+    XDA   = 0x01,
+  };
+
+  enum {
+    SPI_MULT  = 0x40u,
+    SPI_READ  = 0x80u,
+    SPI_WRITE = 0x00u,
+  };
+
+  public:
+    LIS331DLH(HWSPIDriver* driver);
+    virtual ~LIS331DLH();
+
+    chibios_rt::EvtSource* getEventSource();
+    msg_t configure(LIS331DLHConfig* config);
+
+    /**
+    * Return the accelaration in LSB for the given axis.
+    *
+    * @param axis can be one of the axis [AXIS_X | AXIS_Y | AXIS_Z]
+    *
+    * @return Measured accelaration in in LSB for the given axis
+    */
+    int16_t getAcceleration(const uint8_t axis);
+
+    /**
+    * Return the accelaration in milli g for the given axis
+    *
+    * @param axis can be one of the axis [AXIS_X | AXIS_Y | AXIS_Z]
+    *
+    * @return Measured accelaration in in milli g for the given axis
+    */
+    int16_t getAccelerationForce(const uint8_t axis);
+
+    /**
+     * Check the presence of the accelerometer by reading
+     * the identifier register and comparing it to the standard
+     * value
+     *
+     * @return [CHECK_OK | CHECK_FAIL]
+     */
+    uint8_t getCheck();
+
+    /**
+     * Do the build in self-test of the device by applying
+     * an electrostatic force and measuring the amplitude.
+     * The result is then checked against the standard values
+     * of the manual
+     * TODO Check if it works correct
+     *
+     * @param config A working config as template (if NULL, predefined config is taken)
+     */
+    void printSelfTest(LIS331DLHConfig* config);
+
+  protected:
+    virtual msg_t main();
+
+  private:
+    inline void updateSensorData();
+
+  private:
+
+    HWSPIDriver* driver;
+    chibios_rt::EvtSource eventSource;
+    int16_t accelerations[AXIS_Z - AXIS_X + 1];
+    /**
+    * Stores the configuration [FS_2G | FS_4G | FS_8G] of
+    * the current setup
+    */
+    uint8_t currentFullScaleConfiguration;
+
+
+  };
+
+} /* amiro */
+
+#endif /* LIS331DLH_HPP_ */
diff --git a/include/amiro/bluetooth/bluetooth-connector.hpp b/include/amiro/bluetooth/bluetooth-connector.hpp
new file mode 100644
index 0000000..6bda6eb
--- /dev/null
+++ b/include/amiro/bluetooth/bluetooth-connector.hpp
@@ -0,0 +1,26 @@
+#ifndef _BLUETOOTH_CONNECTOR_H_
+#define _BLUETOOTH_CONNECTOR_H_
+
+#include <amiro/bluetooth/bluetooth.hpp>
+
+namespace amiro {
+
+  class BluetoothConnector {
+  public:
+    BluetoothConnector(BLUETOOTH *bluetooth, void *bluetoothDev, const char *typeConn);
+
+    void bluetoothConnectorListen(const char *addr);
+    void bluetoothConnectorConnect(const char *addr);
+    void bluetoothConnectorDisconnect(const char *addr);
+
+  private:
+    void bluetoothConnectorActivate(uint8_t linkid);
+    void bluetoothConnectorDeactivate();
+
+    BluetoothIwrap *iwrap;
+    void * objDev;
+    const char *typeDev;
+  };
+}
+
+#endif /* _BLUETOOTH_CONNECTOR_H_ */
diff --git a/include/amiro/bluetooth/bluetooth-descriptor.hpp b/include/amiro/bluetooth/bluetooth-descriptor.hpp
new file mode 100644
index 0000000..fb4aeb0
--- /dev/null
+++ b/include/amiro/bluetooth/bluetooth-descriptor.hpp
@@ -0,0 +1,34 @@
+#ifndef _BLUETOOTH_DESCRIPTOR_H_
+#define _BLUETOOTH_DESCRIPTOR_H_
+
+#include <string.h>
+
+#define BLUETOOTH_DESCRIPTOR_BUFFER_SIZE 64
+
+namespace amiro {
+
+  class BluetoothDescriptor {
+
+  public:
+    BluetoothDescriptor();
+
+    void bluetoothDescriptorSetLinkId(uint8_t linkId);
+    void bluetoothDescriptorSetPayloadLength(size_t length);
+
+    uint8_t bluetoothDescriptorGetLinkId();
+    size_t bluetoothDescriptorGetBufferLength();
+    size_t bluetoothDescriptorGetPayloadLength();
+
+    uint8_t *bluetoothDescriptorGetBuffer();
+    uint8_t *bluetoothDescriptorGetPayload();
+
+  private:
+//    unsigned char flags;
+    uint8_t linkId;
+    size_t length;
+    uint8_t buffer[BLUETOOTH_DESCRIPTOR_BUFFER_SIZE];
+
+  };
+}
+
+#endif /* _BLUETOOTH_DESCRIPTOR_H_ */
diff --git a/include/amiro/bluetooth/bluetooth-iwrap.hpp b/include/amiro/bluetooth/bluetooth-iwrap.hpp
new file mode 100644
index 0000000..e564a0d
--- /dev/null
+++ b/include/amiro/bluetooth/bluetooth-iwrap.hpp
@@ -0,0 +1,42 @@
+#ifndef _BLUETOOTH_IWRAP_H_
+#define _BLUETOOTH_IWRAP_H_
+
+#include <amiro/bluetooth/bluetooth-transport.hpp>
+#include <amiro/bluetooth/bluetooth-profile.hpp>
+
+#if !CH_USE_QUEUES || !CH_USE_EVENTS
+#error "Bluetooth Serial Port Profile requires CH_USE_QUEUES, CH_USE_EVENTS"
+#endif
+
+#define BLUETOOTH_IWRAP_MAILBOX_SIZE  5
+
+#define HOST_IWRAP_PLAIN_LINKID       0xFF
+
+namespace amiro {
+
+  class BluetoothIwrap : public chibios_rt::BaseStaticThread<128> {
+  public:
+    BluetoothIwrap(UARTDriver* uart);
+
+    void bluetoothIwrapSendCommand(const char* cmd);
+    msg_t iwrapTransmit(uint8_t linkid, const uint8_t* txdata, size_t length);
+
+    BluetoothTransport transport;
+
+    BluetoothProfile profiles[8];
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+    msg_t iwrapReceive();
+
+    chibios_rt::Mailbox mailbox;
+    msg_t mailboxBuffer[BLUETOOTH_IWRAP_MAILBOX_SIZE];
+
+    char connAddr[17];
+    int iwrapblock;
+  };
+}
+
+#endif /* _BLUETOOTH_IWRAP_H_ */
diff --git a/include/amiro/bluetooth/bluetooth-profile.hpp b/include/amiro/bluetooth/bluetooth-profile.hpp
new file mode 100644
index 0000000..f5de805
--- /dev/null
+++ b/include/amiro/bluetooth/bluetooth-profile.hpp
@@ -0,0 +1,25 @@
+#ifndef _BLUETOOTH_PROFILE_H_
+#define _BLUETOOTH_PROFILE_H_
+
+#define NO_RX_TX  0
+#define TX_ONLY   1
+#define RX_ONLY   2
+#define RX_TX     3
+
+namespace amiro {
+
+  class BluetoothConnector;
+
+  class BluetoothProfile {
+  public:
+    BluetoothProfile() {};
+
+    const char *addr;
+    void *connector;
+    void (BluetoothConnector::*connect) (uint8_t linkid);
+    void (BluetoothConnector::*disconnect) ();
+    uint8_t linkid;
+  };
+}
+
+#endif /* _BLUETOOTH_PROFILE_H_ */
diff --git a/include/amiro/bluetooth/bluetooth-serial.hpp b/include/amiro/bluetooth/bluetooth-serial.hpp
new file mode 100644
index 0000000..7e7e3cb
--- /dev/null
+++ b/include/amiro/bluetooth/bluetooth-serial.hpp
@@ -0,0 +1,43 @@
+#ifndef _BLUETOOTH_SERIAL_H_
+#define _BLUETOOTH_SERIAL_H_
+
+#include <amiro/bluetooth/bluetooth-connector.hpp>
+
+#define BLUETOOTH_SERIAL_MAILBOX_SIZE 5
+
+#define SERIAL_START_EVENT  1
+#define SERIAL_STOP_EVENT   2
+
+namespace amiro {
+  class BluetoothSerial : public chibios_rt::BaseStaticThread<128> {
+  public:
+    BluetoothSerial(BLUETOOTH *bluetooth, uint8_t rxtx);
+
+    void bluetoothSerialStart(uint8_t linkid);
+    void bluetoothSerialStop();
+    bool bluetoothSerialIsConnected();
+
+    void bluetoothSerialListen(const char *addr);
+    void bluetoothSerialConnect(const char *addr);
+    void bluetoothSerialDisconnect(const char *addr);
+
+    msg_t serialTransmit(const uint8_t* serialdata, size_t length);
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+    msg_t serialReceive();
+
+    BluetoothConnector serialConn;
+    uint8_t rx_tx;
+
+    chibios_rt::Mailbox mailbox;
+    msg_t mailboxBuffer[BLUETOOTH_SERIAL_MAILBOX_SIZE];
+
+    BluetoothIwrap *iwrap;
+    uint8_t linkId;
+    uint8_t stopflag;
+  };
+}
+#endif /* _BLUETOOTH_SERIAL_H_ */
diff --git a/include/amiro/bluetooth/bluetooth-transport.hpp b/include/amiro/bluetooth/bluetooth-transport.hpp
new file mode 100644
index 0000000..7968708
--- /dev/null
+++ b/include/amiro/bluetooth/bluetooth-transport.hpp
@@ -0,0 +1,76 @@
+#ifndef _BLUETOOTH_TRANSPORT_H_
+#define _BLUETOOTH_TRANSPORT_H_
+
+#include <amiro/bluetooth/bluetooth-descriptor.hpp>
+
+#define BLUETOOTH_TRANSPORT_STORAGE_MAILBOX_SIZE   10
+#define BLUETOOTH_TRANSPORT_TRANSMIT_MAILBOX_SIZE   5
+
+#define TRANSMIT_COMPLETED_EVENT  1
+
+#define FRAME_HEADER_SIZE    4
+#define FRAME_TRAILER_SIZE   1
+
+namespace amiro {
+
+  class BluetoothTransport : public chibios_rt::BaseStaticThread<128>, protected UARTConfig {
+  public:
+    BluetoothTransport(UARTDriver* uart);
+
+    chibios_rt::Mailbox* bluetoothTransportGetStorageMailbox();
+    chibios_rt::Mailbox* bluetoothTransportGetTransmitMailbox();
+
+    void bluetoothTransportSetReceiveMailbox(uint8_t linkId, chibios_rt::Mailbox* mailbox);
+    void bluetoothTransportResetState();
+    bool bluetoothTransportIsMuxMode();
+
+  protected:
+    virtual msg_t main(void);
+
+    virtual void bluetoothTransportTransmitCompleted();
+    virtual void bluetoothTransportCharacterReceived(uint16_t c);
+    virtual void bluetoothTransportReceiveCompleted();
+
+  private:
+    static void bluetoothTransportTransmitCompleted_cb(UARTDriver *uart);
+    static void bluetoothTransportCharacterReceived_cb(UARTDriver *uart, uint16_t c);
+    static void bluetoothTransportReceiveCompleted_cb(UARTDriver *uart);
+
+    UARTDriver *uart;
+
+    chibios_rt::Mailbox* receiveMailbox[10];
+    chibios_rt::Mailbox storageMailbox;
+    chibios_rt::Mailbox transmitMailbox;
+
+    msg_t storageMailboxBuffer[BLUETOOTH_TRANSPORT_STORAGE_MAILBOX_SIZE];
+    msg_t transmitMailboxBuffer[BLUETOOTH_TRANSPORT_TRANSMIT_MAILBOX_SIZE];
+
+    enum BluetoothTransportState {
+      BLUETOOTH_TRANSPORT_PLAIN   = 0,
+      BLUETOOTH_TRANSPORT_HEADER  = 1,
+      BLUETOOTH_TRANSPORT_DECODE  = 2,
+      BLUETOOTH_TRANSPORT_PAYLOAD = 3,
+      BLUETOOTH_TRANSPORT_TRAILER = 4,
+      BLUETOOTH_TRANSPORT_IDLE    = 5
+    }transmitState, receiveState;
+
+    BluetoothDescriptor descriptor[BLUETOOTH_TRANSPORT_STORAGE_MAILBOX_SIZE];
+    BluetoothDescriptor *receive;
+    BluetoothDescriptor *transmit;
+
+    size_t rcvlength;
+
+    /* These arrays Contain header and trailer */
+    uint8_t receiveFrameBuffer[5];
+    uint8_t transmitFrameBuffer[5];
+
+    void postReceiveDescriptorI();
+    void postTransmitDescriptorI();
+
+    uint8_t* getEncodeHeader();
+    uint8_t* getEncodeTailer();
+    int decodeHeader();
+  };
+}
+
+#endif /* _BLUETOOTH_TRANSPORT_H_ */
diff --git a/include/amiro/bluetooth/bluetooth-wiimote.hpp b/include/amiro/bluetooth/bluetooth-wiimote.hpp
new file mode 100644
index 0000000..c23065f
--- /dev/null
+++ b/include/amiro/bluetooth/bluetooth-wiimote.hpp
@@ -0,0 +1,56 @@
+#ifndef _BLUETOOTH_WIIMOTE_H_
+#define _BLUETOOTH_WIIMOTE_H_
+
+#include <amiro/bluetooth/bluetooth-connector.hpp>
+
+#define BLUETOOTH_WIIMOTE_MAILBOX_SIZE 5
+
+#define WIIMOTE_START_EVENT  1
+#define WIIMOTE_STOP_EVENT   2
+
+namespace amiro {
+  class BluetoothWiimote : public chibios_rt::BaseStaticThread<128> {
+  public:
+    BluetoothWiimote(BLUETOOTH *bluetooth, uint8_t rxtx);
+
+    void bluetoothWiimoteStart(uint8_t linkid);
+    void bluetoothWiimoteStop();
+    bool bluetoothWiimoteIsConnected();
+
+    void bluetoothWiimoteListen(const char *addr);
+    void bluetoothWiimoteConnect(const char *addr);
+    void bluetoothWiimoteDisconnect(const char *addr);
+
+    msg_t wiimoteTransmit(const uint8_t* wiimotecmd, size_t length);
+    void bluetoothWiimoteLedVibrate(uint8_t ledid_flag, uint8_t vibrate_flag);
+    void bluetoothWiimoteStatusInfo();
+    void bluetoothWiimoteDataBtn();
+    void bluetoothWiimoteDataBtnAcc();
+
+    class Accelerometer {
+    public:
+      int16_t x_axis, y_axis, z_axis;
+    };
+    Accelerometer *getAccelerometer();
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+    msg_t wiimoteReceive();
+
+    BluetoothConnector wiimoteConn;
+    uint8_t rx_tx;
+
+    chibios_rt::Mailbox mailbox;
+    msg_t mailboxBuffer[BLUETOOTH_WIIMOTE_MAILBOX_SIZE];
+
+    BluetoothIwrap *iwrap;
+    uint8_t linkId;
+    uint8_t stopflag;
+
+    Accelerometer accelerometer;
+  };
+}
+
+#endif /* _BLUETOOTH_WIIMOTE_H_ */
diff --git a/include/amiro/bluetooth/bluetooth.hpp b/include/amiro/bluetooth/bluetooth.hpp
new file mode 100644
index 0000000..8b1a21d
--- /dev/null
+++ b/include/amiro/bluetooth/bluetooth.hpp
@@ -0,0 +1,28 @@
+#ifndef AMIRO_BLUETOOTH_H_
+#define AMIRO_BLUETOOTH_H_
+
+#include <amiro/bluetooth/bluetooth-iwrap.hpp>
+
+namespace amiro {
+
+  class BLUETOOTH {
+  public:
+    BLUETOOTH(UARTDriver* uart);
+
+    void bluetoothStart();
+    void bluetoothReset();
+    void bluetoothEnableMux();
+    void bluetoothDisableMux();
+    void bluetoothDiscoverDevices();
+    void bluetoothListAllConnections();
+    void bluetoothSetName(const char *name);
+    void bluetoothSetPin(const char *pin);
+    void bluetoothCloseConnection(uint8_t linkid);
+    void bluetoothSendCommand(const char *cmd);
+    bool bluetoothIsMuxMode();
+
+    BluetoothIwrap iwrap;
+  };
+}
+
+#endif /* BLUETOOTH_H_ */
diff --git a/include/amiro/bluetooth/checkutility.hpp b/include/amiro/bluetooth/checkutility.hpp
new file mode 100644
index 0000000..a52b794
--- /dev/null
+++ b/include/amiro/bluetooth/checkutility.hpp
@@ -0,0 +1,50 @@
+#ifndef AMIRO_WT12_CHECKUTILITY_H_
+#define AMIRO_WT12_CHECKUTILITY_H_
+
+#include <ch.hpp>
+
+namespace amiro {
+
+  class WT12CheckUtility
+  {
+  public:
+    enum Constants {
+      MUX_StartOfFrame = 0xBFu
+    };
+
+  private:
+    struct Message {
+      struct Head {
+        uint8_t start_of_frame;
+        uint8_t link_id;
+        uint8_t flags;
+        uint16_t length;
+      };
+
+      Head head;
+      char* message;
+      uint8_t n_link;
+    };
+
+  public:
+    explicit WT12CheckUtility();
+    ~WT12CheckUtility();
+
+    void reset();
+    bool waitForReady(const bool print = false);
+    uint8_t selftest();
+    void msgPrint(const char* message, const uint16_t length);
+    void msgPrintMulti(const char* message_buf, const uint16_t* length_buf, const uint16_t num_messages);
+    void sendMessage(char* message, uint8_t link_id = 0xFF);
+    void sendMessageRaw(const char* message);
+    uint16_t receiveMessage(char* message, const uint16_t max_length);
+    uint16_t receiveMessages(char* message_buf, const uint16_t msg_buf_size, uint16_t* length_buf, const uint16_t len_buf_size, const char* last_msg, const char* filter_in = "", const char* filter_out = NULL);
+
+    void receiveRaw(uint8_t* buffer, uint16_t num_bytes, const bool print = false);
+    void receiveRawLine(const bool print = true);
+    void clearMessageBuffer(const bool print = false);
+
+  };
+}
+
+#endif // AMIRO_WT12_CHECKUTILITY_H_
diff --git a/include/amiro/bus/i2c/HWI2CDriver.hpp b/include/amiro/bus/i2c/HWI2CDriver.hpp
new file mode 100644
index 0000000..62d04fe
--- /dev/null
+++ b/include/amiro/bus/i2c/HWI2CDriver.hpp
@@ -0,0 +1,49 @@
+/*
+ * HWI2CDriver.hpp
+ *
+ *  Created on: Jun 6, 2014
+ *      Author: Robert ABEL
+ */
+
+#ifndef HWI2CDRIVER_HPP_
+#define HWI2CDRIVER_HPP_
+
+#include <ch.hpp>
+#include <hal.h>
+#include <amiro/bus/i2c/I2CDriver.hpp>
+
+/**
+ * @brief   AMiRo
+ */
+namespace amiro {
+
+  struct I2CTxParams;
+  struct I2CRxParams;
+
+  /**
+   * @brief   I²C HW Driver Abstraction
+   */
+  class HWI2CDriver : public I2CDriver {
+
+  public:
+
+    HWI2CDriver(::I2CDriver *d);
+    virtual ~HWI2CDriver() {};
+
+    virtual void start(const ::I2CConfig *config);
+    virtual void stop();
+    virtual i2cflags_t getErrors();
+    virtual msg_t masterTransmit(const I2CTxParams *params, systime_t timeout = TIME_INFINITE);
+    virtual msg_t masterReceive(const I2CRxParams *params, systime_t timeout = TIME_INFINITE);
+    virtual void acquireBus();
+    virtual void releaseBus();
+
+  private:
+
+    ::I2CDriver *driver;
+
+  };
+
+} /* amiro */
+
+#endif /* HWI2CDRIVER_HPP_ */
diff --git a/include/amiro/bus/i2c/I2CDriver.hpp b/include/amiro/bus/i2c/I2CDriver.hpp
new file mode 100644
index 0000000..794711f
--- /dev/null
+++ b/include/amiro/bus/i2c/I2CDriver.hpp
@@ -0,0 +1,45 @@
+/*
+ * AI2CDriver.hpp
+ *
+ *  Created on: Jun 6, 2014
+ *      Author: Robert ABEL
+ */
+
+#ifndef AI2CDRIVER_HPP_
+#define AI2CDRIVER_HPP_
+
+#include <ch.hpp>
+#include <hal.h>
+
+/**
+ * @brief   AMiRo
+ */
+namespace amiro {
+
+  struct I2CTxParams;
+  struct I2CRxParams;
+
+  /**
+   * @brief   I²C Driver Interface
+   */
+  class I2CDriver {
+
+  public:
+
+    I2CDriver() {};
+    virtual ~I2CDriver() {};
+
+    virtual void start(const ::I2CConfig *config) = 0;
+    virtual void stop() = 0;
+    virtual i2cflags_t getErrors() = 0;
+    virtual msg_t masterTransmit(const I2CTxParams *params, systime_t timeout = TIME_INFINITE) = 0;
+    virtual msg_t masterReceive(const I2CRxParams *params, systime_t timeout = TIME_INFINITE) = 0;
+
+    virtual void acquireBus() = 0;
+    virtual void releaseBus() = 0;
+
+  };
+
+} /* amiro */
+
+#endif /* AI2CDRIVER_HPP_ */
diff --git a/include/amiro/bus/i2c/I2CMultiplexer.hpp b/include/amiro/bus/i2c/I2CMultiplexer.hpp
new file mode 100644
index 0000000..a0f8956
--- /dev/null
+++ b/include/amiro/bus/i2c/I2CMultiplexer.hpp
@@ -0,0 +1,52 @@
+#ifndef AI2CMULTIPLEXER
+#define AI2CMULTIPLEXER
+
+#include <ch.hpp>
+#include <hal.h>
+#include <amiro/bus/i2c/I2CDriver.hpp>
+
+/**
+ * @brief   AMiRo
+ */
+namespace amiro {
+
+  /**
+   * @brief   I²C Bus Multiplexer
+   */
+  class I2CMultiplexer {
+
+  public:
+
+    I2CMultiplexer(I2CDriver *driver);
+    virtual ~I2CMultiplexer() = 0;
+
+    virtual i2cflags_t getErrors();
+    virtual msg_t masterTransmit(const I2CTxParams *params, systime_t timeout = TIME_INFINITE);
+    virtual msg_t masterReceive(const I2CRxParams *params, systime_t timeout = TIME_INFINITE);
+
+    virtual void acquireBus();
+    virtual void releaseBus();
+
+    virtual msg_t select(const uint8_t bus_id) = 0;
+    virtual msg_t deselect() = 0;
+
+  private:
+
+    I2CDriver *driver;
+
+#if I2C_USE_MUTUAL_EXCLUSION || defined(__DOXYGEN__)
+#if CH_USE_MUTEXES || defined(__DOXYGEN__)
+    /**
+    * @brief   Mutex protecting the bus.
+    */
+    ::Mutex                     mutex;
+#elif CH_USE_SEMAPHORES
+    ::Semaphore                 semaphore;
+#endif
+#endif /* I2C_USE_MUTUAL_EXCLUSION */
+
+  };
+
+} /* amiro */
+
+#endif /* AI2CMULTIPLEXER */
diff --git a/include/amiro/bus/i2c/I2CParams.hpp b/include/amiro/bus/i2c/I2CParams.hpp
new file mode 100644
index 0000000..60f021b
--- /dev/null
+++ b/include/amiro/bus/i2c/I2CParams.hpp
@@ -0,0 +1,36 @@
+/*
+ * I2CParams.hpp
+ *
+ *  Created on: Jun 10, 2014
+ *      Author: Robert ABEL
+ */
+
+#ifndef I2CPARAMS_HPP_
+#define I2CPARAMS_HPP_
+
+#include <ch.hpp>
+#include <hal.h>
+
+namespace amiro {
+
+struct I2CTxParams {
+
+  i2caddr_t      addr;
+  const uint8_t *txbuf;
+  size_t         txbytes;
+  uint8_t       *rxbuf;
+  size_t         rxbytes;
+
+};
+
+struct I2CRxParams {
+
+  i2caddr_t      addr;
+  uint8_t       *rxbuf;
+  size_t         rxbytes;
+
+};
+
+} /* amiro */
+
+#endif /* I2CPARAMS_HPP_ */
diff --git a/include/amiro/bus/i2c/VI2CDriver.hpp b/include/amiro/bus/i2c/VI2CDriver.hpp
new file mode 100644
index 0000000..36a589d
--- /dev/null
+++ b/include/amiro/bus/i2c/VI2CDriver.hpp
@@ -0,0 +1,56 @@
+#ifndef MUXEDI2CDRIVER_HPP_
+#define MUXEDI2CDRIVER_HPP_
+
+#include <ch.hpp>
+#include <hal.h>
+#include <amiro/bus/i2c/I2CDriver.hpp>
+
+/**
+ * @brief   AMiRo
+ */
+namespace amiro {
+
+  struct I2CTxParams;
+  struct I2CRxParams;
+
+  class I2CMultiplexer;
+
+  /**
+   * @brief   Virtual I²C Driver
+   */
+  class VI2CDriver : public I2CDriver {
+
+  public:
+
+    VI2CDriver(I2CMultiplexer *driver, const uint8_t bus_id);
+    virtual ~VI2CDriver() {}
+    virtual void start(const ::I2CConfig *config);
+    virtual void stop();
+    virtual i2cflags_t getErrors();
+    virtual msg_t masterTransmit(const I2CTxParams *params, systime_t timeout = TIME_INFINITE);
+    virtual msg_t masterReceive(const I2CRxParams *params, systime_t timeout = TIME_INFINITE);
+
+    virtual void acquireBus();
+    virtual void releaseBus();
+
+  private:
+
+    I2CMultiplexer *driver;
+    uint8_t id;
+
+#if I2C_USE_MUTUAL_EXCLUSION || defined(__DOXYGEN__)
+#if CH_USE_MUTEXES || defined(__DOXYGEN__)
+    /**
+    * @brief   Mutex protecting the bus.
+    */
+    ::Mutex                     mutex;
+#elif CH_USE_SEMAPHORES
+    ::Semaphore                 semaphore;
+#endif
+#endif /* I2C_USE_MUTUAL_EXCLUSION */
+
+  };
+
+} /* amiro */
+
+#endif /* MUXEDI2CDRIVER_HPP_ */
diff --git a/include/amiro/bus/i2c/mux/pca9544.hpp b/include/amiro/bus/i2c/mux/pca9544.hpp
new file mode 100644
index 0000000..9b697d9
--- /dev/null
+++ b/include/amiro/bus/i2c/mux/pca9544.hpp
@@ -0,0 +1,73 @@
+/*
+ * pca9544.hpp
+ *
+ *  Created on: Jun 6, 2014
+ *      Author: Robert ABEL
+ */
+
+#ifndef AMIRO_PCA9544_HPP_
+#define AMIRO_PCA9544_HPP_
+
+#include <ch.hpp>
+#include <hal.h>
+#include <amiro/bus/i2c/I2CParams.hpp>
+#include <amiro/bus/i2c/I2CMultiplexer.hpp>
+
+/**
+ * @brief   AMiRo
+ */
+namespace amiro {
+
+  class I2CDriver;
+
+  /**
+   * @brief   Input/Output driver class
+   */
+  template <bool ignore_deselect>
+  class PCA9544 : public I2CMultiplexer {
+
+    enum { SLA = 0x70u };
+    enum { BUS_OFF = 0x00u, BUS_ON = 0x04u };
+
+  public:
+
+    PCA9544(I2CDriver *driver, const uint8_t master_id);
+    virtual ~PCA9544() {}
+
+    virtual msg_t select(const uint8_t bus_id);
+    virtual msg_t deselect();
+
+    uint8_t getInterruptSource();
+
+    /**
+     * Return types of getCheck()
+     */
+    enum {
+      CHECK_OK   = 0x00u,
+      CHECK_FAIL = 0x01u,
+    };
+
+    /**
+     * Checks the multiplexer by writing all possible bit
+     * combinations into the MUX register and directly
+     * reading from it.
+     * TODO Check possible register write corruption if vcnl4020 is running in parallel
+     *
+     * @return [CHECK_OK | CHECK_FAIL]
+     */
+    uint8_t getCheck(void);
+
+  private:
+
+    uint8_t master_id;
+    I2CTxParams tx_params;
+    I2CRxParams rx_params;
+    uint8_t selected;
+
+  };
+
+} /* amiro */
+
+#include <components/bus/i2c/mux/pca9544.tpp>
+
+#endif /* AMIRO_PCA9544_HPP_ */
diff --git a/include/amiro/bus/spi/HWSPIDriver.hpp b/include/amiro/bus/spi/HWSPIDriver.hpp
new file mode 100644
index 0000000..6ad0920
--- /dev/null
+++ b/include/amiro/bus/spi/HWSPIDriver.hpp
@@ -0,0 +1,30 @@
+#ifndef HWSPIDRIVER_HPP_
+#define HWSPIDRIVER_HPP_
+
+#include <ch.hpp>
+#include <hal.h>
+
+/**
+ * @brief   AMiRo
+ */
+namespace amiro {
+
+  /**
+  * @brief   Input/Output driver class
+  */
+  class HWSPIDriver {
+  public:
+    HWSPIDriver(::SPIDriver *driver, const ::SPIConfig *config);
+    virtual ~HWSPIDriver();
+    void exchange(uint8_t *readData, const uint8_t *writeData, size_t length);
+    void read(uint8_t *data, size_t length);
+    void write(const uint8_t *data, size_t length);
+
+  private:
+    ::SPIDriver *driver;
+    const ::SPIConfig *config;
+  };
+
+} /* amiro */
+
+#endif /* HWSPIDRIVER_HPP_ */
diff --git a/include/amiro/debug.h b/include/amiro/debug.h
new file mode 100644
index 0000000..d5db40c
--- /dev/null
+++ b/include/amiro/debug.h
@@ -0,0 +1,82 @@
+#ifndef AMIRO_DEBUG_H_
+#define AMIRO_DEBUG_H
+
+#include <amiroosconf.h>
+#include <chprintf.h>
+#include <chdebug.h>
+
+
+
+/**
+ * @brief Simple message printing macro
+ *
+ * @param[in] fmt     the string message to print
+ * @param[in] args... the optional list of arguments to insert in the message
+ *
+ * @todo Make output stream adjustable
+ */
+#define amiroPrintf(fmt, args...) {                   \
+  chprintf((BaseSequentialStream*)&SD1, fmt, ##args); \
+}
+
+#if AMIRO_DBG || defined(__DOXYGEN__)
+
+/**
+ * @brief Macro to print an error
+ *
+ * @param[in] c       the condition to be verified to be true
+ * @param[in] msg     the string message to print
+ * @param[in] args... the optional list of arguments to insert in the message
+ */
+#define amiroDbgError(c, msg, args...) {                                     \
+  if (!(c))                                                                  \
+    amiroPrintf("ERROR: " msg "\tAT %s:%s()\n", ##args, __FILE__, __func__); \
+}
+
+/**
+ * @brief Macro to print a warning
+ *
+ * @param[in] c       the condition to be verified to be true
+ * @param[in] msg     the text message
+ * @param[in] args... the optional list of arguments to insert in the message
+ */
+#define amiroDbgWarning(c, msg, args...) {                                      \
+  if (!(c))                                                                     \
+    amiroPrintf("WARNING: " msg "\tAT: %s:%s()\n", ##args, __FILE__, __func__); \
+}
+
+/**
+ * @brief Macro to print a note
+ *
+ * @param[in] msg     the text message
+ * @param[in] args... the optional list of arguments to insert in the message
+ */
+#define amiroDbgNote(msg, args...) {                                       \
+  amiroPrintf("NOTE: " msg "\tAT: %s:%s()\n", ##args, __FILE__, __func__); \
+}
+
+/**
+ * @brief Macro to print a todo
+ *
+ * @param[in] msg     the text message
+ * @param[in] args... the optional list of arguments to insert in the message
+ */
+#define amiroDbgTodo(msg, args...) {                                       \
+  amiroPrintf("TODO: " msg "\tAT: %s:%s()\n", ##args, __FILE__, __func__); \
+}
+
+#else
+
+/*
+ * empty macro definitions, if AMIRO_DBG is FALSE
+ */
+#define amiroDbgError(c, msg, args...)
+#define amiroDbgWarning(c, msg, args...)
+#define amiroDbgNote(msg, args...)
+#define amiroDbgTodo(msg, args...)
+
+#endif
+
+
+#endif // AMIRO_DEBUG_H_
+
diff --git a/include/amiro/eeprom/at24.hpp b/include/amiro/eeprom/at24.hpp
new file mode 100644
index 0000000..5004a31
--- /dev/null
+++ b/include/amiro/eeprom/at24.hpp
@@ -0,0 +1,47 @@
+#ifndef AT24_HPP_
+#define AT24_HPP_
+
+#include <ch.hpp>
+#include <hal.h>
+
+#include <amiro/eeprom/eeprom.hpp>
+
+namespace amiro {
+
+class I2CDriver;
+
+class AT24 : public EEPROM {
+
+  enum {
+    AT24_SLA = 0x50u,
+  };
+
+  enum {
+    AT24_MAX_PAGE_SIZE = 8,
+  };
+
+  public:
+
+  /**
+    * Return types of getCheck()
+    */
+   enum {
+     CHECK_OK          = 0x00u,
+     CHECK_READ_FAIL   = 0x01u,
+     CHECK_WRITE_FAIL  = 0x02u,
+     CHECK_CHKSUM_FAIL = 0x03u,
+   };
+
+    AT24(size_t size, uint8_t page_size, uint16_t max_t_wr, I2CDriver* i2c_driver);
+    ~AT24();
+
+    static msg_t poll_ack(void* instance);
+    static size_t write(void* instance, const uint8_t* bp, size_t n);
+    static size_t read(void* instance, uint8_t* bp, size_t n);
+    static uint32_t close(void* instance);
+
+};
+
+} /* amiro */
+
+#endif /* AT24_HPP_ */
diff --git a/include/amiro/eeprom/eeprom.hpp b/include/amiro/eeprom/eeprom.hpp
new file mode 100644
index 0000000..92687ac
--- /dev/null
+++ b/include/amiro/eeprom/eeprom.hpp
@@ -0,0 +1,92 @@
+#ifndef EEPROM_HPP_
+#define EEPROM_HPP_
+
+#include <ch.hpp>
+#include <hal.h>
+
+#include <amiro/bus/i2c/I2CParams.hpp>
+
+namespace amiro {
+
+class I2CDriver;
+class HWSPIDriver;
+
+class EEPROM {
+
+  public:
+    /**
+     * Base File Stream
+     * eepromInit initializes this member.
+     */
+    BaseFileStream bfs;
+
+    msg_t error;
+    fileoffset_t position;
+
+    size_t size;
+    uint8_t page_size;
+
+    /**
+     * Maximum Write Cycle time
+     * Expressed in multiples of 10 µs.
+     */
+    uint16_t max_t_wr;
+
+    union {
+
+#if HAL_USE_I2C
+      struct {
+        I2CDriver* i2c_driver;
+        I2CTxParams i2c_txparams;
+      };
+#endif /* HAL_USE_I2C */
+
+#if HAL_USE_SPI
+      struct {
+        HWSPIDriver* spi_driver;
+      };
+#endif /* HAL_USE_SPI */
+
+    }; /* union */
+
+#if CH_USE_MUTEXES
+    /**
+     * Mutex Lock
+     */
+    Mutex mutex;
+#elif CH_USE_SEMAPHORES
+    /**
+     * Semaphore
+     */
+    Semaphore semaphore;
+#endif
+
+    enum {
+      EEPROM_ERROR_NONE = 0x00u,
+    };
+
+  protected:
+
+#if HAL_USE_I2C
+    EEPROM(const struct BaseFileStreamVMT* vmt, size_t size, uint8_t page_size, uint16_t max_t_wr, uint8_t sla, I2CDriver* i2c_driver);
+#endif /* HAL_USE_I2C */
+#if HAL_USE_SPI
+    EEPROM(const struct BaseFileStreamVMT* vmt, size_t size, uint8_t page_size, uint16_t max_t_wr, HWSPIDriver* spi_driver);
+#endif /* HAL_USE_SPI */
+    ~EEPROM();
+
+  public:
+
+    void acquire();
+    void release();
+
+    static int geterror(void* instance);
+    static fileoffset_t getsize(void* instance);
+    static fileoffset_t getposition(void* instance);
+    static fileoffset_t lseek(void *instance, fileoffset_t offset);
+
+};
+
+} /* amiro */
+
+#endif /* EEPROM_HPP_ */
diff --git a/include/amiro/gyro/l3g4200d.hpp b/include/amiro/gyro/l3g4200d.hpp
new file mode 100644
index 0000000..ac7d879
--- /dev/null
+++ b/include/amiro/gyro/l3g4200d.hpp
@@ -0,0 +1,253 @@
+#ifndef L3G4200D_HPP_
+#define L3G4200D_HPP_
+
+#include <ch.hpp>
+
+namespace amiro {
+
+class HWSPIDriver;
+
+class L3G4200D : public chibios_rt::BaseStaticThread<256> {
+
+ public:
+  enum {
+    DR_100_HZ   = 0x00,
+    DR_200_HZ   = 0x40,
+    DR_400_HZ   = 0x80,
+    DR_800_HZ   = 0xC0,
+  };
+  enum {
+    BW_12_5  = 0x00,
+    BW_20    = 0x00,
+    BW_25    = 0x10,
+    BW_30    = 0x00,
+    BW_35    = 0x10,
+    BW_50    = 0x20,
+    BW_70    = 0x30,
+    BW_110   = 0x30,
+  };
+  enum {
+    PD  = 0x08,
+    ZEN = 0x04,
+    YEN = 0x02,
+    XEN = 0x01,
+  };
+  enum {
+    HPM_NORMAL_RST = 0x00,
+    HPM_REFERENCE  = 0x10,
+    HPM_NORMAL     = 0x20,
+    HPM_AUTO_RST   = 0x30,
+  };
+  enum {
+    HPCF_2    = 0x00,
+    HPCF_4    = 0x01,
+    HPCF_8    = 0x02,
+    HPCF_16   = 0x03,
+    HPCF_32   = 0x04,
+    HPCF_64   = 0x05,
+    HPCF_128  = 0x06,
+    HPCF_256  = 0x07,
+    HPCF_512  = 0x08,
+    HPCF_1024 = 0x09,
+  };
+  enum {
+    I1_INT1   = 0x80,
+    I1_BOOT   = 0x40,
+    H_IACTIVE = 0x20,
+    PP_OD     = 0x10,
+    I2_DRDY   = 0x08,
+    I2_WTM    = 0x04,
+    I2_ORUN   = 0x02,
+    I2_EMPTY  = 0x01,
+  };
+  enum {
+    BDU_CONT    = 0x00,
+    BDU_SINGLE  = 0x80,
+    BLE_MSB     = 0x40,
+    BLE_LSB     = 0x00,
+    FS_250_DPS  = 0x00,
+    FS_500_DPS  = 0x10,
+    FS_2000_DPS = 0x20,
+    ST_SIGN_M   = 0x04,
+    ST_SIGN_P   = 0x00,
+    ST_EN       = 0x02,
+    SIM_3W      = 0x01,
+    SIM_4W      = 0x00,
+  };
+  enum {
+    BOOT          = 0x80,
+    FIFO_EN       = 0x40,
+    HP_EN         = 0x10,
+    INT1_SEL_NOHP = 0x00,
+    INT1_SEL_HP   = 0x04,
+    INT1_SEL_LP   = 0x08,
+    OUT_SEL_NOHP  = 0x00,
+    OUT_SEL_HP    = 0x01,
+    OUT_SEL_LP    = 0x02,
+  };
+  enum {
+    ZYXOR = 0x80,
+    ZOR   = 0x40,
+    YOR   = 0x20,
+    XOR   = 0x10,
+    ZYXDA = 0x08,
+    ZDA   = 0x04,
+    YDA   = 0x02,
+    XDA   = 0x01,
+  };
+  enum {
+    FM_BYPASS        = 0x00,
+    FM_FMMODE        = 0x20,
+    FM_STREAM        = 0x40,
+    FM_STREAM2FIFO   = 0x60,
+    FM_BYPASS2STREAM = 0x80,
+    WTM_MASK         = 0x1F,
+  };
+  enum {
+    WTM      = 0x80,
+    OVRN     = 0x40,
+    EMPTY    = 0x20,
+    FSS_MASK = 0x1F,
+  };
+  enum {
+    ANDOR = 0x80,
+    LIR   = 0x40,
+    ZHIE  = 0x20,
+    ZLIE  = 0x10,
+    YHIE  = 0x08,
+    YLIE  = 0x04,
+    XHIE  = 0x02,
+    XLIE  = 0x01,
+  };
+  enum {
+    IA = 0x40,
+    ZH = 0x20,
+    ZL = 0x10,
+    YH = 0x08,
+    YL = 0x04,
+    XH = 0x02,
+    XL = 0x01,
+  };
+  enum {
+    THS_L_MASK = 0x7F
+  };
+  enum {
+    INT1_WAIT          = 0x80,
+    INT1_DURATION_MASK = 0x7F,
+  };
+
+  /**
+   * Return types of getCheck()
+   */
+  enum {
+    CHECK_OK = 0x00u,
+    CHECK_FAIL = 0x01u,
+  };
+
+  /**
+   * Top view of the AMiRo with charger in the back (F:Front, B:Back).
+   * Z is pointing into the ground (Apply right-hand-rule):
+   *    ___________
+   *   /    F      \
+   *  /     X       \
+   * |      |        |
+   * |      Z---Y    |
+   * |               |
+   *  \             /
+   *   \____B______/
+   */
+  enum {
+    AXIS_X = 0x00u,
+    AXIS_Y = 0x01u,
+    AXIS_Z = 0x02u,
+  };
+
+  //private:
+
+  struct registers {
+    uint8_t RESERVED_0x00_0x0E[0x0Fu];
+    uint8_t WHO_AM_I;
+    uint8_t RESERVED_0x10_0x1F[0x10u];
+    uint8_t CTRL_REG1;
+    uint8_t CTRL_REG2;
+    uint8_t CTRL_REG3;
+    uint8_t CTRL_REG4;
+    uint8_t CTRL_REG5;
+    uint8_t REFERENCE;
+
+    uint8_t OUT_TEMP;
+
+    uint8_t STATUS_REG;
+
+    uint16_t OUT_X; /* LE */
+    uint16_t OUT_Y; /* LE */
+    uint16_t OUT_Z; /* LE */
+
+    uint8_t FIFO_CTRL_REG;
+    uint8_t FIFO_SRC_REG;
+
+    uint8_t INT1_CFG;
+    uint8_t INT1_SRC;
+    uint8_t INT1_THS_XH;
+    uint8_t INT1_THS_XL;
+    uint8_t INT1_THS_YH;
+    uint8_t INT1_THS_YL;
+    uint8_t INT1_THS_ZH;
+    uint8_t INT1_THS_ZL;
+    uint8_t INT1_DURATION;
+    uint8_t RESERVED_0x38_0x3F[0x08u];
+  }__attribute__((packed));
+
+  enum {
+    L3G4200D_ID = 0xD3u
+  };
+
+  enum {
+    SPI_MULT = 0x40u,
+    SPI_READ = 0x80u,
+    SPI_WRITE = 0x00u,
+  };
+
+ public:
+
+  struct L3G4200DConfig {
+    uint8_t         ctrl1;
+    uint8_t         ctrl2;
+    uint8_t         ctrl3;
+    uint8_t         ctrl4;
+    uint8_t         ctrl5;
+  };
+
+  L3G4200D(HWSPIDriver* driver);
+  virtual ~L3G4200D();
+
+  chibios_rt::EvtSource* getEventSource();
+  msg_t configure(const L3G4200DConfig* config);
+  int16_t angularRate[AXIS_Z - AXIS_X + 1];
+
+  int16_t getAngularRate(const uint8_t axis);
+
+
+  /**
+   * Check the presence of the accelerometer by reading
+   * the identifier register and comparing it to the standard
+   * value
+   */
+  uint8_t getCheck();
+
+ protected:
+  virtual msg_t main();
+
+ private:
+  inline void updateSensorData();
+
+ private:
+
+  HWSPIDriver* driver;
+  chibios_rt::EvtSource eventSource;
+
+};
+
+} /* amiro */
+
+#endif /* L3G4200D_HPP_ */
diff --git a/include/amiro/input/mpr121.hpp b/include/amiro/input/mpr121.hpp
new file mode 100644
index 0000000..f860cc4
--- /dev/null
+++ b/include/amiro/input/mpr121.hpp
@@ -0,0 +1,199 @@
+#ifndef MPR121_HPP_
+#define MPR121_HPP_
+
+#include <ch.hpp>
+#include <amiro/bus/i2c/I2CParams.hpp>
+
+namespace amiro {
+
+  class I2CDriver;
+
+  class MPR121 : public chibios_rt::BaseStaticThread<256>{
+
+  enum { SLA = 0x5Au };
+
+  struct thresholds {
+
+    uint8_t touch_threshold;
+    uint8_t release_threshold;
+
+  } __attribute__((packed));
+
+  struct charge_time {
+
+    uint8_t cdt0 :3;
+    uint8_t res0 :1;
+    uint8_t cdt1 :3;
+    uint8_t res1 :1;
+
+  }__attribute__((packed));
+
+  struct registers {
+
+    uint16_t           touch_status;
+    uint16_t           oor_status;
+    uint16_t           ele_filt_data[13]; /* LE */
+    uint8_t            ele_baseline[13];
+    uint8_t            mhd_rising;
+    uint8_t            nhd_amt_rising;
+    uint8_t            ncl_rising;
+    uint8_t            fdl_rising;
+    uint8_t            mhd_falling;
+    uint8_t            nhd_amt_falling;
+    uint8_t            ncl_falling;
+    uint8_t            fdl_falling;
+    uint8_t            ndh_amt_touched;
+    uint8_t            ncl_touched;
+    uint8_t            fdl_touched;
+    uint8_t            eleprox_mhd_rising;
+    uint8_t            eleprox_nhd_amt_rising;
+    uint8_t            eleprox_ncl_rising;
+    uint8_t            eleprox_fdl_rising;
+    uint8_t            eleprox_mhd_falling;
+    uint8_t            eleprox_nhd_amt_falling;
+    uint8_t            eleprox_ncl_falling;
+    uint8_t            eleprox_fdl_falling;
+    uint8_t            eleprox_nhd_amt_touched;
+    uint8_t            eleprox_ncl_touched;
+    uint8_t            eleprox_fdl_touched;
+    struct thresholds  ele_thresholds[13];
+    uint8_t            debounce;
+    uint8_t            cdc_config;
+    uint8_t            cdt_config;
+    uint8_t            ele_config;
+    uint8_t            ele_current[13];
+    struct charge_time ele_charge_time[(13 + 1)/2];
+    uint16_t           gpio_ctrl;
+    uint8_t            gpio_data;
+    uint8_t            gpio_ddr;
+    uint8_t            gpio_en;
+    uint8_t            gpio_set;
+    uint8_t            gpio_clear;
+    uint8_t            gpio_toggle;
+    uint16_t           auto_cfg_ctrl;
+    uint8_t            auto_cfg_usl;
+    uint8_t            auto_cfg_lsl;
+    uint8_t            auto_cfg_target;
+    uint8_t            soft_reset;
+
+  } __attribute__((packed));
+
+  public:
+
+  struct MPR121Config {
+
+    uint16_t global_config;
+    uint8_t  ele_config;
+    uint16_t auto_config;
+    uint8_t  up_side_limit;
+    uint8_t  low_side_limit;
+    uint8_t  target_level;
+
+  };
+
+  enum {
+
+    FFI_6     = 0x0000u,
+    FFI_10    = 0x0040u,
+    FFI_18    = 0x0080u,
+    FFI_34    = 0x00C0u,
+
+    CDT_OFF   = 0x0000u,
+    CDT_0_5   = 0x2000u,
+    CDT_1     = 0x4000u,
+    CDT_2     = 0x6000u,
+    CDT_4     = 0x8000u,
+    CDT_8     = 0xA000u,
+    CDT_16    = 0xC000u,
+    CDT_32    = 0xE000u,
+    SFI_4     = 0x0000u,
+    SFI_6     = 0x0800u,
+    SFI_10    = 0x1000u,
+    SFI_18    = 0x1800u,
+    ESI_1     = 0x0000u,
+    ESI_2     = 0x0100u,
+    ESI_4     = 0x0200u,
+    ESI_8     = 0x0300u,
+    ESI_16    = 0x0400u,
+    ESI_32    = 0x0500u,
+    ESI_64    = 0x0600u,
+    ESI_128   = 0x0700u,
+  };
+
+  enum {
+
+    CL_ON_CURRENT  = 0x00u,
+    CL_OFF         = 0x40u,
+    CL_ON_HIGH5    = 0x80u,
+    CL_ON_ALL      = 0xC0u,
+    ELEPROX_0      = 0x00u,
+    ELEPROX_2      = 0x10u,
+    ELEPROX_4      = 0x20u,
+    ELEPROX_12     = 0x30u,
+
+  };
+
+  enum {
+
+    AC_ENABLE              = 0x0001u,
+    AC_RECONF_EN           = 0x0002u,
+    BVA_ON_CURRENT         = 0x0000u,
+    BVA_OFF                = 0x0004u,
+    BVA_ON_HIGH5           = 0x0008u,
+    BVA_ON_ALL             = 0x000Cu,
+    RETRY_OFF              = 0x0000u,
+    RETRY_2                = 0x0010u,
+    RETRY_4                = 0x0020u,
+    RETRY_8                = 0x0030u,
+    /* skip FFI */
+    AC_FAIL_INT_EN         = 0x0100u,
+    AC_RECONF_FAIL_INT_EN  = 0x0200u,
+    AC_OUT_OF_RANGE_INT_EN = 0x0400u,
+    AC_SKIP_CHRG_TIME_SRCH = 0x8000u,
+
+  };
+
+  enum {
+
+    SOFT_RST_MAGIC = 0x63u,
+
+  };
+
+  public:
+
+    MPR121(I2CDriver *driver, const uint8_t master_id);
+    virtual ~MPR121();
+
+    chibios_rt::EvtSource* getEventSource();
+    uint16_t getButtonStatus();
+    uint8_t getButtonStatus(uint8_t ix);
+    uint16_t getElectrodeData(uint8_t ix);
+    uint8_t getBaselineData(uint8_t ix);
+    msg_t configure(const MPR121Config *cfg);
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+
+    inline msg_t softReset();
+    inline msg_t writeConfig(const MPR121Config *cfg);
+    inline msg_t updateButtonData();
+
+  private:
+
+    I2CDriver *driver;
+    uint8_t master_id;
+    chibios_rt::EvtSource eventSource;
+
+    uint16_t button_state;
+    uint16_t electrode_data[13];
+    uint8_t baseline_data[13];
+    I2CTxParams tx_params;
+
+};
+
+
+} /* amiro */
+
+#endif /* MPR121_HPP_ */
diff --git a/include/amiro/leds/tlc5947.hpp b/include/amiro/leds/tlc5947.hpp
new file mode 100644
index 0000000..08f953d
--- /dev/null
+++ b/include/amiro/leds/tlc5947.hpp
@@ -0,0 +1,37 @@
+#ifndef AMIRO_TLC5947_H_
+#define AMIRO_TLC5947_H_
+
+#include <amiro/Color.h>
+
+namespace amiro {
+
+  class HWSPIDriver;
+
+  class TLC5947 : public chibios_rt::BaseStaticThread<192> {
+  public:
+    TLC5947(HWSPIDriver *spi, ioportid_t blankPort, int blankPad);
+    virtual ~TLC5947();
+    void disable();
+    void enable();
+    void setBrightness(int brightness);
+    void setColor(int index, Color color);
+    void update();
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+    int caluclateRedGrayscale(Color color, int brightness) const;
+    int caluclateGreenGrayscale(Color color, int brightness) const;
+    int caluclateBlueGrayscale(Color color, int brightness) const;
+
+    HWSPIDriver *spi;
+    ioportid_t blankPort;
+    int blankPad;
+    Color colors[8];
+    int brightness;
+  };
+
+}
+
+#endif /* AMIRO_TLC5947_H_ */
diff --git a/include/amiro/magneto/hmc5883l.hpp b/include/amiro/magneto/hmc5883l.hpp
new file mode 100644
index 0000000..55d1e9a
--- /dev/null
+++ b/include/amiro/magneto/hmc5883l.hpp
@@ -0,0 +1,196 @@
+#ifndef HMC5883L_H_
+#define HMC5883L_H_
+
+#include <amiro/bus/i2c/I2CParams.hpp>
+
+namespace amiro {
+
+class I2CDriver;
+
+/**
+ * HMC5883L Magnetometer
+ * \todo Interrupt Support, self-test
+ */
+class HMC5883L : public chibios_rt::BaseStaticThread<256> {
+
+ public:
+
+  struct registers {
+    uint8_t ctrlA;
+    uint8_t ctrlB;
+    uint8_t mode;
+    uint8_t xMsb;
+    uint8_t xLsb;
+    uint8_t zMsb;
+    uint8_t zLsb;
+    uint8_t yMsb;
+    uint8_t yLsb;
+    uint8_t status;
+    uint8_t idA;
+    uint8_t idB;
+    uint8_t idC;
+  }__attribute__((packed));
+
+  struct HMC5883LConfig {
+    uint8_t ctrlA;
+    uint8_t ctrlB;
+    uint8_t mode;
+  };
+
+  enum {
+    SLA = 0x1E
+  };
+
+  enum {
+    MA_AVG1 = 0x00,
+    MA_AVG2 = 0x20,
+    MA_AVG4 = 0x40,
+    MA_AVG8 = 0x60,
+  };
+
+  enum {
+    DO_0_5_HZ = 0x00,
+    DO_1_HZ = 0x04,
+    DO_2_HZ = 0x08,
+    DO_5_HZ = 0x0C,
+    DO_10_HZ = 0x10,
+    DO_20_HZ = 0x14,
+    DO_50_HZ = 0x18,
+  };
+
+  enum {
+    MS_NORMAL = 0x00,
+    MS_BIASPOS = 0x01,
+    MS_BIASNEG = 0x02,
+  };
+
+  enum {
+    GN_0_GA = 0x00,
+    GN_1_GA = 0x20,
+    GN_2_GA = 0x40,
+    GN_3_GA = 0x60,
+    GN_4_GA = 0x80,
+    GN_5_GA = 0xA0,
+    GN_6_GA = 0xC0,
+    GN_7_GA = 0xE0,
+  };
+
+  enum {
+    MD_CONTCV = 0x00,
+    MD_SINGCV = 0x01,
+    MD_IDLE = 0x02,
+    MD_SLEEP = 0x03,
+  };
+
+  enum {
+    HS_DISABLE = 0x00,
+    HS_ENABLE = 0x80,
+  };
+
+  enum {
+    SR_REN = 0x04,
+    SR_LOCK = 0x02,
+    SR_RDY = 0x01,
+  };
+
+  enum {
+    ID_IRA = 'H',
+    ID_IRB = '4',
+    ID_IRC = '3',
+  };
+
+  enum {
+    MAGNETO_AVG = 10
+  };
+
+  /**
+   * Return types of getCheck()
+   */
+  enum {
+    CHECK_OK = 0x00u,
+    CHECK_FAIL = 0x01u,
+  };
+
+ public:
+  HMC5883L(I2CDriver *driver, const HMC5883LConfig *config);
+  virtual ~HMC5883L();
+
+  msg_t configure(HMC5883LConfig* config);
+
+  chibios_rt::EvtSource* getEventSource();
+
+  /**
+   * Check the presence of the magnetometer by reading
+   * the identifier register and comparing it to the standard
+   * value
+   */
+  uint8_t getCheck();
+
+  /**
+   * Top view of the AMiRo with charger in the back (F:Front, B:Back).
+   * Z is pointing into the ground (Apply right-hand-rule):
+   *    ___________
+   *   /    F      \
+   *  /     X       \
+   * |      |        |
+   * |      Z---Y    |
+   * |               |
+   *  \             /
+   *   \____B______/
+   */
+  enum {
+    AXIS_X = 0x00u,
+    AXIS_Y = 0x02u,
+    AXIS_Z = 0x01u,
+  };
+
+  /**
+   * Return the magnetization in LSB for the given axis.
+   *
+   * @param axis can be one of the axis [AXIS_X | AXIS_Y | AXIS_Z]
+   *
+   * @return Measured magnetization in in LSB for the given axis
+   */
+  int16_t getMagnetization(const uint8_t axis);
+
+  /**
+   * Return the magnetization in µGauss for the given axis.
+   *
+   * @param axis can be one of the axis [AXIS_X | AXIS_Y | AXIS_Z]
+   *
+   * @return Measured magnetization in µGauss for the given axis
+   */
+  int32_t getMagnetizationGauss(const uint8_t axis);
+
+ protected:
+  virtual msg_t main(void);
+
+ private:
+
+  /**
+   * Update the sensor values by reading accessing
+   * the sensor over I2C
+   */
+  inline void updateSensorData();
+
+  /**
+   * Writes the config to the HMC5883L
+   *
+   * @return I2C write return message
+   */
+  msg_t writeConf();
+
+  /**
+   * Stores the three orientations x,z,y in LSB
+   */
+  int16_t data[3];
+
+  I2CDriver *driver;
+  const HMC5883LConfig *config;
+  chibios_rt::EvtSource eventSource;
+  I2CTxParams txParams;
+};
+
+}
+
+#endif /* HMC5883L_H_ */
diff --git a/include/amiro/memoryLayout.h b/include/amiro/memoryLayout.h
new file mode 100644
index 0000000..3695785
--- /dev/null
+++ b/include/amiro/memoryLayout.h
@@ -0,0 +1,297 @@
+#ifndef AMIRO_FILESYSTEMIO_H_
+#define AMIRO_FILESYSTEMIO_H_
+
+/*! \brief memory layouts for the eeproms
+ *
+ * This header defines the memory layout.
+ * Every memory layout starts with a preamble
+ * of 3 byte. The first one is a magic byte
+ * for checking purpose. The second one declares
+ * the byte structure major version (BSMV) and the third
+ * the byte structure minor version (bsmv) of the subsequently
+ * bytes in the memory. With different major versions, the
+ * layout of the memory may totaly change, so one should
+ * not read from such a different layout if the version
+ * differs from the expected one. With greater minor
+ * versions the old memory layout may not change, but
+ * new elements emerge.
+ */
+
+#include <amiro/eeprom/at24.hpp>
+#include <chprintf.h>
+
+namespace amiro {
+namespace fileSystemIo {
+
+  class FileSystemIoBase {
+
+  public:
+    AT24 *at24c01;
+    const uint8_t BSMV;
+    const uint8_t bsmv;
+    const uint8_t HMV;
+    const uint8_t hmv;
+    const uint8_t magicByteValue = 0xAAu;
+
+    // TODO implement CRC check
+    struct preamble {
+      uint8_t magicByte;
+      uint8_t byteStructureMajorVersion;
+      uint8_t byteStructureMinorVersion;
+      uint8_t hardwareMajorVersion;
+      uint8_t hardwareMinorVersion;
+      uint8_t reserved_0x04_0x14[15];
+    };
+
+    enum{
+      OK                    = 0x00u,
+      NOT_IMPLEMENTED       = 0x01u,
+      WRONG_BSMV            = 0x02u,
+      WRONG_bsmv            = 0x03u,
+      IO_ERROR              = 0x04u,
+      WRONG_MAGICBYTE_VALUE = 0x05u,
+      WRONG_INDEX           = 0x06u,
+      WRONG_HMV             = 0x07u,
+      WRONG_hmv             = 0x08u,
+    };
+
+  public:
+
+    FileSystemIoBase(AT24 *at24c01, uint8_t BSMV, uint8_t bsmv, uint8_t HMV, uint8_t hmv)
+      : at24c01(at24c01),
+        BSMV(BSMV),
+        bsmv(bsmv),
+        HMV(HMV),
+        hmv(hmv) {
+     }
+
+    msg_t getCheck(){
+
+      // Get the preamble
+      const uint8_t bufferSize = sizeof(FileSystemIoBase::preamble);
+      uint8_t buffer[bufferSize];
+      chFileStreamSeek((BaseFileStream*)at24c01, offsetof(FileSystemIoBase::preamble, magicByte));
+      msg_t bytesWritten = chSequentialStreamRead((BaseFileStream*)at24c01, (uint8_t*) buffer, sizeof(FileSystemIoBase::preamble));
+
+      if (bytesWritten != bufferSize) {
+        return IO_ERROR;
+      } else if (buffer[offsetof(FileSystemIoBase::preamble, magicByte)] != magicByteValue) {
+        return WRONG_MAGICBYTE_VALUE;
+      } else if (buffer[offsetof(FileSystemIoBase::preamble, byteStructureMajorVersion)] != BSMV) {
+        return WRONG_BSMV;
+      } else if (buffer[offsetof(FileSystemIoBase::preamble, byteStructureMinorVersion)] < bsmv) {
+        return WRONG_bsmv;
+      } else if (buffer[offsetof(FileSystemIoBase::preamble, hardwareMajorVersion)] != HMV) {
+        return WRONG_HMV;
+      } else if (buffer[offsetof(FileSystemIoBase::preamble, hardwareMinorVersion)] < hmv) {
+        return WRONG_hmv;
+      } else {
+        return OK;
+      }
+    }
+
+    msg_t initMemory(){
+
+      // Get the preamble
+      const uint8_t bufferSize = offsetof(FileSystemIoBase::preamble, hardwareMinorVersion) + 1;
+      uint8_t buffer[bufferSize];
+
+      chFileStreamSeek((BaseFileStream*)at24c01, offsetof(FileSystemIoBase::preamble, magicByte));
+
+      buffer[offsetof(FileSystemIoBase::preamble, magicByte)]                 = magicByteValue;
+      buffer[offsetof(FileSystemIoBase::preamble, byteStructureMajorVersion)] = BSMV;
+      buffer[offsetof(FileSystemIoBase::preamble, byteStructureMinorVersion)] = bsmv;
+      buffer[offsetof(FileSystemIoBase::preamble, hardwareMajorVersion)]      = HMV;
+      buffer[offsetof(FileSystemIoBase::preamble, hardwareMinorVersion)]      = hmv;
+
+      msg_t bytesWritten = chSequentialStreamWrite((BaseFileStream*)at24c01, (uint8_t*) buffer, bufferSize);
+
+      if (bytesWritten != bufferSize) {
+        return IO_ERROR;
+      } else {
+        return OK;
+      }
+    }
+
+
+
+};
+
+
+class DiWheelDriveFSIO : public FileSystemIoBase {
+  private:
+
+    /** \brief Layout for DiWheelDriveFSIO with BSMV 1 and bsmv 1*/
+    struct DiWheelDrive_1_1 {
+      uint8_t  reserved_0x00_0x14[20];
+      uint16_t vcnl4020offset[4];       // U [12, 13, 15, 16]
+      uint8_t  generalPurpose[96];      // 1Cx_7Cx
+      uint8_t  reserved_0x7D_0x80[4];   // TODO CRC
+    };
+
+  public:
+    DiWheelDriveFSIO(AT24 *at24c01, uint8_t BSMV, uint8_t bsmv, uint8_t HMV, uint8_t hmv)
+      : FileSystemIoBase(at24c01, BSMV, bsmv, HMV, hmv) {
+
+    }
+
+    msg_t getVcnl4020Offset (uint16_t *buffer, uint8_t idx) {
+
+      msg_t bytesWritten;
+      const uint8_t bufferSize = sizeof(uint16_t);
+
+      // Check if the vcnl index is in a valid range
+      if (idx >= sizeof(DiWheelDrive_1_1::vcnl4020offset)) {
+        return WRONG_INDEX;
+      }
+
+      // Get the data out of the memory
+      switch (this->BSMV) {
+        case 0x01u:
+          // Set the pointer to the address
+          chFileStreamSeek((BaseFileStream*)at24c01, offsetof(DiWheelDriveFSIO::DiWheelDrive_1_1, vcnl4020offset[idx]));
+          switch (this->bsmv) {
+            default:
+              bytesWritten = chSequentialStreamRead((BaseFileStream*)at24c01, (uint8_t*) buffer, bufferSize);
+              break;
+          }
+          break;
+        }
+
+        if (bytesWritten != bufferSize) {
+          return IO_ERROR;
+        } else {
+          return OK;
+        }
+    }
+
+    msg_t setVcnl4020Offset (uint16_t *buffer, uint8_t idx) {
+
+      msg_t bytesWritten;
+      const uint8_t bufferSize = sizeof(uint16_t);
+
+      // Check if the vcnl index is in a valid range
+      if (idx >= sizeof(DiWheelDrive_1_1::vcnl4020offset)) {
+        return WRONG_INDEX;
+      }
+
+      // Get the data out of the memory
+      switch (this->BSMV) {
+        case 0x01u:
+          // Set the pointer to the address
+          chFileStreamSeek((BaseFileStream*)at24c01, offsetof(DiWheelDriveFSIO::DiWheelDrive_1_1, vcnl4020offset[idx]));
+          switch (this->bsmv) {
+            default:
+              bytesWritten = chSequentialStreamWrite((BaseFileStream*)at24c01, (const uint8_t*) buffer, bufferSize);
+              break;
+          }
+          break;
+        }
+
+        if (bytesWritten != bufferSize) {
+          return IO_ERROR;
+        } else {
+          return OK;
+        }
+    }
+};
+
+class PowerBoardFSIO : public FileSystemIoBase {
+  private:
+
+    /** \brief Layout for PowerBoardFSIO with BSMV 1 and bsmv 1*/
+    struct PowerBoard_1_1 {
+      uint8_t  reserved_0x00_0x14[20];
+      uint16_t vcnl4020offset[8];        // U1.Proximity [11, 21, 31, 41, 12, 22, 32, 42]
+      uint8_t  generalPurpose[88];       // 24x_7Cx
+      uint8_t  reserved_0x7D_0x80[4];    // TODO CRC
+    };
+
+  public:
+    PowerBoardFSIO(AT24 *at24c01, uint8_t BSMV, uint8_t bsmv, uint8_t HMV, uint8_t hmv)
+      : FileSystemIoBase(at24c01, BSMV, bsmv, HMV, hmv) {
+
+    }
+
+    msg_t getVcnl4020Offset (uint16_t *buffer, uint8_t idx) {
+
+      msg_t bytesWritten;
+      const uint8_t bufferSize = sizeof(uint16_t);
+
+      // Check if the vcnl index is in a valid range
+      if (idx >= sizeof(PowerBoard_1_1::vcnl4020offset)) {
+        return WRONG_INDEX;
+      }
+
+      // Get the data out of the memory
+      switch (this->BSMV) {
+        case 0x01u:
+          // Set the pointer to the address
+          chFileStreamSeek((BaseFileStream*)at24c01, offsetof(PowerBoardFSIO::PowerBoard_1_1, vcnl4020offset[idx]));
+          switch (this->bsmv) {
+            default:
+              bytesWritten = chSequentialStreamRead((BaseFileStream*)at24c01, (uint8_t*) buffer, bufferSize);
+              break;
+          }
+          break;
+        }
+
+        if (bytesWritten != bufferSize) {
+          return IO_ERROR;
+        } else {
+          return OK;
+        }
+    }
+
+    msg_t setVcnl4020Offset (uint16_t *buffer, uint8_t idx) {
+
+      msg_t bytesWritten;
+      const uint8_t bufferSize = sizeof(uint16_t);
+
+      // Check if the vcnl index is in a valid range
+      if (idx >= sizeof(PowerBoard_1_1::vcnl4020offset)) {
+        return WRONG_INDEX;
+      }
+
+      // Get the data out of the memory
+      switch (this->BSMV) {
+        case 0x01u:
+          // Set the pointer to the address
+          chFileStreamSeek((BaseFileStream*)at24c01, offsetof(PowerBoardFSIO::PowerBoard_1_1, vcnl4020offset[idx]));
+          switch (this->bsmv) {
+            default:
+              bytesWritten = chSequentialStreamWrite((BaseFileStream*)at24c01, (const uint8_t*) buffer, bufferSize);
+              break;
+          }
+          break;
+        }
+
+        if (bytesWritten != bufferSize) {
+          return IO_ERROR;
+        } else {
+          return OK;
+        }
+    }
+};
+
+class LightRingFSIO : public FileSystemIoBase {
+  private:
+
+    /** \brief Layout for LightRingFSIO with BSMV 1 and bsmv 1*/
+    struct LightRing_1_1 {
+      uint8_t  reserved_0x00_0x14[20];
+      uint8_t  generalPurpose[104];      // 15x_80x
+      uint8_t  reserved_0x7D_0x80[4];    // TODO CRC
+    };
+
+  public:
+    LightRingFSIO(AT24 *at24c01, uint8_t BSMV, uint8_t bsmv, uint8_t HMV, uint8_t hmv)
+      : FileSystemIoBase(at24c01, BSMV, bsmv, HMV, hmv) {
+
+    }
+};
+
+}
+}
+
+#endif /* AMIRO_FILESYSTEMIO_H_ */
diff --git a/include/amiro/power/adconverter.hpp b/include/amiro/power/adconverter.hpp
new file mode 100644
index 0000000..5f7b684
--- /dev/null
+++ b/include/amiro/power/adconverter.hpp
@@ -0,0 +1,51 @@
+/*
+ * ADC for monitoring the system voltage and handling the charger logic
+ */
+
+#ifndef AMIRO_ADCONVERTER_HPP
+#define AMIRO_ADCONVERTER_HPP
+
+#include <ch.hpp>
+#include <hal.h>
+#include <evtimer.h>
+
+namespace amiro {
+
+class ADConverter : public chibios_rt::BaseStaticThread<256>
+{
+public:
+  enum Events {
+    EVT_WATCHDOG = 0x00,
+    EVT_TIMER = 0x01
+  };
+
+  static uint16_t uV2Adc(const uint32_t uV);
+  static uint32_t adc2uV(const uint16_t adc);
+
+private:
+  ADCDriver& driver;
+  ADCConversionGroup& conversion_group;
+  uint32_t watchdog_threshold_uV;
+  chibios_rt::EvtListener evt_listener;
+  chibios_rt::EvtSource* evt_source;
+  EvTimer evt_timer;
+
+public:
+  ADConverter(ADCDriver &adcdrv, ADCConversionGroup& adccgrp, const uint32_t threshold_uV);
+  virtual ~ADConverter();
+
+  ADCConversionGroup& getConversionGroup() {return this->conversion_group;}
+  const ADCConversionGroup& getConversionGroup() const {return this->conversion_group;}
+
+protected:
+  virtual msg_t main(void);
+
+private:
+  void enableWatchdog(const bool enable = true);
+
+};
+
+}
+
+#endif // AMIRO_ADCONVERTER_HPP
+
diff --git a/include/amiro/power/bq24103a.hpp b/include/amiro/power/bq24103a.hpp
new file mode 100644
index 0000000..6c40902
--- /dev/null
+++ b/include/amiro/power/bq24103a.hpp
@@ -0,0 +1,65 @@
+/*
+ * BQ24103A - Battery Charger
+ */
+
+#ifndef AMIRO_BQ24103A_H_
+#define AMIRO_BQ24103A_H_
+
+namespace amiro {
+
+class BaseBQ24103A
+{
+public:
+  BaseBQ24103A()
+  {}
+
+  virtual ~BaseBQ24103A()
+  {}
+
+  virtual void enable(const bool en = true) = 0;
+
+  virtual bool isEnabled() const = 0;
+
+  virtual bool isCharging() const = 0;
+};
+
+template <uintptr_t EN_PadGrp, uint32_t EN_Pad, uintptr_t STAT_PadGrp, uint32_t STAT_Pad>
+class BQ24103A : public BaseBQ24103A
+{
+private:
+  bool is_enabled;
+
+public:
+  BQ24103A(const bool enable = false) :
+    is_enabled(enable)
+  {
+    this->enable(enable);
+  }
+
+  virtual ~BQ24103A()
+  {}
+
+  void enable(const bool enable = true)
+  {
+    // writing to a port is not atomic, thus the system must be locked
+    chSysLock();
+    palWritePad((GPIO_TypeDef*)EN_PadGrp, EN_Pad, (enable? PAL_LOW : PAL_HIGH));
+    this->is_enabled = enable;
+    chSysUnlock();
+    return;
+  }
+
+  bool isEnabled() const
+  {
+    return this->is_enabled;
+  }
+
+  bool isCharging() const
+  {
+    return (palReadPad((GPIO_TypeDef*)STAT_PadGrp, STAT_Pad) == PAL_LOW);
+  }
+};
+
+}
+
+#endif /* AMIRO_BQ24103A_H_ */
diff --git a/include/amiro/power/bq27500.hpp b/include/amiro/power/bq27500.hpp
new file mode 100644
index 0000000..f174056
--- /dev/null
+++ b/include/amiro/power/bq27500.hpp
@@ -0,0 +1,496 @@
+/*
+ * BQ27500 - Fuel Gauge
+ */
+
+#ifndef AMIRO_BQ27500_H_
+#define AMIRO_BQ27500_H_
+
+#include <ch.hpp>
+#include <amiro/bus/i2c/I2CParams.hpp>
+#include <amiro/bus/i2c/I2CDriver.hpp>
+
+#include <amiro/BaseSensor.hpp>
+
+namespace amiro {
+
+  namespace BQ27500 {
+
+    struct InitData {};
+
+    struct CalibData
+    {
+      struct Configuration
+      {
+        struct Safety {
+          enum SubclassID {SUB_ID = 2};
+          const int16_t ot_chg = 450; // 0.1°C
+          const uint8_t ot_chg_time = 2; // seconds
+          const int16_t ot_chg_recovery = 400; // 0.1°C
+          const int16_t ot_dsg = 600; // 0.1°C
+          const uint8_t ot_dsg_time = 2; // seconds
+          const int16_t ot_dsg_recovery = 500; // 0.1°C
+          enum Offset {
+            OFFSET_OtChg = 0,
+            OFFSET_OtChgTime = 2,
+            OFFSET_OtChgRecovery = 3,
+            OFFSET_OtDsg = 5,
+            OFFSET_OtDsgTime = 7,
+            OFFSET_OtDsgRecovery = 8
+          };
+        } safety;
+
+        struct ChargeInhibitConfig {
+          enum SubclassID {SUB_ID = 32};
+          const int16_t charge_inhibit_temp_low = 0; // 0.1°C
+          const int16_t charge_inhibit_temp_high = 450; // 0.1°C
+          const int16_t temp_hys = 50; // 0.1°C
+          enum Offset {
+            OFFSET_ChargeInhibitTempLow = 0,
+            OFFSET_ChargeInhibitTempHigh = 2,
+            OFFSET_TempHys = 4
+          };
+        } charge_inhibit_config;
+
+        struct Charge {
+          enum SubclassID {SUB_ID = 34};
+          const int16_t charging_voltage = 4200; // mV
+          const int16_t delta_temperature = 50; // 0.1°C
+          const int16_t suspend_temperature_low = -200; // 0.1°C
+          const int16_t suspend_temperature_high = 600; // 0.1°C
+          enum Offset {
+            OFFSET_ChargingVoltage = 2,
+            OFFSET_DeltaTemperature = 4,
+            OFFSET_SuspendTemperatureLow = 6,
+            OFFSET_SuspendTemperatureHigh = 8
+          };
+        } charge;
+
+        struct ChargeTermination {
+          enum SubclassID {SUB_ID = 36};
+          const int16_t taper_current = 100; // mA
+          const int16_t minimum_taper_charge = 25; // 0.01mAh
+          const int16_t taper_voltage = 100; // mV
+          const uint16_t current_taper_window = 40; // seconds
+          enum Offset {
+            OFFSET_TaperCurrent = 2,
+            OFFSET_MinimumTaperCharge = 4,
+            OFFSET_TaperVoltage = 6,
+            OFFSET_CurrentTaperWindow = 8
+          };
+        } charge_termination;
+
+        struct Data {
+          enum SubclassID {SUB_ID = 48};
+          const int8_t initial_standby_current = -10; // mA
+          const int16_t initial_max_load_current = -500; // mA
+          const int16_t cc_threshold = 900; // mAh
+          const int16_t design_capacity = 1950; // mAh
+          const char device_name[8] = "bq27500";
+          enum Offset {
+            OFFSET_InitialStandbyCurrent = 4,
+            OFFSET_InitialMaxLoadCurrent = 5,
+            OFFSET_CCThreshold = 7,
+            OFFSET_DesignCapacity = 10,
+            OFFSET_DeviceName = 12
+          };
+        } data;
+
+        struct Discharge {
+          enum SublcassID {SUB_ID = 49};
+          const uint8_t soc1_set_threshold = 150; // mAh
+          const uint8_t soc1_clear_threshold = 175; // mAh
+          const uint8_t socf_set_threshold = 75; // mAh
+          const uint8_t socf_clear_threshold = 100; // mAh
+          enum Offset {
+            OFFSET_Soc1SetThreshold = 0,
+            OFFSET_Soc1ClearThreshold = 1,
+            OFFSET_SocfSetThreshold = 2,
+            OFFSET_SocfClearThreshold = 3
+          };
+        } discharge;
+
+        struct Registers {
+          enum SubclassID {SUB_ID = 64};
+          union OperationConfiguration {
+            const uint16_t value = 0x0979u;
+            struct {
+              uint16_t temps : 1;
+              uint16_t rsvd_1 : 1;
+              uint16_t batg_pol : 1;
+              uint16_t batl_pol : 1;
+              uint16_t rmfcc : 1;
+              uint16_t sleep : 1;
+              uint16_t idselen : 1;
+              uint16_t rsvd_7 : 1;
+              uint16_t rsns : 2;
+              uint16_t iwake : 1;
+              uint16_t pfc : 2;
+              uint16_t rsvd_13 : 1;
+              uint16_t batg_ovr : 1;
+              uint16_t rescap : 1;
+            } content;
+          } operation_configuration;
+          enum Offset {
+            OFFSET_OperationConfiguration = 0
+          };
+        } registers;
+
+        struct Power {
+          enum SubclassID {SUB_ID = 68};
+          const int16_t flash_update_ok_voltage = 2800; // mV
+          const int16_t sleep_current = 10; // mA
+          const uint16_t hibernate_current = 8; // mA
+          const uint16_t hibernate_voltage = 2550; // mV
+          enum Offset {
+            OFFSET_FlashUpdateOkVoltage = 0,
+            OFFSET_SleepCurrent = 7,
+            OFFSET_HibernateCurrent = 16,
+            OFFSET_HibernateVoltage = 18
+          };
+        } power;
+      } configuration;
+
+      struct SystemData
+      {
+        struct ManufacturerInfo {
+          enum SubclassID {SUB_ID = 57};
+          // the very first byte is reserved as magic byte (0xAA)
+          uint8_t version_major = 1;
+          uint8_t version_minor = 0;
+          enum Offset {
+            OFFSET_VersionMajor = 1,
+            OFFSET_VersionMinor = 2
+          };
+        } manufacturer_info;
+      } system_data;
+
+      struct FuelGauging
+      {
+        struct ITConfig {
+          enum SubclassID {SUB_ID = 80};
+          const uint8_t load_select = 1;
+          const uint8_t load_mode = 0;
+          const int16_t terminate_voltage = 3000; // mV
+          const int16_t user_rate_mA = 0; // mA
+          const int16_t user_rate_mW = 0; // mW
+          const int16_t reserve_cap_mAh = 0; // mAh
+          const int16_t reserve_cap_mWh = 0; // mWh
+          enum Offset {
+            OFFSET_LoadSelect = 0,
+            OFFSET_LoadMode = 1,
+            OFFSET_TerminateVoltage = 48,
+            OFFSET_UserRate_mA = 53,
+            OFFSET_UserRate_mW = 55,
+            OFFSET_ReserveCap_mAh = 57,
+            OFFSET_ReserveCap_mWh = 59
+          };
+        } it_config;
+
+        struct CurrentThresholds {
+          enum SubclassID {SUB_ID = 81};
+          const int16_t dsg_current_threshold = 60; // mA
+          const int16_t chg_current_threshold = 75; // mA
+          const int16_t quit_current = 40; // mA
+          const uint16_t dsg_relax_time = 60; // seconds
+          const uint16_t chg_relax_time = 60; // seconds
+          const uint8_t quit_relax_time = 1; // seconds
+          enum Offset {
+            OFFSET_DsgCurrentThreshold = 0,
+            OFFSET_ChgCurrentThreshold = 2,
+            OFFSET_QuitCurrent = 4,
+            OFFSET_DsgRelaxTime = 6,
+            OFFSET_ChgRelaxTime = 8,
+            OFFSET_QuitRelaxTime = 10
+          };
+        } current_thresholds;
+
+        struct State {
+          enum SubclassID {SUB_ID = 82};
+          const uint8_t it_enable = 0x00;
+          const uint8_t application_status = 0x00;
+          const int16_t qmax_0 = 1950; // mAh
+          const uint16_t cycle_count_0 = 0;
+          const uint8_t update_status_0 = 0x00;
+          const int16_t qmax_1 = 1950; // mAh
+          const uint16_t cycle_count_1 = 0;
+          const uint8_t update_status_1 = 0x00;
+          const int16_t avg_I_last_run = -299; // mA
+          const int16_t avg_P_last_run = -1131; // mAh
+          enum Offset {
+            OFFSET_ItEnable = 0,
+            OFFSET_ApplicationStatus = 1,
+            OFFSET_Qmax0 = 2,
+            OFFSET_CycleCount0 = 4,
+            OFFSET_UpdateStatus0 = 6,
+            OFFSET_Qmax1 = 7,
+            OFFSET_CycleCount1 = 9,
+            OFFSET_UpdateStatus1 = 11,
+            OFFSET_AvgILastRun = 16,
+            OFFSET_AvgPLastRun = 18
+          };
+        } state;
+      } fuel_gauging;
+
+      struct Security
+      {
+        struct Codes {
+          enum SubclassID {SUB_ID = 112};
+          const uint16_t unseal_key_0 = 0x3672;
+          const uint16_t unseal_key_1 = 0x0414;
+          const uint16_t full_access_key_0 = 0xFFFF;
+          const uint16_t full_access_key_1 = 0xFFFF;
+          enum Offset {
+            OFFSET_UnsealKey0 = 0,
+            OFFSET_UnsealKey1 = 2,
+            OFFSET_FullAccessKey0 = 4,
+            OFFSET_FullAccessKey1 = 6
+          };
+        } codes;
+      } security;
+
+    };
+
+    class Driver : public BaseSensor<InitData,CalibData>, public chibios_rt::BaseStaticThread<256>
+    {
+    public:
+      enum StandardCommand {
+                                                                            //  unit    |      access
+                                                                            //          | sealed  | unsealed
+                                                                            // ---------+---------+----------
+        STD_CMD_Control = 0x00u, STD_CMD_CNTL = 0x00u,                      // N/A      | R/W     | R/W
+        STD_CMD_AtRate = 0x02u, STD_CMD_AR = 0x02u,                         // mA       | R/W     | R/W
+        STD_CMD_AtRateTimeToEmpty = 0x04u, STD_CMD_ARTTE = 0x04u,           // minutes  | R       | R/W
+        STD_CMD_Temperatur = 0x06u, STD_CMD_TEMP = 0x06u,                   // 0.1 K    | R       | R/W
+        STD_CMD_Voltage = 0x08u, STD_CMD_VOLT = 0x08u,                      // mV       | R       | R/W
+        STD_CMD_Flags = 0x0Au, STD_CMD_FLAGS = 0x0Au,                       // N/A      | R       | R/W
+        STD_CMD_NominalAvailableCapacity = 0x0Cu, STD_CMD_NAC = 0x0Cu,      // mAh      | R       | R/W
+        STD_CMD_FullAvailableCapacity = 0x0Eu, STD_CMD_FAC = 0x0Eu,         // mAh      | R       | R/W
+        STD_CMD_RemainingCapacity = 0x10u, STD_CMD_RM = 0x10u,              // mAh      | R       | R/W
+        STD_CMD_FullChargeCapacity = 0x12u, STD_CMD_FCC = 0x12u,            // mAh      | R       | R/W
+        STD_CMD_AverageCurrent = 0x14u, STD_CMD_AI = 0x14u,                 // mA       | R       | R/W
+        STD_CMD_TimeToEmpty = 0x16u, STD_CMD_TTE = 0x16u,                   // minutes  | R       | R/W
+        STD_CMD_TimeToFull = 0x18u, STD_CMD_TTF = 0x18u,                    // minutes  | R       | R/W
+        STD_CMD_StandbyCurrent = 0x1Au, STD_CMD_SI = 0x1Au,                 // mA       | R       | R/W
+        STD_CMD_StandbyTimeToEmpty = 0x1Cu, STD_CMD_STTE = 0x1Cu,           // minutes  | R       | R/W
+        STD_CMD_MaxLoadCurrent = 0x1Eu, STD_CMD_MLI = 0x1Eu,                // mA       | R       | R/W
+        STD_CMD_MaxLoadTimeToEmpty = 0x20u, STD_CMD_MLTTE = 0x20u,          // minutes  | R       | R/W
+        STD_CMD_AvailableEnergy = 0x22u, STD_CMD_AE = 0x22u,                // mWh      | R       | R/W
+        STD_CMD_AveragePower = 0x24u, STD_CMD_AP = 0x24u,                   // mW       | R       | R/W
+        STD_CMD_TimeToEmptyAtConstantPower = 0x26u, STD_CMD_TTECP = 0x26u,  // minutes  | R       | R/W
+        STD_CMD_CycleCount = 0x2Au, STD_CMD_CC = 0x2Au,                     // counts   | R       | R/W
+        STD_CMD_StateOfCharge = 0x2Cu, STD_CMD_SOC = 0x2Cu                  // %        | R       | R/W
+      };
+
+      enum ControlSubcommand {
+                                            // sealed access
+                                            // -------------
+        SUB_CMD_CONTROL_STATUS = 0x0000u,   // yes
+        SUB_CMD_DEVICE_TYPE = 0x0001u,      // yes
+        SUB_CMD_FW_VERSION = 0x0002u,       // yes
+        SUB_CMD_HW_VERSION = 0x0003u,       // yes
+        SUB_CMD_DF_CHECKSUM = 0x0004u,      // no
+        SUB_CMD_RESET_DATA = 0x0005u,       // yes
+        SUB_CMD_PREV_MACWRITE = 0x0007u,    // yes
+        SUB_CMD_CHEM_ID = 0x0008u,          // yes
+        SUB_CMD_BOARD_OFFSET = 0x0009u,     // no
+        SUB_CMD_CC_INT_OFFSET = 0x000Au,    // no
+        SUB_CMD_WRITE_OFFSET = 0x000Bu,     // no
+        SUB_CMD_SET_HIBERNATE = 0x0011u,    // yes
+        SUB_CMD_CLEAR_HIBERNATE = 0x0012u,  // yes
+        SUB_CMD_SET_SLEEPp = 0x0013u,       // yes
+        SUB_CMD_CLEAR_SLEEPp = 0x0014u,     // yes
+        SUB_CMD_SEALED = 0x0020u,           // no
+        SUB_CMD_IT_ENABLE = 0x0021u,        // no
+        SUB_CMD_IF_CHECKSUM = 0x0022u,      // no
+        SUB_CMD_CAL_MODE = 0x0040u,         // no
+        SUB_CMD_RESET = 0x0041u,            // no
+      };
+
+      enum ExtendedCommand {
+                                                                    // length | unit | sealed | unsealed
+                                                                    // -------+------+--------+----------
+        EXT_CMD_DesignCapacity = 0x3Cu, EXT_CMD_DCAP = 0x3Cu,       //      2 |  mAh |     R  |      R
+        EXT_CMD_DataFlashClass = 0x3Eu, EXT_CMD_DFCLS = 0x3Eu,      //      1 |  N/A |    N/A |     R/W
+        EXT_CMD_DataFlashBlock = 0x3Fu, EXT_CMD_DFBLK = 0x3Fu,      //      1 |  N/A |    R/W |     R/W
+        EXT_CMD_BlockData = 0x40u, EXT_CMD_DFD = 0x40u,             //     32 |  N/A |     R  |     R/W
+        EXT_CMD_BlockDataCheckSum = 0x60u,EXT_CMD_DFDCKS = 0x60u,   //      1 |  N/A |    R/W |     R/W
+        EXT_CMD_BlockDataControl = 0x61u, EXT_CMD_DFDCNTL = 0x61u,  //      1 |  N/A |    N/A |     R/W
+        EXT_CMD_DeviceNameLength = 0x62u, EXT_CMD_DNAMELEN = 0x62u, //      1 |  N/A |     R  |      R
+        EXT_CMD_DeviceName = 0x63u, EXT_CMD_DNAME = 0x63u,          //      7 |  N/A |     R  |      R
+        EXT_CMD_ApplicationStatus = 0x6Au, EXT_CMD_APPSTAT = 0x6Au  //      1 |  N/A |     R  |      R
+      };
+
+      enum ExtendedCommandAccess {
+        EXT_CMD_READ = 0,
+        EXT_CMD_WRITE = 1,
+      };
+
+      enum SelftestResult {
+        ST_OK = BaseSensor<>::OK,
+        ST_ABORT_NO_BAT = BaseSensor<>::OK+1,
+        ST_FAIL_ANY = BaseSensor<>::FAIL,
+        ST_FAIL_READ_HW_VERSION = BaseSensor<>::FAIL + 1,
+        ST_FAIL_READ_FW_VERSION = BaseSensor<>::FAIL + 2,
+        ST_FAIL_READ_DEVICENAMELENGTH = BaseSensor<>::FAIL + 3,
+        ST_FAIL_READ_DEVICENAME = BaseSensor<>::FAIL + 4,
+        ST_FAIL_SET_ITENABLE = BaseSensor<>::FAIL + 5,
+        ST_FAIL_READ_STATUS = BaseSensor<>::FAIL + 6,
+        ST_FAIL_READ_FLAGS = BaseSensor<>::FAIL + 7,
+        ST_FAIL_READ_TEMP = BaseSensor<>::FAIL + 8,
+        ST_FAIL_READ_FAC = BaseSensor<>::FAIL + 9,
+        ST_FAIL_READ_FCC = BaseSensor<>::FAIL + 10,
+        ST_FAIL_READ_RM = BaseSensor<>::FAIL + 11,
+        ST_FAIL_READ_SOC = BaseSensor<>::FAIL + 12,
+        ST_FAIL_READ_VOLT = BaseSensor<>::FAIL + 13,
+        ST_FAIL_READ_AI = BaseSensor<>::FAIL + 14,
+        ST_FAIL_READ_AP = BaseSensor<>::FAIL + 15,
+        ST_FAIL_READ_TTE = BaseSensor<>::FAIL + 16,
+        ST_FAIL_READ_TTF = BaseSensor<>::FAIL + 17
+      };
+
+      union ControlStatus {
+        uint16_t value = 0;
+        struct {
+          uint16_t qen : 1;
+          uint16_t vok : 1;
+          uint16_t rup_dis : 1;
+          uint16_t ldmd : 1;
+          uint16_t sleep : 1;
+          uint16_t snooze : 1;
+          uint16_t hibernate : 1;
+          uint16_t rsvd_7_8_9 : 3;
+          uint16_t bca : 1;
+          uint16_t cca : 1;
+          uint16_t csv : 1;
+          uint16_t ss : 1;
+          uint16_t fas : 1;
+          uint16_t rsvd_15 : 1;
+        } content;
+      };
+
+      union Flags {
+        uint16_t value = 0;
+        struct {
+          uint16_t dsg : 1;
+          uint16_t socf : 1;
+          uint16_t soc1 : 1;
+          uint16_t bat_det : 1;
+          uint16_t wait_id : 1;
+          uint16_t ocv_gd : 1;
+          uint16_t rsvd_6_7 : 2;
+          uint16_t chg : 1;
+          uint16_t fc : 1;
+          uint16_t xchg : 1;
+          uint16_t chg_inh : 1;
+          uint16_t rsvd_12_13 : 2;
+          uint16_t otd : 1;
+          uint16_t otc : 1;
+        } content;
+      };
+
+      union Version {
+        uint16_t value = 0;
+        struct {
+          uint16_t minor_low : 4;
+          uint16_t minor_high : 4;
+          uint16_t major_low : 4;
+          uint16_t major_high : 4;
+        } content;
+      };
+
+      struct UpdateData {
+        uint16_t minutes_to_empty;
+        uint16_t minutes_to_full;
+        uint16_t average_power_mW;
+        uint8_t state_of_charge; // %
+      };
+
+    private:
+      enum I2CAddress {
+        I2C_ADDR = 0x55u
+      };
+
+      enum DataFlashSubClassID {
+        CONFIGURATION_Safety = 2,
+        CONFIGURATION_ChargeInhibitConfig = 32,
+        CONFIGURATION_Charge = 34,
+        CONFIGURATION_ChargeTermination = 36,
+        CONFIGURATION_Data = 48,
+        CONFIGURATION_Discharge = 49,
+        SYSTEM_DATA_ManufacturerInfo = 57,
+        CONFIGURATION_Registers = 64,
+        CONFIGURATION_Power = 68,
+        FUEL_GAUGING_ItCfg = 80,
+        FUEL_GAUGING_CurrentThresholds = 81,
+        FUEL_GAUGING_State = 82,
+        DEFAULT_RA_TABLES_Def0Ra = 87,
+        DEFAULT_RA_TABLES_Def1Ra = 88,
+        RA_TABLES_Pack0Ra = 91,
+        RA_TABLES_Pack1Ra = 92,
+        RA_TABLES_Pack0Rax = 93,
+        RA_TABLES_Pack1Rax = 94,
+        CALIBRATION_Data = 104,
+        CALIBRATION_Current = 107,
+        SECURITY_Codes = 112
+      };
+
+      union DataFlashBlock {
+        uint8_t raw[33];
+        struct Content {
+          uint8_t data[32];
+          uint8_t checksum;
+        } content;
+      };
+
+      I2CDriver *const i2c_driver;
+      I2CTxParams tx_params;
+
+      chibios_rt::EvtSource eventSource;
+
+      const GPIO_TypeDef* const batgd_pingrp;
+      const uint8_t batgd_pin;
+      const GPIO_TypeDef* const batlow_pingrp;
+      const uint8_t batlow_pin;
+
+      UpdateData status;
+
+    public:
+      Driver(I2CDriver &i2c_driver, const GPIO_TypeDef &batgd_pingrp, const uint8_t batgd_pin, const GPIO_TypeDef &batlow_pingrp, const uint8_t batlow_pin);
+      virtual ~Driver();
+
+      chibios_rt::EvtSource* getEventSource();
+
+      msg_t init(InitData* initialization_data = NULL);
+      msg_t update();
+      msg_t wakeup();
+      msg_t hibernate();
+  #ifndef AMIRO_NCALIBRATION
+      msg_t calibration(CalibData* calibration_data = NULL);
+  #endif
+  #ifndef AMIRO_NSELFTEST
+      msg_t selftest();
+  #endif
+
+      bool isBatteryGood() const;
+      bool isBatteryLow() const;
+
+      const UpdateData& getStatus() const;
+
+    protected:
+      virtual msg_t main(void);
+
+    private:
+      msg_t stdCommand(const StandardCommand cmd, uint16_t &dst);
+      msg_t readName();
+      msg_t subCommand(const ControlSubcommand cmd, uint16_t* dst = NULL);
+      msg_t extCommand(const ExtendedCommand cmd, const ExtendedCommandAccess rw, uint8_t* buf = NULL, const uint8_t length = 1, const uint8_t offset = 0);
+
+      msg_t readDataFlashBlock(DataFlashBlock &block, const DataFlashSubClassID sub_id, const uint8_t sub_block = 0);
+
+    };
+
+  }
+
+}
+
+#endif /* AMIRO_BQ27500_H_ */
diff --git a/include/amiro/power/ina219.hpp b/include/amiro/power/ina219.hpp
new file mode 100644
index 0000000..b8a3a39
--- /dev/null
+++ b/include/amiro/power/ina219.hpp
@@ -0,0 +1,241 @@
+/*
+ * INA219 - Power Monitor
+ */
+
+#ifndef AMIRO_INA219_H_
+#define AMIRO_INA219_H_
+
+#include <ch.hpp>
+#include <amiro/bus/i2c/I2CParams.hpp>
+#include <amiro/bus/i2c/I2CDriver.hpp>
+
+#include <amiro/BaseSensor.hpp>
+
+namespace amiro {
+
+  namespace INA219 {
+
+    namespace Configuration
+    {
+      union Register {
+        uint16_t value = 0;
+        struct {
+          uint16_t mode : 3;
+          uint16_t sadc : 4;
+          uint16_t badc : 4;
+          uint16_t pg : 2;
+          uint16_t brng : 1;
+          uint16_t rsvd : 1;
+          uint16_t rst : 1;
+        } content;
+      };
+
+      enum BusRange {
+        BRNG_16V = 0,
+        BRNG_32V = 1, BRNG_DEFAULT = 1
+      };
+
+      enum Pga {
+        PGA_40mV = 0,
+        PGA_80mV = 1,
+        PGA_160mV = 2,
+        PGA_320mV = 3, PGA_DEFAULT = 3
+      };
+
+      enum Adc {
+        ADC_84us = 0x0u,
+        ADC_148us = 0x1u,
+        ADC_276us = 0x2u,
+        ADC_532us = 0x3u, ADC_632us_alt = 0x8u, ADC_DEFAULT = 0x3u,
+        ADC_1060us = 0x9u,
+        ADC_2130us = 0xAu,
+        ADC_4260us = 0xBu,
+        ADC_8510us = 0xCu,
+        ADC_17020us = 0xDu,
+        ADC_34050us = 0xEu,
+        ADC_68100us = 0xFu
+      };
+
+      enum Mode {
+        MODE_PowerDown = 0x0u,
+        MODE_ShuntV_Triggered = 0x1u,
+        MODE_BusV_Triggered = 0x2u,
+        MODE_ShuntBus_Triggered = 0x3u,
+        MODE_AdcOff = 0x4u,
+        MODE_ShuntV_Continuous = 0x5u,
+        MODE_BusV_Continuous = 0x6u,
+        MODE_ShuntBus_Continuous = 0x7u, MODE_DEFAULT = 0x7u
+      };
+    } // end of namespace Configuration
+
+
+
+    struct InitData {
+      Configuration::Register configuration;
+      uint16_t calibration;
+      uint16_t current_lsb_uA;
+    };
+
+    struct CalibData {
+      struct CalibInput {
+        float shunt_resistance_O;
+        float max_expected_current_A;
+        uint16_t current_lsb_uA;
+        Configuration::Register configuration;
+      } input;
+      struct CalibOutput {
+        float max_current_before_overflow_A;
+        float max_shuntvoltage_before_overflow_V;
+        uint16_t current_lsb_uA;
+        uint16_t calibration_value;
+      } output;
+
+      uint16_t power_lsb_uW() const
+      {
+        return 20 * output.current_lsb_uA;
+      }
+
+      float maximum_power_W() const
+      {
+        return output.max_current_before_overflow_A * ((input.configuration.content.brng == Configuration::BusRange::BRNG_32V)? 32 : 16);}
+    };
+
+    struct BusVoltage {
+      uint32_t voltage_uV : 25;
+      uint32_t rsvd : 5;
+      uint32_t conversion_ready : 1;
+      uint32_t overflow : 1;
+    };
+
+    class Driver : public BaseSensor<InitData,CalibData>, public chibios_rt::BaseStaticThread<256>
+    {
+    public:
+      enum SelftestResult {
+        ST_OK = BaseSensor<>::OK,
+        ST_FAIL_ANY = BaseSensor<>::FAIL,
+        ST_FAIL_BACKUP = BaseSensor<>::FAIL + 1,
+        ST_FAIL_RESET = BaseSensor<>::FAIL + 2,
+        ST_FAIL_IS_DEFAULT = BaseSensor<>::FAIL + 3,
+        ST_FAIL_WRITE_CONFIG = BaseSensor<>::FAIL + 4,
+        ST_FAIL_WRITE_CALIB = BaseSensor<>::FAIL + 5,
+        ST_FAIL_CHECK_CONFIG = BaseSensor<>::FAIL + 6,
+        ST_FAIL_CHECK_CALIB = BaseSensor<>::FAIL + 7
+      };
+
+    private:
+      union BusVoltage {
+        uint16_t value = 0;
+        struct {
+          uint16_t overflow : 1;
+          uint16_t conversion_ready : 1;
+          uint16_t rsvd : 1;
+          uint16_t voltage : 13;
+        } content;
+      };
+
+      struct Status {
+        Driver::BusVoltage bus_voltage;
+        uint16_t power;
+
+        Status() {bus_voltage.value = 0; power = 0;}
+      };
+
+      enum RegisterAddress {
+        REG_CONFIGURATION = 0x00u,
+        REG_SHUNT_VOLTAGE = 0x01u,
+        REG_BUS_VOLTAGE = 0x02u,
+        REG_POWER = 0x03u,
+        REG_CURRENT = 0x04u,
+        REG_CALIBRATION = 0x05u
+      };
+
+      enum RegisterMask {
+        MASK_CONFIGURATION = 0x3FFFu,
+        MASK_CALIBRATION = 0xFFFEu,
+        MASK_RESET = 0x8000u
+      };
+
+      I2CDriver *const i2c_driver;
+      I2CTxParams tx_params;
+      Configuration::Register config;
+      uint16_t current_lsb_uA;
+      Status status;
+
+    public:
+      /* constructor & destructor */
+      explicit Driver(I2CDriver &i2c_driver, const uint8_t i2c_address);
+      virtual ~Driver();
+
+      /* inherited functions from BaseSensor */
+      msg_t init(InitData* initialization_data = NULL);
+      msg_t update();
+      msg_t wakeup();
+      msg_t hibernate();
+#ifndef AMIRO_NCALIBRATION
+      msg_t calibration(CalibData* calibration_data = NULL);
+#endif
+#ifndef AMIRO_NSELFTEST
+      msg_t selftest();
+#endif
+
+      INA219::BusVoltage getVoltage() const
+      {
+        return this->busVoltageReg2uV(this->status.bus_voltage);
+      }
+
+      uint32_t getPower_uW() const
+      {
+        return this->powerReg2uW(this->status.power);
+      }
+
+      uint32_t getCurrent_uA() const
+      {
+        return this->powerReg2uW(this->status.power) / this->busVoltageReg2uV(this->status.bus_voltage).voltage_uV;
+      }
+
+
+
+      inline Configuration::Register readConfiguration();
+      inline int32_t readShuntVoltage_uV();
+      inline INA219::BusVoltage readBusVoltage();
+      inline uint32_t readPower_uW();
+      inline int32_t readCurrent_uA();
+      inline uint16_t readCalibration();
+
+      uint8_t reset();
+
+  protected:
+      virtual msg_t main(void);
+
+    private:
+      msg_t readRegister(const RegisterAddress reg, uint16_t& dst);
+      msg_t writeRegister(const RegisterAddress reg, const uint16_t& val);
+
+      static inline INA219::BusVoltage busVoltageReg2uV(const INA219::Driver::BusVoltage reg_val)
+      {
+        INA219::BusVoltage bus_voltage;
+        bus_voltage.conversion_ready = reg_val.content.conversion_ready;
+        bus_voltage.overflow = reg_val.content.conversion_ready;
+        /*
+         * The least significant bit represents 4mV.
+         * -> Multiply with 4000 in order to convert to uV.
+         */
+        bus_voltage.voltage_uV = reg_val.content.voltage * 4000;
+        return bus_voltage;
+      }
+
+      inline uint32_t powerReg2uW(const uint16_t reg_val) const
+      {
+        /*
+         * Multiply with the value of the least significant bit for the current and the constant 20.
+         * -> see data sheet, chapter 'Programming the INA219 power measurement engine' for details
+         */
+        return reg_val * 20 * this->current_lsb_uA;
+      }
+    };
+
+  } // end of namespace INA219
+
+}
+
+#endif /* AMIRO_INA219_H_ */
diff --git a/include/amiro/power/ltc4412.hpp b/include/amiro/power/ltc4412.hpp
new file mode 100644
index 0000000..195b23c
--- /dev/null
+++ b/include/amiro/power/ltc4412.hpp
@@ -0,0 +1,98 @@
+/*
+ * LTC4412 - PowerPath Controller
+ */
+
+#ifndef AMIRO_LTC4412_H_
+#define AMIRO_LTC4412_H_
+
+#include <hal.h>
+
+namespace amiro {
+
+class BaseLTC4412
+{
+public:
+  BaseLTC4412()
+  {}
+
+  virtual ~BaseLTC4412()
+  {}
+
+  virtual bool isPluggedIn() const = 0;
+};
+
+class BaseLTC4412wEN : public BaseLTC4412
+{
+public:
+  BaseLTC4412wEN()
+  {}
+
+  virtual ~BaseLTC4412wEN()
+  {}
+
+  virtual void enable(const bool enable = true) = 0;
+
+  virtual bool isEnabled() const = 0;
+
+  virtual bool isPluggedIn() const = 0;
+};
+
+
+
+template <uintptr_t STAT_PadGrp, uint8_t STAT_Pad>
+class LTC4412 : public BaseLTC4412
+{
+public:
+  LTC4412()
+  {}
+
+  virtual ~LTC4412()
+  {}
+
+  bool isPluggedIn() const
+  {
+    return (palReadPad((GPIO_TypeDef*)STAT_PadGrp, STAT_Pad) == PAL_HIGH);
+  }
+
+};
+
+template <uintptr_t STAT_PadGrp, uint8_t STAT_Pad, uintptr_t EN_PadGrp, uint8_t EN_Pad>
+class LTC4412wEN : public BaseLTC4412wEN
+{
+private:
+  bool is_enabled;
+
+public:
+  LTC4412wEN(const bool enable = false) :
+    is_enabled(enable)
+  {
+    this->enable(enable);
+  }
+
+  virtual ~LTC4412wEN()
+  {}
+
+  void enable(const bool enable = true)
+  {
+    // writing to a port is not atomic, thus the system must be locked
+    chSysLock();
+    palWritePad((GPIO_TypeDef*)EN_PadGrp, EN_Pad, enable? PAL_LOW : PAL_HIGH);
+    this->is_enabled = enable;
+    chSysUnlock();
+  }
+
+  bool isEnabled() const
+  {
+    return this->is_enabled;
+  }
+
+  bool isPluggedIn() const
+  {
+    return (palReadPad((GPIO_TypeDef*)STAT_PadGrp, STAT_Pad) == PAL_HIGH);
+  }
+
+};
+
+}
+
+#endif /* AMIRO_LTC4412_H_ */
diff --git a/include/amiro/power/tps62113.hpp b/include/amiro/power/tps62113.hpp
new file mode 100644
index 0000000..cd6989f
--- /dev/null
+++ b/include/amiro/power/tps62113.hpp
@@ -0,0 +1,29 @@
+/*
+ * TPS62113 - StepDown Converter
+ */
+
+#ifndef AMIRO_TPS62113_H_
+#define AMIRO_TPS62113_H_
+
+namespace amiro {
+
+  template <uintptr_t PadGrp, uint32_t Pad>
+  class TPS62113
+  {
+    public:
+    TPS62113()
+    {}
+
+    ~TPS62113()
+    {}
+
+    static void enable(const bool en = true)
+    {
+      palWritePad((GPIO_TypeDef*)PadGrp, Pad, (en? PAL_HIGH : PAL_LOW));
+      return;
+    }
+  };
+
+}
+
+#endif /* AMIRO_TPS62113_H_ */
diff --git a/include/amiro/proximity/vcnl4020.hpp b/include/amiro/proximity/vcnl4020.hpp
new file mode 100644
index 0000000..06c9dfb
--- /dev/null
+++ b/include/amiro/proximity/vcnl4020.hpp
@@ -0,0 +1,236 @@
+#ifndef AMIRO_VCNL4020_H_
+#define AMIRO_VCNL4020_H_
+
+#include <amiro/bus/i2c/I2CParams.hpp>
+
+namespace amiro {
+
+class I2CDriver;
+
+/**
+ * VCNL4020 IR Proximity/Ambient Light Sensor Driver
+ * \todo Interrupt Support
+ */
+class VCNL4020 : public chibios_rt::BaseStaticThread<256> {
+
+    enum { SLA = 0x13u };
+
+    enum { PRODUCT_ID_REVISION = 0x21u };
+
+    struct registers {
+
+      uint8_t  reserved_0x00_0x7F[0x80u];
+      uint8_t  command;
+      uint8_t  revision;
+      uint8_t  proximity_rate;
+      uint8_t  ir_led_current;
+      uint8_t  ambient_param;
+      uint16_t ambient_result; /* BE */
+      uint16_t proximity_result; /* BE */
+      uint8_t  interrupt_ctrl;
+      uint16_t low_thres; /* BE */
+      uint16_t high_thres; /* BE */
+      uint8_t  interrupt_status;
+      uint8_t  proximity_adjust;
+      uint8_t  ambient_ir_level; /* Do Not Use */
+    } __attribute__((packed));
+
+  public:
+
+    struct VCNL4020Config {
+
+      uint8_t command;
+      uint8_t ambient_parameter;
+      uint8_t ir_led_current_mA;
+      uint8_t proximity_rate;
+
+    };
+
+    enum {
+
+      PROX_RATE_1_95    = 0x00u, /* 250 / (2**(7-0)) */
+      PROX_RATE_3_90625 = 0x01u, /* 250 / (2**(7-1)) */
+      PROX_RATE_7_8125  = 0x02u, /* 250 / (2**(7-2)) */
+      PROX_RATE_15_625  = 0x03u, /* 250 / (2**(7-3)) */
+      PROX_RATE_31_25   = 0x04u, /* 250 / (2**(7-4)) */
+      PROX_RATE_62_5    = 0x05u, /* 250 / (2**(7-5)) */
+      PROX_RATE_125     = 0x06u, /* 250 / (2**(7-6)) */
+      PROX_RATE_250     = 0x07u  /* 250 / (2**(7-7)) */
+
+    };
+
+    enum {
+
+      AMBIENT_CONT_CONV   = 0x80u,
+      AMBIENT_RATE_1      = 0x00u,
+      AMBIENT_RATE_2      = 0x10u,
+      AMBIENT_RATE_3      = 0x20u,
+      AMBIENT_RATE_4      = 0x30u,
+      AMBIENT_RATE_5      = 0x40u,
+      AMBIENT_RATE_6      = 0x50u,
+      AMBIENT_RATE_8      = 0x60u,
+      AMBIENT_RATE_10     = 0x70u,
+      AMBIENT_AUTO_OFFSET = 0x10u,
+      AMBIENT_AVG_1       = 0x00u,
+      AMBIENT_AVG_2       = 0x01u,
+      AMBIENT_AVG_4       = 0x02u,
+      AMBIENT_AVG_8       = 0x03u,
+      AMBIENT_AVG_16      = 0x04u,
+      AMBIENT_AVG_32      = 0x05u,
+      AMBIENT_AVG_64      = 0x06u,
+      AMBIENT_AVG_128     = 0x07u,
+
+    };
+
+    enum {
+      INT_THRES_SEL     = 0x01u,
+      INT_THRES_EN      = 0x02u,
+      INT_ALS_READY_EN  = 0x04u,
+      INT_PROX_READY_EN = 0x08u,
+    };
+
+    enum {
+      INT_TH_HI      = 0x01u,
+      INT_TH_LOW     = 0x02u,
+      INT_ALS_READY  = 0x04u,
+      INT_PROX_READY = 0x08u,
+      INT_CNT_1      = 0x00u,
+      INT_CNT_2      = 0x20u,
+      INT_CNT_4      = 0x40u,
+      INT_CNT_8      = 0x60u,
+      INT_CNT_16     = 0x80u,
+      INT_CNT_32     = 0xA0u,
+      INT_CNT_64     = 0xC0u,
+      INT_CNT_128    = 0xE0u,
+    };
+
+    enum {
+
+      MOD_DLY_MASK    = 0x07u,
+      MOD_DLY_SHIFT   = 0x05u,
+      MOD_DEAD_MASK   = 0x07u,
+      MOD_DEAD_SHIFT  = 0x00u,
+      PROX_FREQ_390   = 0x00u,
+      PROX_FREQ_781   = 0x08u,
+      PROX_FREQ_1_562 = 0x10u,
+      PROX_FREQ_3_125 = 0x18u,
+
+    };
+
+    enum {
+
+      CONFIG_LOCK   = 0x80u,
+      ALS_DATA_RDY  = 0x40u,
+      PROX_DATA_RDY = 0x20u,
+      ALS_OD        = 0x10u,
+      PROX_OD       = 0x08u,
+      ALS_EN        = 0x04u,
+      PROX_EN       = 0x02u,
+      SELFTIMED_EN  = 0x01u,
+
+    };
+
+    /**
+     * Return types of getCheck()
+     */
+    enum {
+      CHECK_OK   = 0x00u,
+      CHECK_FAIL = 0x01u,
+    };
+
+    /**
+     * Return types of calibration
+     */
+    enum {
+      CALIB_OK   = 0x00u,
+      CALIB_FAIL = 0x01u,
+    };
+
+  public:
+    VCNL4020(I2CDriver *driver, const VCNL4020Config *config);
+    virtual ~VCNL4020();
+
+    chibios_rt::EvtSource* getEventSource();
+
+    /**
+     * Returns the last measured ambient light value.
+     *
+     * @return Measured ambient light value
+     */
+    uint16_t getAmbientLight();
+
+    /**
+     * Returns the last measured proximity value w/o offset.
+     * Additionally, the value is scaled, so that full-scale
+     * as a measurement can be reached.
+     *
+     * @return Measured proximity value without offset
+     */
+    uint16_t getProximityScaledWoOffset();
+
+    /**
+     * Returns the last measured proximity value.
+     *
+     * @return Measured proximity value
+     */
+    uint16_t getProximity();
+
+    /**
+     * Returns the offset of the vcnl4020.
+     * calibrate() or setOffset() needs to be run first,
+     * otherwise the return value is 0.
+     *
+     * @return Offset value of the device
+     */
+    uint16_t getProximityOffset();
+
+    /**
+     * Sets the offset value, which will be substracted from
+     * the measured value.
+     *
+     * @param offset
+     */
+    void setProximityOffset(uint16_t offset);
+
+    /**
+     * Checks the vcnl4020 by reading the product
+     * id from it an comparing it to the standard
+     * value
+     *
+     * @return [CHECK_OK | CHECK_FAIL]
+     */
+    uint8_t getCheck();
+
+    /**
+     * Starts the calibration of the vcnl4020 and
+     *
+     * @return [ CALIB_OK | CALIB_FAIL]
+     */
+    uint8_t calibrate();
+
+  protected:
+    virtual msg_t main(void);
+
+  private:
+    inline msg_t readIntensities();
+    inline msg_t writeIRConf();
+
+    /**
+     * Starts the offset calibration
+     *
+     * @return [ CALIB_OK | CALIB_FAIL]
+     */
+    uint8_t calibrateOffset(uint16_t &proximityFloorMeanValue);
+
+    I2CDriver *driver;
+    const VCNL4020Config *config;
+    chibios_rt::EvtSource eventSource;
+    uint16_t ambient;
+    uint16_t proximity;
+    uint16_t proximityOffset;
+    I2CTxParams tx_params;
+};
+
+}
+
+#endif /* AMIRO_VCNL4020_H_ */
diff --git a/include/amiro/radio/a2500r24a.hpp b/include/amiro/radio/a2500r24a.hpp
new file mode 100644
index 0000000..32fe696
--- /dev/null
+++ b/include/amiro/radio/a2500r24a.hpp
@@ -0,0 +1,164 @@
+#ifndef A2500R24A_HPP_
+#define A2500R24A_HPP_
+
+#include <ch.hpp>
+
+namespace amiro {
+
+class HWSPIDriver;
+
+class A2500R24A : public chibios_rt::BaseStaticThread<256> {
+
+ public:
+
+  struct A2500R24AConfig {
+  };
+
+  /**
+   * Return types of getCheck()
+   */
+  enum {
+    CHECK_OK = 0x00u,
+    CHECK_FAIL = 0x01u,
+  };
+
+  //private:
+
+  struct registers {
+    uint8_t IOCFG2;
+    uint8_t IOCFG1;
+    uint8_t IOCFG0;
+    uint8_t FIFOTHR;
+    uint8_t SYNC1;
+    uint8_t SYNC0;
+    uint8_t PKTLEN;
+    uint8_t PKTCTRL1;
+    uint8_t PKTCTRL0;
+    uint8_t ADDR;
+    uint8_t CHANNR;
+    uint8_t FSCTRL1;
+    uint8_t FSCTRL0;
+    uint8_t FREQ2;
+    uint8_t FREQ1;
+    uint8_t FREQ0;
+    uint8_t MDMCFG4;
+    uint8_t MDMCFG3;
+    uint8_t MDMCFG2;
+    uint8_t MDMCFG1;
+    uint8_t MDMCFG0;
+    uint8_t DEVIATN;
+    uint8_t MCSM2;
+    uint8_t MCSM1;
+    uint8_t MCSM0;
+    uint8_t FOCCFG;
+    uint8_t BSCFG;
+    uint8_t AGCCTRL2;
+    uint8_t AGCCTRL1;
+    uint8_t AGCCTRL0;
+    uint8_t WOREVT1;
+    uint8_t WOREVT0;
+    uint8_t WORCTRL;
+    uint8_t FREND1;
+    uint8_t FREND0;
+    uint8_t FSCAL3;
+    uint8_t FSCAL2;
+    uint8_t FSCAL1;
+    uint8_t FSCAL0;
+    uint8_t RCCTRL1;
+    uint8_t RCCTRL0;
+    uint8_t FSTEST;
+    uint8_t PTEST;
+    uint8_t AGCTEST;
+    uint8_t TEST2;
+    uint8_t TEST1;
+    uint8_t TEST0;
+    uint8_t reserved_0x2F;
+  }__attribute__((packed));
+
+  struct registerStrobe {
+    uint8_t configurationRegister[offsetof(A2500R24A::registers, reserved_0x2F)+1];
+    uint8_t SRES;
+    uint8_t SFSTXON;
+    uint8_t SXOFF;
+    uint8_t SCAL;
+    uint8_t SRX;
+    uint8_t STX;
+    uint8_t SIDLE;
+    uint8_t reserved_0x37;
+    uint8_t SWOR;
+    uint8_t SPWD;
+    uint8_t SFRX;
+    uint8_t SFTX;
+    uint8_t SWORRST;
+    uint8_t SNOP;
+    uint8_t PATABLE;
+    uint8_t RX_TX_FIFO;
+  }__attribute__((packed));
+
+  struct registerStatus {
+    uint8_t configurationRegister[offsetof(A2500R24A::registers, reserved_0x2F)+1];
+    uint8_t PARTNUM;
+    uint8_t VERSION;
+    uint8_t FREQEST;
+    uint8_t LQI;
+    uint8_t RSSI;
+    uint8_t MARCSTATE;
+    uint8_t WORTIME1;
+    uint8_t WORTIME0;
+    uint8_t PKTSTATUS;
+    uint8_t VCO_VC_DAC;
+    uint8_t TXBYTES;
+    uint8_t RXBYTES;
+    uint8_t RCCTRL1_STATUS;
+    uint8_t RCCTRL0_STATUS;
+  }__attribute__((packed));
+
+  enum {
+    A2500R24A_PARTNUM = 0x80u,
+    A2500R24A_VERSION = 0x03u,
+  };
+
+  enum {
+    // For register access below address 0x30 the burst mode is available
+    // to read multiple bytes
+    SPI_BURST_ENA     = 0x40u,
+    SPI_BURST_DIS     = 0x00u,
+    // For addresses above (and including) 0x30 either the status or strobe access
+    // has to be choosen, instead of burst mode
+    SPI_STATUS_ACCESS = 0x40u,
+    SPI_STROBE_ACCESS = 0x00u,
+    // Read or write the register address
+    SPI_READ      = 0x80u,
+    SPI_WRITE     = 0x00u,
+  };
+
+ public:
+  A2500R24A(HWSPIDriver* driver);
+  virtual ~A2500R24A();
+
+  chibios_rt::EvtSource* getEventSource();
+  msg_t configure(A2500R24AConfig* config);
+
+  /**
+   * Check the presence of the radio chip by reading
+   * the identifier register and comparing it to the standard
+   * value
+   */
+  uint8_t getCheck();
+
+ protected:
+  virtual msg_t main();
+
+ private:
+  inline void updateSensorData();
+
+ private:
+
+  HWSPIDriver* driver;
+  chibios_rt::EvtSource eventSource;
+
+};
+
+} /* amiro */
+
+#endif /* A2500R24A_HPP_ */
diff --git a/include/amiro/serial_reset/iwrap_can_mux.hpp b/include/amiro/serial_reset/iwrap_can_mux.hpp
new file mode 100644
index 0000000..e9b50b6
--- /dev/null
+++ b/include/amiro/serial_reset/iwrap_can_mux.hpp
@@ -0,0 +1,81 @@
+#ifndef IWRAP_CAN_MUX_HPP_
+#define IWRAP_CAN_MUX_HPP_
+
+#include <ch.hpp>
+
+#define BLUETOOTH_START_FRAME   0xBF
+/** \brief Define bluetooth link address to iWRAP control */
+#define BLUETOOTH_LINK          0x00
+/** \brief Define bluetooth frame flags */
+#define BLUETOOTH_FLAGS         0x00
+/** \brief Define nLINK = {LINK} XOR 0xFF */
+#define BLUETOOTH_NLINK         0xFF
+
+/* States of bluetooth communication */
+/** \brief bluetooth waiting for start frame signal */
+#define BLUETOOTH_STATUS_START    0
+/** \brief bluetooth waiting for link address */
+#define BLUETOOTH_STATUS_LINK     1
+/** \brief bluetooth waiting for frame flags */
+#define BLUETOOTH_STATUS_FLAGS    2
+/** \brief bluetooth waiting for data length */
+#define BLUETOOTH_STATUS_LENGTH   3
+/** \brief bluetooth waiting for data */
+#define BLUETOOTH_STATUS_DATA     4
+/** \brief bluetooth waiting for frame end (nLINK) */
+#define BLUETOOTH_STATUS_END      5
+
+#define OFFSET_TIME_RESET_MS                2000
+#define INPUT_BL_RESET_ADDITIONAL_BYTES     0
+#define INPUT_BL_RESET_LENGTH               3
+
+namespace amiro {
+
+  class IwrapCanMux : public chibios_rt::BaseSequentialStreamInterface {
+  public:
+    IwrapCanMux(SerialDriver *_sd_, CANDriver *can, const uint8_t boardId);
+    virtual ~IwrapCanMux();
+
+    void convCan2Serial(uint8_t *inputs, size_t n);
+    void sendSwitchCmd(uint8_t setid);
+    void rcvSwitchCmd(uint8_t setid);
+
+    size_t write(const uint8_t *bp, size_t n);
+    size_t read(uint8_t *bp, size_t n);
+    msg_t put(uint8_t b);
+    msg_t get(void);
+
+  protected:
+
+
+  private:
+    /* Reset command. */
+    const uint8_t inputBLReset[3] = {0x02, 0xFF, 0x00};
+//    const uint8_t inputchshell[3] = {0x00, 0x68, 0x0D};
+//    const uint8_t inputsh_board[8] = {0x73, 0x68, 0x5F, 0x62, 0x6F, 0x61, 0x72, 0x6C};
+
+//    void checkBluetoothStatus(void)
+    void restartSystem(void);
+    bool checkByteForBLReset(uint8_t *inputs, size_t size);
+    msg_t sendViaCan(uint32_t id, uint8_t *inputs, size_t size);
+
+    uint8_t inputAddBytes[INPUT_BL_RESET_ADDITIONAL_BYTES];    // Buffer for additional bytes.
+    uint8_t resetStatus;      // Status of reset
+    uint8_t resetAddBytes;    // Counter of additional bytes
+    uint8_t bluetoothStatus;
+
+    SerialDriver *sd_;
+    CANDriver *canDriver;
+    uint8_t myID;
+    volatile uint8_t queryShellID;
+    volatile uint8_t replyShellID;
+
+    CANTxFrame txmsg;
+    uint8_t inputChar[16] = {};
+  };
+
+} /* amiro */
+
+#endif /* IWRAP_CAN_MUX_HPP_ */
+
+
diff --git a/include/amiro/serial_reset/serial_can_mux.hpp b/include/amiro/serial_reset/serial_can_mux.hpp
new file mode 100644
index 0000000..b19849d
--- /dev/null
+++ b/include/amiro/serial_reset/serial_can_mux.hpp
@@ -0,0 +1,56 @@
+#ifndef SERIAL_CAN_MUX_HPP_
+#define SERIAL_CAN_MUX_HPP_
+
+#include <ch.hpp>
+
+#define OFFSET_TIME_RESET_MS                2000
+#define INPUT_BL_RESET_ADDITIONAL_BYTES     0
+#define INPUT_BL_RESET_LENGTH               3
+
+namespace amiro {
+
+  class SerialCanMux : public chibios_rt::BaseSequentialStreamInterface {
+  public:
+    SerialCanMux(SerialDriver *_sd_, CANDriver *can, const uint8_t boardId);
+    virtual ~SerialCanMux();
+
+    void convCan2Serial(uint8_t *inputs, size_t n);
+    void sendSwitchCmd(uint8_t setid);
+    void rcvSwitchCmd(uint8_t setid);
+
+    size_t write(const uint8_t *bp, size_t n);
+    size_t read(uint8_t *bp, size_t n);
+    msg_t put(uint8_t b);
+    msg_t get(void);
+
+  protected:
+
+  private:
+    /* Reset command. */
+    const uint8_t inputBLReset[3] = {0x02, 0xFF, 0x00};
+//    const uint8_t inputchshell[3] = {0x00, 0x68, 0x0D};
+//    const uint8_t inputsh_board[8] = {0x73, 0x68, 0x5F, 0x62, 0x6F, 0x61, 0x72, 0x6C};
+
+    void restartSystem(void);
+    bool checkByteForBLReset(uint8_t *inputs, size_t size);
+    msg_t sendViaCan(uint32_t id, uint8_t *inputs, size_t size);
+
+    uint8_t inputAddBytes[INPUT_BL_RESET_ADDITIONAL_BYTES];    // Buffer for additional bytes.
+    uint8_t resetStatus;      // Status of reset
+    uint8_t resetAddBytes;    // Counter of additional bytes
+
+    SerialDriver *sd_;
+    CANDriver *canDriver;
+    uint8_t myID;
+    volatile uint8_t queryShellID;
+    volatile uint8_t replyShellID;
+
+    CANTxFrame txmsg;
+    uint8_t inputChar[16] = {};
+  };
+
+} /* amiro */
+
+#endif /* SERIAL_CAN_MUX_HPP_ */
+
+
diff --git a/include/amiro/util/rwlock.hpp b/include/amiro/util/rwlock.hpp
new file mode 100644
index 0000000..a6263fe
--- /dev/null
+++ b/include/amiro/util/rwlock.hpp
@@ -0,0 +1,105 @@
+#ifndef AMIRO_RWLOCK_H_
+#define AMIRO_RWLOCK_H_
+
+#include <ch.hpp>
+
+namespace amiro {
+
+/**
+ * @brief An enhanced mutex with parallel read but exclusive write capabilities
+ */
+class RWLock
+{
+public:
+  /**
+   * @brief An enum to specify the favoritism of the RWLock
+   */
+  enum Favor : bool {
+    FAVOR_READERS = false, /**< Favors readers, even if writers are waiting */
+    FAVOR_WRITERS = true,  /**< Favors writers and holds new readers back */
+  };
+
+  /**
+   * @brief An enum to specify what to wait for in the tryLockReading() and tryLockWriting() methods.
+   */
+  enum WaitingType : uint8_t {
+    WAIT_FOR_NONE = 0,  /**< does not block at all */
+    WAIT_FOR_MUTEX = 1, /**< blocks at the mutex */
+    WAIT_FOR_QUEUE = 2, /**< waits for the queues */
+  };
+
+private:
+  /**
+   * @brief A status struct holding the current configuration of the RWLock.
+   */
+  struct AccessStatus {
+    uint8_t reader_count : 7; /**< The number of currently active readers (128 max) */
+    uint8_t favor        : 1; /**< The currently selected favoritism */
+  };
+
+  chibios_rt::Mutex mutex;             /**< A mutex lock used for synchronization */
+  chibios_rt::CondVar waiting_readers; /**< A condition variable for waiting readers */
+  chibios_rt::CondVar waiting_writers; /**< A condition variable for all waiting writers */
+  AccessStatus status;                 /**< The current status of the RWLock */
+
+public:
+  /**
+   * @brief Constructor of the RWLock class.
+   * @param[in] favor The initial favoritism (default: FAVOR_WRITERS)
+   */
+  explicit RWLock(const Favor favor = FAVOR_WRITERS);
+
+  /**
+   * @brief Destructor of the RWLock class.
+   */
+  virtual ~RWLock();
+
+  /**
+   * @brief Lock the RWLock as a reader (shared mode)
+   */
+  void lockReading();
+
+  /**
+   * @brief Lock the RWLock as a writer (exclusive mode)
+   */
+  void lockWriting();
+
+  /**
+   * @brief Try to lock the RWLock as a reader (shared mode) but do not block
+   * @param[in] wait Can be used to wait for the mutex to lock or the queue to be empty
+   * @return True if the RWLock was successfully locked; False if not
+   */
+  bool tryLockReading(const WaitingType wait = WAIT_FOR_NONE);
+
+  /**
+   * @brief Try to lock the RWLock as a writer (exclusive mode) but do not block
+   * @param[in] wait Can be used to wait for the mutex to lock or the queue to be empty
+   * @return True if the RWLock was successfully locked; False if not
+   */
+  bool tryLockWriting(const WaitingType wait = WAIT_FOR_NONE);
+
+  /**
+   * @brief Unlocks the RWLock.
+   * If it was locked in shared or exclusive mode is detected automatically.
+   */
+  void unlock();
+
+  /**
+   * @brief Set the favoritism of the RWLock
+   * @param[in] favor Either FAVOR_READERS or FAVOR_WRITERS
+   */
+  void setFavoritism(const Favor favor);
+
+  /**
+   * @brief Try to set the favoritism of the RWLock but do not block
+   * @param[in] favor Either FAVOR_READERS or FAVOR_WRITERS
+   * @return True if the favoritism was successfully set; False if not
+   */
+  bool trySetFavoritism(const Favor favor);
+
+};
+
+} // end of namespace amiro
+
+#endif // AMIRO_RWLOCK_H_
+
diff --git a/include/amiro/util/stringstream.h b/include/amiro/util/stringstream.h
new file mode 100644
index 0000000..7b258f2
--- /dev/null
+++ b/include/amiro/util/stringstream.h
@@ -0,0 +1,30 @@
+#ifndef STRINGSTREAM_H_
+#define STRINGSTREAM_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct BaseSequentialStreamVMT;
+
+struct stringstream_t {
+  const struct BaseSequentialStreamVMT *vmt;
+  uint8_t string[256];
+  uint8_t ins_ix;
+  uint8_t out_ix;
+};
+
+void StringStreamInit(void *instance);
+size_t StringStreamWrite(void *instance, const uint8_t *bp, size_t n);
+size_t StringStreamRead(void *instance, uint8_t *bp, size_t n);
+msg_t StringStreamPut(void *instance, uint8_t b);
+msg_t StringStreamGet(void *instance);
+size_t StringStreamTellp(void *instance);
+uint8_t *StringStreamStrGet(void *instance);
+void StringStreamStrSet(void *instance, const char *str);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* STRINGSTREAM_H_ */
diff --git a/include/amiro/util/util.h b/include/amiro/util/util.h
new file mode 100644
index 0000000..e3070fe
--- /dev/null
+++ b/include/amiro/util/util.h
@@ -0,0 +1,15 @@
+/*
+ * util.h
+ *
+ *  Created on: Jun 18, 2014
+ *      Author: Robert ABEL
+ */
+
+#ifndef UTIL_H_
+#define UTIL_H_
+
+#define ARRAY_SIZE(_X_) (sizeof(_X_) / sizeof((_X_)[0]))
+
+#define MEMBER_SIZE(_TYPE_, _MEMBER_) sizeof(((_TYPE_ *)0)->_MEMBER_)
+
+#endif /* UTIL_H_ */
diff --git a/license.html b/license.html
new file mode 100644
index 0000000..33edf21
--- /dev/null
+++ b/license.html
@@ -0,0 +1,695 @@
+<!DOCTYPE html PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<!-- saved from url=(0051)http://www.gnu.org/licenses/gpl-3.0-standalone.html -->
+<html><head><meta http-equiv="Content-Type" content="text/html; charset=UTF-8">
+ 
+ <title>GNU General Public License v3.0 - GNU Project - Free Software Foundation (FSF)</title>
+ <link rel="alternate" type="application/rdf+xml" href="http://www.gnu.org/licenses/gpl-3.0.rdf"> 
+</head>
+<body>
+
+<h3 style="text-align: center;">GNU GENERAL PUBLIC LICENSE</h3>
+<p style="text-align: center;">Version 3, 29 June 2007</p>
+
+<p>Copyright © 2007 Free Software Foundation, Inc.
+ &lt;<a href="http://fsf.org/">http://fsf.org/</a>&gt;</p><p>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.</p>
+
+<h3><a name="preamble"></a>Preamble</h3>
+
+<p>The GNU General Public License is a free, copyleft license for
+software and other kinds of works.</p>
+
+<p>The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.  We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors.  You can apply it to
+your programs, too.</p>
+
+<p>When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.</p>
+
+<p>To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights.  Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.</p>
+
+<p>For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received.  You must make sure that they, too, receive
+or can get the source code.  And you must show them these terms so they
+know their rights.</p>
+
+<p>Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.</p>
+
+<p>For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software.  For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.</p>
+
+<p>Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so.  This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software.  The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable.  Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products.  If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.</p>
+
+<p>Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary.  To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.</p>
+
+<p>The precise terms and conditions for copying, distribution and
+modification follow.</p>
+
+<h3><a name="terms"></a>TERMS AND CONDITIONS</h3>
+
+<h4><a name="section0"></a>0. Definitions.</h4>
+
+<p>“This License” refers to version 3 of the GNU General Public License.</p>
+
+<p>“Copyright” also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.</p>
+ 
+<p>“The Program” refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as “you”.  “Licensees” and
+“recipients” may be individuals or organizations.</p>
+
+<p>To “modify” a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a “modified version” of the
+earlier work or a work “based on” the earlier work.</p>
+
+<p>A “covered work” means either the unmodified Program or a work based
+on the Program.</p>
+
+<p>To “propagate” a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.</p>
+
+<p>To “convey” a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.</p>
+
+<p>An interactive user interface displays “Appropriate Legal Notices”
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.</p>
+
+<h4><a name="section1"></a>1. Source Code.</h4>
+
+<p>The “source code” for a work means the preferred form of the work
+for making modifications to it.  “Object code” means any non-source
+form of a work.</p>
+
+<p>A “Standard Interface” means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.</p>
+
+<p>The “System Libraries” of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+“Major Component”, in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.</p>
+
+<p>The “Corresponding Source” for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.</p>
+
+<p>The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.</p>
+
+<p>The Corresponding Source for a work in source code form is that
+same work.</p>
+
+<h4><a name="section2"></a>2. Basic Permissions.</h4>
+
+<p>All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.</p>
+
+<p>You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.</p>
+
+<p>Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.</p>
+
+<h4><a name="section3"></a>3. Protecting Users' Legal Rights From Anti-Circumvention Law.</h4>
+
+<p>No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.</p>
+
+<p>When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.</p>
+
+<h4><a name="section4"></a>4. Conveying Verbatim Copies.</h4>
+
+<p>You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.</p>
+
+<p>You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.</p>
+
+<h4><a name="section5"></a>5. Conveying Modified Source Versions.</h4>
+
+<p>You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:</p>
+
+<ul>
+<li>a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.</li>
+
+<li>b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    “keep intact all notices”.</li>
+
+<li>c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.</li>
+
+<li>d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.</li>
+</ul>
+
+<p>A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+“aggregate” if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.</p>
+
+<h4><a name="section6"></a>6. Conveying Non-Source Forms.</h4>
+
+<p>You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:</p>
+
+<ul>
+<li>a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.</li>
+
+<li>b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.</li>
+
+<li>c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.</li>
+
+<li>d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.</li>
+
+<li>e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.</li>
+</ul>
+
+<p>A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.</p>
+
+<p>A “User Product” is either (1) a “consumer product”, which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, “normally used” refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.</p>
+
+<p>“Installation Information” for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.</p>
+
+<p>If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).</p>
+
+<p>The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.</p>
+
+<p>Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.</p>
+
+<h4><a name="section7"></a>7. Additional Terms.</h4>
+
+<p>“Additional permissions” are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.</p>
+
+<p>When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.</p>
+
+<p>Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:</p>
+
+<ul>
+<li>a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or</li>
+
+<li>b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or</li>
+
+<li>c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or</li>
+
+<li>d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or</li>
+
+<li>e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or</li>
+
+<li>f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.</li>
+</ul>
+
+<p>All other non-permissive additional terms are considered “further
+restrictions” within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.</p>
+
+<p>If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.</p>
+
+<p>Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.</p>
+
+<h4><a name="section8"></a>8. Termination.</h4>
+
+<p>You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).</p>
+
+<p>However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.</p>
+
+<p>Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.</p>
+
+<p>Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.</p>
+
+<h4><a name="section9"></a>9. Acceptance Not Required for Having Copies.</h4>
+
+<p>You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.</p>
+
+<h4><a name="section10"></a>10. Automatic Licensing of Downstream Recipients.</h4>
+
+<p>Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.</p>
+
+<p>An “entity transaction” is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.</p>
+
+<p>You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.</p>
+
+<h4><a name="section11"></a>11. Patents.</h4>
+
+<p>A “contributor” is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's “contributor version”.</p>
+
+<p>A contributor's “essential patent claims” are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, “control” includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.</p>
+
+<p>Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.</p>
+
+<p>In the following three paragraphs, a “patent license” is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To “grant” such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.</p>
+
+<p>If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  “Knowingly relying” means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.</p>
+  
+<p>If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.</p>
+
+<p>A patent license is “discriminatory” if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.</p>
+
+<p>Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.</p>
+
+<h4><a name="section12"></a>12. No Surrender of Others' Freedom.</h4>
+
+<p>If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.</p>
+
+<h4><a name="section13"></a>13. Use with the GNU Affero General Public License.</h4>
+
+<p>Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.</p>
+
+<h4><a name="section14"></a>14. Revised Versions of this License.</h4>
+
+<p>The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.</p>
+
+<p>Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU General
+Public License “or any later version” applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.</p>
+
+<p>If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.</p>
+
+<p>Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.</p>
+
+<h4><a name="section15"></a>15. Disclaimer of Warranty.</h4>
+
+<p>THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM “AS IS” WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.</p>
+
+<h4><a name="section16"></a>16. Limitation of Liability.</h4>
+
+<p>IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.</p>
+
+<h4><a name="section17"></a>17. Interpretation of Sections 15 and 16.</h4>
+
+<p>If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.</p>
+
+<p>END OF TERMS AND CONDITIONS</p>
+
+<h3><a name="howto"></a>How to Apply These Terms to Your New Programs</h3>
+
+<p>If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.</p>
+
+<p>To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the “copyright” line and a pointer to where the full notice is found.</p>
+
+<pre>    &lt;one line to give the program's name and a brief idea of what it does.&gt;
+    Copyright (C) &lt;year&gt;  &lt;name of author&gt;
+
+    This program 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 3 of the License, or
+    (at your option) any later version.
+
+    This program 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 this program.  If not, see &lt;http://www.gnu.org/licenses/&gt;.
+</pre>
+
+<p>Also add information on how to contact you by electronic and paper mail.</p>
+
+<p>If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:</p>
+
+<pre>    &lt;program&gt;  Copyright (C) &lt;year&gt;  &lt;name of author&gt;
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+</pre>
+
+<p>The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, your program's commands
+might be different; for a GUI interface, you would use an “about box”.</p>
+
+<p>You should also get your employer (if you work as a programmer) or school,
+if any, to sign a “copyright disclaimer” for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+&lt;<a href="http://www.gnu.org/licenses/">http://www.gnu.org/licenses/</a>&gt;.</p>
+
+<p>The GNU General Public License does not permit incorporating your program
+into proprietary programs.  If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.  But first, please read
+&lt;<a href="http://www.gnu.org/philosophy/why-not-lgpl.html">http://www.gnu.org/philosophy/why-not-lgpl.html</a>&gt;.</p>
+
+
+</body></html>
diff --git a/patches/0001-ARM-fix-rounding-issue-with-WORKING_AREA-stack-size.patch b/patches/0001-ARM-fix-rounding-issue-with-WORKING_AREA-stack-size.patch
new file mode 100644
index 0000000..65b8518
--- /dev/null
+++ b/patches/0001-ARM-fix-rounding-issue-with-WORKING_AREA-stack-size.patch
@@ -0,0 +1,40 @@
+From 8dc86a6495b16e656d5fff4d55b4b6f78e3b3c82 Mon Sep 17 00:00:00 2001
+From: Robert ABEL <rabel@cit-ec.uni-bielefeld.de>
+Date: Fri, 4 Jul 2014 16:51:30 +0200
+Subject: [PATCH] ARM: fix rounding issue with WORKING_AREA stack size
+
+Signed-off-by: Robert ABEL <rabel@cit-ec.uni-bielefeld.de>
+---
+ os/ports/GCC/ARMCMx/chcore_v6m.h | 2 +-
+ os/ports/GCC/ARMCMx/chcore_v7m.h | 2 +-
+ 2 files changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/os/ports/GCC/ARMCMx/chcore_v6m.h b/os/ports/GCC/ARMCMx/chcore_v6m.h
+index be1381e..2937be9 100644
+--- a/os/ports/GCC/ARMCMx/chcore_v6m.h
++++ b/os/ports/GCC/ARMCMx/chcore_v6m.h
+@@ -238,7 +238,7 @@ struct context {
+  * @details This macro is used to allocate a static thread working area
+  *          aligned as both position and size.
+  */
+-#define WORKING_AREA(s, n) stkalign_t s[THD_WA_SIZE(n) / sizeof(stkalign_t)]
++#define WORKING_AREA(s, n) stkalign_t s[(THD_WA_SIZE(n) + sizeof(stkalign_t) - 1) / sizeof(stkalign_t)]
+ 
+ /**
+  * @brief   IRQ prologue code.
+diff --git a/os/ports/GCC/ARMCMx/chcore_v7m.h b/os/ports/GCC/ARMCMx/chcore_v7m.h
+index 3706a92..5cb8867 100644
+--- a/os/ports/GCC/ARMCMx/chcore_v7m.h
++++ b/os/ports/GCC/ARMCMx/chcore_v7m.h
+@@ -338,7 +338,7 @@ struct context {
+  * @details This macro is used to allocate a static thread working area
+  *          aligned as both position and size.
+  */
+-#define WORKING_AREA(s, n) stkalign_t s[THD_WA_SIZE(n) / sizeof(stkalign_t)]
++#define WORKING_AREA(s, n) stkalign_t s[(THD_WA_SIZE(n) + sizeof(stkalign_t) - 1) / sizeof(stkalign_t)]
+ 
+ /**
+  * @brief   IRQ prologue code.
+-- 
+1.8.4.msysgit.0
+
diff --git a/patches/0006-STM32F4xx-ADC-add-AWD-support.patch b/patches/0006-STM32F4xx-ADC-add-AWD-support.patch
new file mode 100644
index 0000000..a05480d
--- /dev/null
+++ b/patches/0006-STM32F4xx-ADC-add-AWD-support.patch
@@ -0,0 +1,101 @@
+From: Thomas SCHÖPPING <tschoepp@techfak.uni-bielefeld.de>
+Date: Wed, 3 Mar 2015 15:43:17 +0200
+Subject: [PATCH] STM32F4xx: added support for the ADC analog watchdog
+
+Signed-off-by: Thomas SCHÖPPING <tschoepp@techfak.uni-bielefeld.de>
+---
+diff --git a/os/hal/platforms/STM32F4xx/adc_lld.c b/os/hal/platforms/STM32F4xx/adc_lld.c
+index e5de44a..5495649 100644
+--- a/os/hal/platforms/STM32F4xx/adc_lld.c
++++ b/os/hal/platforms/STM32F4xx/adc_lld.c
+@@ -124,7 +124,10 @@ CH_IRQ_HANDLER(ADC1_2_3_IRQHandler) {
+     if (ADCD1.grpp != NULL)
+       _adc_isr_error_code(&ADCD1, ADC_ERR_OVERFLOW);
+   }
+-  /* TODO: Add here analog watchdog handling.*/
++  if (sr & ADC_SR_AWD) {
++    if (ADCD1.grpp != NULL)
++      _adc_isr_error_code(&ADCD1, ADC_ERR_WATCHDOG);
++  }
+ #endif /* STM32_ADC_USE_ADC1 */
+ 
+ #if STM32_ADC_USE_ADC2
+@@ -138,7 +141,10 @@ CH_IRQ_HANDLER(ADC1_2_3_IRQHandler) {
+     if (ADCD2.grpp != NULL)
+       _adc_isr_error_code(&ADCD2, ADC_ERR_OVERFLOW);
+   }
+-  /* TODO: Add here analog watchdog handling.*/
++  if (sr & ADC_SR_AWD) {
++    if (ADCD1.grpp != NULL)
++      _adc_isr_error_code(&ADCD2, ADC_ERR_WATCHDOG);
++  }
+ #endif /* STM32_ADC_USE_ADC2 */
+ 
+ #if STM32_ADC_USE_ADC3
+@@ -152,7 +158,10 @@ CH_IRQ_HANDLER(ADC1_2_3_IRQHandler) {
+     if (ADCD3.grpp != NULL)
+       _adc_isr_error_code(&ADCD3, ADC_ERR_OVERFLOW);
+   }
+-  /* TODO: Add here analog watchdog handling.*/
++  if (sr & ADC_SR_AWD) {
++    if (ADCD1.grpp != NULL)
++      _adc_isr_error_code(&ADCD3, ADC_ERR_WATCHDOG);
++  }
+ #endif /* STM32_ADC_USE_ADC3 */
+ 
+   CH_IRQ_EPILOGUE();
+@@ -340,6 +349,8 @@ void adc_lld_start_conversion(ADCDriver *adcp) {
+   adcp->adc->SR    = 0;
+   adcp->adc->SMPR1 = grpp->smpr1;
+   adcp->adc->SMPR2 = grpp->smpr2;
++  adcp->adc->HTR   = grpp->htr;
++  adcp->adc->LTR   = grpp->ltr;
+   adcp->adc->SQR1  = grpp->sqr1;
+   adcp->adc->SQR2  = grpp->sqr2;
+   adcp->adc->SQR3  = grpp->sqr3;
+diff --git a/os/hal/platforms/STM32F4xx/adc_lld.h b/os/hal/platforms/STM32F4xx/adc_lld.h
+index 7cda791..be81ed3 100644
+--- a/os/hal/platforms/STM32F4xx/adc_lld.h
++++ b/os/hal/platforms/STM32F4xx/adc_lld.h
+@@ -306,7 +306,8 @@ typedef uint16_t adc_channels_num_t;
+  */
+ typedef enum {
+   ADC_ERR_DMAFAILURE = 0,                   /**< DMA operations failure.    */
+-  ADC_ERR_OVERFLOW = 1                      /**< ADC overflow condition.    */
++  ADC_ERR_OVERFLOW = 1,                     /**< ADC overflow condition.    */
++  ADC_ERR_WATCHDOG = 2                      /**< ADC watchdog condition.    */
+ } adcerror_t;
+ 
+ /**
+@@ -385,6 +386,16 @@ typedef struct {
+    */
+   uint32_t                  smpr2;
+   /**
++   * @brief   ADC watchdog higher threshold register.
++   * @details This filed defines the higher threshold for the analog watchdog.
++   */
++  uint16_t                  htr;
++  /**
++   * @brief   ADC watchdog lower threshold register.
++   * @details This filed defines the lower threshold for the analog watchdog.
++   */
++  uint16_t                  ltr;
++  /**
+    * @brief   ADC SQR1 register initialization data.
+    * @details Conversion group sequence 13...16 + sequence length.
+    */
+@@ -528,6 +539,14 @@ struct ADCDriver {
+ #define ADC_SMPR1_SMP_VBAT(n)   ((n) << 24) /**< @brief VBAT sampling time. */
+ /** @} */
+ 
++/**
++ * @name    Threshold settings helper macros
++ * @{
++ */
++#define ADC_HTR(n)              ((n > ADC_HTR_HT) ? ADC_HTR_HT : n) /**< @brief higher threshold limitation */
++#define ADC_LTR(n)              ((n > ADC_LTR_LT) ? ADC_LTR_LT : n) /**< @brief lower threshold limitation  */
++/** @} */
++
+ /*===========================================================================*/
+ /* External declarations.                                                    */
+ /*===========================================================================*/
diff --git a/patches/0007-SMT32-add-optional-I2C-non-DMA-driver-option.patch b/patches/0007-SMT32-add-optional-I2C-non-DMA-driver-option.patch
new file mode 100644
index 0000000..f720c1b
--- /dev/null
+++ b/patches/0007-SMT32-add-optional-I2C-non-DMA-driver-option.patch
@@ -0,0 +1,507 @@
+From: Thomas SCHÖPPING <tschoepp@techfak.uni-bielefeld.de>
+Date: Wed, 29 Apr 2015 18:15:23 +0200
+Subject: [PATCH] STM32/I2Cv1: added the option to use I2C without DMA
+
+Signed-off-by: Thomas SCHÖPPING <tschoepp@techfak.uni-bielefeld.de>
+---
+diff --git a/os/hal/platforms/STM32/I2Cv1/i2c_lld.c b/os/hal/platforms/STM32/I2Cv1/i2c_lld.c
+index 2a36776..4a27942 100644
+--- a/os/hal/platforms/STM32/I2Cv1/i2c_lld.c
++++ b/os/hal/platforms/STM32/I2Cv1/i2c_lld.c
+@@ -35,6 +35,7 @@
+ /* Driver local definitions.                                                 */
+ /*===========================================================================*/
+ 
++#if STM32_I2C_USE_DMA
+ #define I2C1_RX_DMA_CHANNEL                                                 \
+   STM32_DMA_GETCHANNEL(STM32_I2C_I2C1_RX_DMA_STREAM,                        \
+                        STM32_I2C1_RX_DMA_CHN)
+@@ -58,6 +59,7 @@
+ #define I2C3_TX_DMA_CHANNEL                                                 \
+   STM32_DMA_GETCHANNEL(STM32_I2C_I2C3_TX_DMA_STREAM,                        \
+                        STM32_I2C3_TX_DMA_CHN)
++#endif /* STM32_I2C_USE_DMA */
+ 
+ /*===========================================================================*/
+ /* Driver constants.                                                         */
+@@ -73,6 +75,20 @@
+ #define I2C_EV6_MASTER_REC_MODE_SELECTED                                    \
+   ((uint32_t)(((I2C_SR2_MSL | I2C_SR2_BUSY)<< 16) | I2C_SR1_ADDR))
+ 
++#define I2C_EV7_MASTER_REC_BYTE_RECEIVED                                    \
++  ((uint32_t)(((I2C_SR2_MSL | I2C_SR2_BUSY)<< 16) | I2C_SR1_RXNE))
++
++#define I2C_EV7_MASTER_REC_BYTE_RECEIVED_STOP                               \
++  ((uint32_t)(                                      I2C_SR1_RXNE))
++
++#define I2C_EV7_2_EV7_3_MASTER_REC_BYTE_QUEUED                              \
++  ((uint32_t)(((I2C_SR2_MSL | I2C_SR2_BUSY)<< 16) |                         \
++              I2C_SR1_BTF | I2C_SR1_RXNE))
++
++#define I2C_EV8_MASTER_BYTE_TRANSMITTING                                    \
++  ((uint32_t)(((I2C_SR2_MSL | I2C_SR2_BUSY | I2C_SR2_TRA)<< 16) |           \
++              I2C_SR1_TXE))
++
+ #define I2C_EV8_2_MASTER_BYTE_TRANSMITTED                                   \
+   ((uint32_t)(((I2C_SR2_MSL | I2C_SR2_BUSY | I2C_SR2_TRA) << 16) |          \
+               I2C_SR1_BTF | I2C_SR1_TXE))
+@@ -148,9 +164,11 @@ static void i2c_lld_abort_operation(I2CDriver *i2cp) {
+   dp->CR2 = 0;
+   dp->SR1 = 0;
+ 
++#if STM32_I2C_USE_DMA
+   /* Stops the associated DMA streams.*/
+   dmaStreamDisable(i2cp->dmatx);
+   dmaStreamDisable(i2cp->dmarx);
++#endif /* STM32_I2C_USE_DMA */
+ }
+ 
+ /**
+@@ -242,7 +260,7 @@ static void i2c_lld_set_clock(I2CDriver *i2cp) {
+ 
+     chDbgAssert(clock_div >= 0x01,
+                     "i2c_lld_set_clock(), #7",
+-                    "Clock divider less then 0x04 not allowed");
++                    "Clock divider less then 0x01 not allowed");
+     regCCR |= (I2C_CCR_FS | (clock_div & I2C_CCR_CCR));
+ 
+     /* Sets the Maximum Rise Time for fast mode.*/
+@@ -295,12 +313,13 @@ static void i2c_lld_serve_event_interrupt(I2CDriver *i2cp) {
+   uint32_t regSR2 = dp->SR2;
+   uint32_t event = dp->SR1;
+ 
++#if STM32_I2C_USE_DMA
+   /* Interrupts are disabled just before dmaStreamEnable() because there
+      is no need of interrupts until next transaction begin. All the work is
+      done by the DMA.*/
+   switch (I2C_EV_MASK & (event | (regSR2 << 16))) {
+   case I2C_EV5_MASTER_MODE_SELECT:
+-    if ((i2cp->addr >> 8) > 0) { 
++    if ((i2cp->addr >> 8) > 0) {
+       /* 10-bit address: 1 1 1 1 0 X X R/W */
+       dp->DR = 0xF0 | (0x6 & (i2cp->addr >> 8)) | (0x1 & i2cp->addr);
+     } else {
+@@ -340,8 +359,129 @@ static void i2c_lld_serve_event_interrupt(I2CDriver *i2cp) {
+   /* Clear ADDR flag. */
+   if (event & (I2C_SR1_ADDR | I2C_SR1_ADD10))
+     (void)dp->SR2;
++#else
++  switch (I2C_EV_MASK & (event | (regSR2 << 16))) {
++  case I2C_EV5_MASTER_MODE_SELECT:
++    dp->CR2 |= I2C_CR2_ITBUFEN;
++    dp->DR = i2cp->addr;
++    break;
++  case I2C_EV6_MASTER_TRA_MODE_SELECTED:
++    (void)dp->SR2; // clear ADDR flag
++    /* EV8_1 */
++    dp->DR = *(i2cp->txbuf);
++
++    ++i2cp->txbuf;
++    --i2cp->txbytes;
++
++    /* if N == 1, skip the I2C_EV8_MASTER_BYTE_TRANSMITTING event
++     * but enter I2C_EV8_2_MASTER_BYTE_TRANSMITTED next */
++    if (i2cp->txbytes == 0) {
++      dp->CR2 &= ~I2C_CR2_ITBUFEN;
++    }
++    break;
++  case I2C_EV6_MASTER_REC_MODE_SELECTED:
++    switch (i2cp->rxbytes) {
++    case 1:
++      dp->CR1 &= ~I2C_CR1_ACK;
++      (void)dp->SR2; // clear ADDR flag
++      dp->CR1 |= I2C_CR1_STOP;
++      break;
++    case 2:
++      (void)dp->SR2; // clear ADDR flag
++      /* EV6_1 */
++      dp->CR1 |= I2C_CR1_POS;
++      dp->CR1 &= ~I2C_CR1_ACK;
++      dp->CR2 &= ~I2C_CR2_ITBUFEN;
++      break;
++    case 3: /* N == 3 is a very special case, since EV7 is completely skipped */
++      (void)dp->SR2; // clear ADDR flag
++      /* Disable the I2C_EV7_MASTER_REC_BYTE_RECEIVED event
++       * but enter I2C_EV7_MASTER_REC_BYTE_RECEIVED_STOP next */
++      dp->CR2 &= ~I2C_CR2_ITBUFEN;
++      break;
++    default: /* N > 2 */
++      (void)dp->SR2; // clear ADDR flag
++      break;
++    }
++    break;
++  case I2C_EV7_MASTER_REC_BYTE_RECEIVED:
++    if (i2cp->rxbytes > 3) {
++      *(i2cp->rxbuf) = dp->DR;
++      ++i2cp->rxbuf;
++      --i2cp->rxbytes;
++    }
++    if (i2cp->rxbytes == 3) {
++      /* Disable this event for DataN-2, but force into event
++       * I2C_EV7_2_EV7_3_MASTER_REC_BYTE_RECEIVED_QUEUED by not reading dp->DR. */
++      dp->CR2 &= ~I2C_CR2_ITBUFEN;
++    }
++    break;
++  case I2C_EV7_MASTER_REC_BYTE_RECEIVED_STOP:
++    chDbgAssert(i2cp->rxbytes == 1, "i2c_lld_serve_event_interrupt(), #1",
++               "more than 1 byte to be received");
++    *(i2cp->rxbuf) = dp->DR;
++    --i2cp->rxbytes;
++    dp->CR2 &= ~(I2C_CR2_ITEVTEN | I2C_CR2_ITBUFEN);
++    wakeup_isr(i2cp, RDY_OK);
++    break;
++  case I2C_EV7_2_EV7_3_MASTER_REC_BYTE_QUEUED:
++    if (i2cp->rxbytes == 3) {
++      /* EV7_2 (N > 2) */
++      dp->CR1 &= ~I2C_CR1_ACK;
++      *(i2cp->rxbuf) = dp->DR;
++      ++i2cp->rxbuf;
++      dp->CR1 |= I2C_CR1_STOP;
++      *(i2cp->rxbuf) = dp->DR;
++      ++i2cp->rxbuf;
++      i2cp->rxbytes -= 2;
++      /* enable I2C_EV7_MASTER_REC_BYTE_RECEIVED_STOP event */
++      dp->CR2 |= I2C_CR2_ITBUFEN;
++    } else {
++      /* EV7_3 (N == 2) */
++      dp->CR1 |= I2C_CR1_STOP;
++      *(i2cp->rxbuf) = dp->DR;
++      ++i2cp->rxbuf;
++      *(i2cp->rxbuf) = dp->DR;
++      i2cp->rxbytes -= 2;
++
++      dp->CR1 &= ~I2C_CR1_POS;
++      dp->CR2 &= ~(I2C_CR2_ITEVTEN | I2C_CR2_ITBUFEN);
++
++      wakeup_isr(i2cp, RDY_OK);
++    }
++    break;
++  case I2C_EV8_MASTER_BYTE_TRANSMITTING:
++    dp->DR = *(i2cp->txbuf);
++    ++i2cp->txbuf;
++    --i2cp->txbytes;
++
++    /* if this was the last byte, ensure that this event is not entered again */
++    if (i2cp->txbytes == 0) {
++      dp->CR2 &= ~I2C_CR2_ITBUFEN;
++    }
++    break;
++  case I2C_EV8_2_MASTER_BYTE_TRANSMITTED:
++    if (i2cp->rxbytes > 0) {
++      /* start "read after write" operation (LSB of address = 1 -> read) */
++      i2cp-> addr |= 0x01;
++      dp->CR1 |= I2C_CR1_START | I2C_CR1_ACK;
++    } else {
++      dp->CR1 |= I2C_CR1_STOP;
++
++      dp->CR2 &= ~(I2C_CR2_ITEVTEN | I2C_CR2_ITBUFEN);
++
++      wakeup_isr(i2cp, RDY_OK);
++    }
++    break;
++  default:
++    chDbgAssert(i2cp->rxbytes != 1, "i2c_lld_serve_event_interrupt(), #1",
++               "more than 1 byte to be received");
++    break;
++  }
++#endif /* STM32_I2C_USE_DMA */
+ }
+ 
++#if STM32_I2C_USE_DMA
+ /**
+  * @brief   DMA RX end IRQ handler.
+  *
+@@ -395,6 +535,7 @@ static void i2c_lld_serve_tx_end_irq(I2CDriver *i2cp, uint32_t flags) {
+      of R/W transaction itself.*/
+   dp->CR2 |= I2C_CR2_ITEVTEN;
+ }
++#endif /* STM32_I2C_USE_DMA */
+ 
+ /**
+  * @brief   I2C error handler.
+@@ -406,9 +547,11 @@ static void i2c_lld_serve_tx_end_irq(I2CDriver *i2cp, uint32_t flags) {
+  */
+ static void i2c_lld_serve_error_interrupt(I2CDriver *i2cp, uint16_t sr) {
+ 
++#if STM32_I2C_USE_DMA
+   /* Clears interrupt flags just to be safe.*/
+   dmaStreamDisable(i2cp->dmatx);
+   dmaStreamDisable(i2cp->dmarx);
++#endif /* STM32_I2C_USE_DMA */
+ 
+   i2cp->errors = I2CD_NO_ERROR;
+ 
+@@ -554,24 +697,30 @@ void i2c_lld_init(void) {
+   i2cObjectInit(&I2CD1);
+   I2CD1.thread = NULL;
+   I2CD1.i2c    = I2C1;
++#if STM32_I2C_USE_DMA
+   I2CD1.dmarx  = STM32_DMA_STREAM(STM32_I2C_I2C1_RX_DMA_STREAM);
+   I2CD1.dmatx  = STM32_DMA_STREAM(STM32_I2C_I2C1_TX_DMA_STREAM);
++#endif /* STM32_I2C_USE_DMA */
+ #endif /* STM32_I2C_USE_I2C1 */
+ 
+ #if STM32_I2C_USE_I2C2
+   i2cObjectInit(&I2CD2);
+   I2CD2.thread = NULL;
+   I2CD2.i2c    = I2C2;
++#if STM32_I2C_USE_DMA
+   I2CD2.dmarx  = STM32_DMA_STREAM(STM32_I2C_I2C2_RX_DMA_STREAM);
+   I2CD2.dmatx  = STM32_DMA_STREAM(STM32_I2C_I2C2_TX_DMA_STREAM);
++#endif /* STM32_I2C_USE_DMA */
+ #endif /* STM32_I2C_USE_I2C2 */
+ 
+ #if STM32_I2C_USE_I2C3
+   i2cObjectInit(&I2CD3);
+   I2CD3.thread = NULL;
+   I2CD3.i2c    = I2C3;
++#if STM32_I2C_USE_DMA
+   I2CD3.dmarx  = STM32_DMA_STREAM(STM32_I2C_I2C3_RX_DMA_STREAM);
+   I2CD3.dmatx  = STM32_DMA_STREAM(STM32_I2C_I2C3_TX_DMA_STREAM);
++#endif /* STM32_I2C_USE_DMA */
+ #endif /* STM32_I2C_USE_I2C3 */
+ }
+ 
+@@ -585,6 +734,7 @@ void i2c_lld_init(void) {
+ void i2c_lld_start(I2CDriver *i2cp) {
+   I2C_TypeDef *dp = i2cp->i2c;
+ 
++#if STM32_I2C_USE_DMA
+   i2cp->txdmamode = STM32_DMA_CR_PSIZE_BYTE | STM32_DMA_CR_MSIZE_BYTE |
+                     STM32_DMA_CR_MINC       | STM32_DMA_CR_DMEIE |
+                     STM32_DMA_CR_TEIE       | STM32_DMA_CR_TCIE |
+@@ -593,15 +743,17 @@ void i2c_lld_start(I2CDriver *i2cp) {
+                     STM32_DMA_CR_MINC       | STM32_DMA_CR_DMEIE |
+                     STM32_DMA_CR_TEIE       | STM32_DMA_CR_TCIE |
+                     STM32_DMA_CR_DIR_P2M;
++#endif /* STM32_I2C_USE_DMA */
+ 
+   /* If in stopped state then enables the I2C and DMA clocks.*/
+   if (i2cp->state == I2C_STOP) {
+ 
+ #if STM32_I2C_USE_I2C1
+     if (&I2CD1 == i2cp) {
+-      bool_t b;
+ 
+       rccResetI2C1();
++#if STM32_I2C_USE_DMA
++      bool_t b;
+       b = dmaStreamAllocate(i2cp->dmarx,
+                             STM32_I2C_I2C1_IRQ_PRIORITY,
+                             (stm32_dmaisr_t)i2c_lld_serve_rx_end_irq,
+@@ -612,24 +764,28 @@ void i2c_lld_start(I2CDriver *i2cp) {
+                             (stm32_dmaisr_t)i2c_lld_serve_tx_end_irq,
+                             (void *)i2cp);
+       chDbgAssert(!b, "i2c_lld_start(), #2", "stream already allocated");
++#endif /* STM32_I2C_USE_DMA */
+       rccEnableI2C1(FALSE);
+       nvicEnableVector(I2C1_EV_IRQn,
+           CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
+       nvicEnableVector(I2C1_ER_IRQn,
+           CORTEX_PRIORITY_MASK(STM32_I2C_I2C1_IRQ_PRIORITY));
+ 
++#if STM32_I2C_USE_DMA
+       i2cp->rxdmamode |= STM32_DMA_CR_CHSEL(I2C1_RX_DMA_CHANNEL) |
+                        STM32_DMA_CR_PL(STM32_I2C_I2C1_DMA_PRIORITY);
+       i2cp->txdmamode |= STM32_DMA_CR_CHSEL(I2C1_TX_DMA_CHANNEL) |
+                        STM32_DMA_CR_PL(STM32_I2C_I2C1_DMA_PRIORITY);
++#endif /* STM32_I2C_USE_DMA */
+     }
+ #endif /* STM32_I2C_USE_I2C1 */
+ 
+ #if STM32_I2C_USE_I2C2
+     if (&I2CD2 == i2cp) {
+-      bool_t b;
+ 
+       rccResetI2C2();
++#if STM32_I2C_USE_DMA
++      bool_t b;
+       b = dmaStreamAllocate(i2cp->dmarx,
+                             STM32_I2C_I2C2_IRQ_PRIORITY,
+                             (stm32_dmaisr_t)i2c_lld_serve_rx_end_irq,
+@@ -640,24 +796,28 @@ void i2c_lld_start(I2CDriver *i2cp) {
+                             (stm32_dmaisr_t)i2c_lld_serve_tx_end_irq,
+                             (void *)i2cp);
+       chDbgAssert(!b, "i2c_lld_start(), #4", "stream already allocated");
++#endif /* STM32_I2C_USE_DMA */
+       rccEnableI2C2(FALSE);
+       nvicEnableVector(I2C2_EV_IRQn,
+           CORTEX_PRIORITY_MASK(STM32_I2C_I2C2_IRQ_PRIORITY));
+       nvicEnableVector(I2C2_ER_IRQn,
+           CORTEX_PRIORITY_MASK(STM32_I2C_I2C2_IRQ_PRIORITY));
+ 
++#if STM32_I2C_USE_DMA
+       i2cp->rxdmamode |= STM32_DMA_CR_CHSEL(I2C2_RX_DMA_CHANNEL) |
+                        STM32_DMA_CR_PL(STM32_I2C_I2C2_DMA_PRIORITY);
+       i2cp->txdmamode |= STM32_DMA_CR_CHSEL(I2C2_TX_DMA_CHANNEL) |
+                        STM32_DMA_CR_PL(STM32_I2C_I2C2_DMA_PRIORITY);
++#endif /* STM32_I2C_USE_DMA */
+     }
+ #endif /* STM32_I2C_USE_I2C2 */
+ 
+ #if STM32_I2C_USE_I2C3
+     if (&I2CD3 == i2cp) {
+-      bool_t b;
+ 
+       rccResetI2C3();
++#if STM32_I2C_USE_DMA
++      bool_t b;
+       b = dmaStreamAllocate(i2cp->dmarx,
+                             STM32_I2C_I2C3_IRQ_PRIORITY,
+                             (stm32_dmaisr_t)i2c_lld_serve_rx_end_irq,
+@@ -668,28 +828,37 @@ void i2c_lld_start(I2CDriver *i2cp) {
+                             (stm32_dmaisr_t)i2c_lld_serve_tx_end_irq,
+                             (void *)i2cp);
+       chDbgAssert(!b, "i2c_lld_start(), #6", "stream already allocated");
++#endif /* STM32_I2C_USE_DMA */
+       rccEnableI2C3(FALSE);
+       nvicEnableVector(I2C3_EV_IRQn,
+           CORTEX_PRIORITY_MASK(STM32_I2C_I2C3_IRQ_PRIORITY));
+       nvicEnableVector(I2C3_ER_IRQn,
+           CORTEX_PRIORITY_MASK(STM32_I2C_I2C3_IRQ_PRIORITY));
+ 
++#if STM32_I2C_USE_DMA
+       i2cp->rxdmamode |= STM32_DMA_CR_CHSEL(I2C3_RX_DMA_CHANNEL) |
+                        STM32_DMA_CR_PL(STM32_I2C_I2C3_DMA_PRIORITY);
+       i2cp->txdmamode |= STM32_DMA_CR_CHSEL(I2C3_TX_DMA_CHANNEL) |
+                        STM32_DMA_CR_PL(STM32_I2C_I2C3_DMA_PRIORITY);
++#endif /* STM32_I2C_USE_DMA */
+     }
+ #endif /* STM32_I2C_USE_I2C3 */
+   }
+ 
++#if STM32_I2C_USE_DMA
+   /* I2C registers pointed by the DMA.*/
+   dmaStreamSetPeripheral(i2cp->dmarx, &dp->DR);
+   dmaStreamSetPeripheral(i2cp->dmatx, &dp->DR);
++#endif /* STM32_I2C_USE_DMA */
+ 
+   /* Reset i2c peripheral.*/
+   dp->CR1 = I2C_CR1_SWRST;
+   dp->CR1 = 0;
++#if STM32_I2C_USE_DMA
+   dp->CR2 = I2C_CR2_ITERREN | I2C_CR2_DMAEN;
++#else
++  dp->CR2 = I2C_CR2_ITERREN;
++#endif /* STM32_I2C_USE_DMA */
+ 
+   /* Setup I2C parameters.*/
+   i2c_lld_set_clock(i2cp);
+@@ -713,8 +882,10 @@ void i2c_lld_stop(I2CDriver *i2cp) {
+ 
+     /* I2C disable.*/
+     i2c_lld_abort_operation(i2cp);
++#if STM32_I2C_USE_DMA
+     dmaStreamRelease(i2cp->dmatx);
+     dmaStreamRelease(i2cp->dmarx);
++#endif /* STM32_I2C_USE_DMA */
+ 
+ #if STM32_I2C_USE_I2C1
+     if (&I2CD1 == i2cp) {
+@@ -786,10 +957,15 @@ msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
+   i2cp->addr = (addr << 1) | 0x01;
+   i2cp->errors = 0;
+ 
++#if STM32_I2C_USE_DMA
+   /* RX DMA setup.*/
+   dmaStreamSetMode(i2cp->dmarx, i2cp->rxdmamode);
+   dmaStreamSetMemory0(i2cp->dmarx, rxbuf);
+   dmaStreamSetTransactionSize(i2cp->dmarx, rxbytes);
++#else
++  i2cp->rxbuf = rxbuf;
++  i2cp->rxbytes = rxbytes;
++#endif /* STM32_I2C_USE_DMA */
+ 
+   /* Waits until BUSY flag is reset and the STOP from the previous operation
+      is completed, alternatively for a timeout condition.*/
+@@ -869,6 +1045,7 @@ msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
+   i2cp->addr = addr << 1;
+   i2cp->errors = 0;
+ 
++#if STM32_I2C_USE_DMA
+   /* TX DMA setup.*/
+   dmaStreamSetMode(i2cp->dmatx, i2cp->txdmamode);
+   dmaStreamSetMemory0(i2cp->dmatx, txbuf);
+@@ -878,6 +1055,13 @@ msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
+   dmaStreamSetMode(i2cp->dmarx, i2cp->rxdmamode);
+   dmaStreamSetMemory0(i2cp->dmarx, rxbuf);
+   dmaStreamSetTransactionSize(i2cp->dmarx, rxbytes);
++#else
++  i2cp->txbuf = txbuf;
++  i2cp->txbytes = txbytes;
++
++  i2cp->rxbuf = rxbuf;
++  i2cp->rxbytes = rxbytes;
++#endif /* STM32_I2C_USE_DMA */
+ 
+   /* Waits until BUSY flag is reset and the STOP from the previous operation
+      is completed, alternatively for a timeout condition.*/
+diff --git a/os/hal/platforms/STM32/I2Cv1/i2c_lld.h b/os/hal/platforms/STM32/I2Cv1/i2c_lld.h
+index 6b192dc..27b1263 100644
+--- a/os/hal/platforms/STM32/I2Cv1/i2c_lld.h
++++ b/os/hal/platforms/STM32/I2Cv1/i2c_lld.h
+@@ -76,6 +76,15 @@
+ #endif
+ 
+ /**
++ * @brief   I2C data transfer use dma switch.
++ * @details If set to @p TRUE the support for I2C DMA is included.
++ * @note    The default is @p FALSE.
++ */
++#if !defined(STM32_I2C_USE_DMA) || defined(__DOXYGEN__)
++#define STM32_I2C_USE_DMA               TRUE
++#endif
++
++/**
+  * @brief   I2C1 interrupt priority level setting.
+  */
+ #if !defined(STM32_I2C_I2C1_IRQ_PRIORITY) || defined(__DOXYGEN__)
+@@ -227,6 +236,7 @@
+ #error "I2C driver activated but no I2C peripheral assigned"
+ #endif
+ 
++#if STM32_I2C_USE_DMA
+ #if STM32_I2C_USE_I2C1 &&                                                   \
+     !STM32_DMA_IS_VALID_ID(STM32_I2C_I2C1_RX_DMA_STREAM,                    \
+                            STM32_I2C1_RX_DMA_MSK)
+@@ -266,6 +276,7 @@
+ #if !defined(STM32_DMA_REQUIRED)
+ #define STM32_DMA_REQUIRED
+ #endif
++#endif /* STM32_I2C_USE_DMA */
+ 
+ /* Check clock range. */
+ #if defined(STM32F4XX)
+@@ -386,6 +397,7 @@ struct I2CDriver {
+    * @brief     Current slave address without R/W bit.
+    */
+   i2caddr_t                 addr;
++#if STM32_I2C_USE_DMA
+   /**
+    * @brief RX DMA mode bit mask.
+    */
+@@ -402,6 +414,24 @@ struct I2CDriver {
+    * @brief     Transmit DMA channel.
+    */
+   const stm32_dma_stream_t  *dmatx;
++#else
++  /**
++   * @brief     Receive buffer.
++   */
++  uint8_t                   *rxbuf;
++  /**
++   * @brief     Receive buffer size.
++   */
++  size_t                    rxbytes;
++  /**
++   * @brief     Transmit buffer.
++   */
++   const uint8_t            *txbuf;
++  /**
++   * @brief     Transmit buffer size.
++   */
++  size_t                    txbytes;
++#endif /* STM32_I2C_USE_DMA */
+   /**
+    * @brief     Pointer to the I2Cx registers block.
+    */
diff --git a/ports/STM32F103xE.ld b/ports/STM32F103xE.ld
new file mode 100644
index 0000000..a07f77a
--- /dev/null
+++ b/ports/STM32F103xE.ld
@@ -0,0 +1,136 @@
+/*
+ * ST32F103xE memory setup.
+ */
+__main_stack_size__     = 0x0400;
+__process_stack_size__  = 0x0400;
+
+MEMORY
+{
+    flash : org = 0x08006000, len = 512k-24k
+    ram : org = 0x20000000, len = 64k
+}
+
+__ram_start__           = ORIGIN(ram);
+__ram_size__            = LENGTH(ram);
+__ram_end__             = __ram_start__ + __ram_size__;
+
+__crc_start__		= ORIGIN(flash) + 0x150;
+
+ENTRY(ResetHandler)
+
+SECTIONS
+{
+    . = 0;
+    _text = .;
+
+    startup : ALIGN(16) SUBALIGN(16)
+    {
+        KEEP(*(vectors))
+    } > flash
+
+    crc __crc_start__ :
+    {
+        LONG(0x55aa11ee);
+    } > flash
+
+    constructors : ALIGN(4) SUBALIGN(4)
+    {
+        PROVIDE(__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE(__init_array_end = .);
+    } > flash
+
+    destructors : ALIGN(4) SUBALIGN(4)
+    {
+        PROVIDE(__fini_array_start = .);
+        KEEP(*(.fini_array))
+        KEEP(*(SORT(.fini_array.*)))
+        PROVIDE(__fini_array_end = .);
+    } > flash
+
+    .text : ALIGN(16) SUBALIGN(16)
+    {
+        *(.text.startup.*)
+        *(.text)
+        *(.text.*)
+        *(.rodata)
+        *(.rodata.*)
+        *(.glue_7t)
+        *(.glue_7)
+        *(.gcc*)
+    } > flash
+
+    .ARM.extab :
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > flash
+
+    .ARM.exidx : {
+        PROVIDE(__exidx_start = .);
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+        PROVIDE(__exidx_end = .);
+     } > flash
+
+    .eh_frame_hdr :
+    {
+        *(.eh_frame_hdr)
+    } > flash
+
+    .eh_frame : ONLY_IF_RO
+    {
+        *(.eh_frame)
+    } > flash
+    
+    .textalign : ONLY_IF_RO
+    {
+        . = ALIGN(8);
+    } > flash
+
+    . = ALIGN(4);
+    _etext = .;
+    _textdata = _etext;
+
+    .stacks :
+    {
+        . = ALIGN(8);
+        __main_stack_base__ = .;
+        . += __main_stack_size__;
+        . = ALIGN(8);
+        __main_stack_end__ = .;
+        __process_stack_base__ = .;
+        __main_thread_stack_base__ = .;
+        . += __process_stack_size__;
+        . = ALIGN(8);
+        __process_stack_end__ = .;
+        __main_thread_stack_end__ = .;
+    } > ram
+
+    .data ALIGN(4) : ALIGN(4)
+    {
+        . = ALIGN(4);
+        PROVIDE(_data = .);
+        *(.data)
+        *(.data.*)
+        *(.ramtext)
+        . = ALIGN(4);
+        PROVIDE(_edata = .);
+    } > ram AT > flash
+
+    .bss ALIGN(4) : ALIGN(4)
+    {
+        . = ALIGN(4);
+        PROVIDE(_bss_start = .);
+        *(.bss)
+        *(.bss.*)
+        *(COMMON)
+        . = ALIGN(4);
+        PROVIDE(_bss_end = .);
+    } > ram    
+}
+
+PROVIDE(end = .);
+_end            = .;
+
+__heap_base__   = _end;
+__heap_end__    = __ram_end__;
diff --git a/ports/STM32F405xG.ld b/ports/STM32F405xG.ld
new file mode 100644
index 0000000..aced1ee
--- /dev/null
+++ b/ports/STM32F405xG.ld
@@ -0,0 +1,160 @@
+/*
+ * ST32F405xG memory setup.
+ */
+__main_stack_size__     = 0x0400;
+__process_stack_size__  = 0x0400;
+
+MEMORY
+{
+    flash : org = 0x08008000, len = 1M-32k
+    ram : org = 0x20000000, len = 112k
+    ethram : org = 0x2001C000, len = 16k
+    ccmram : org = 0x10000000, len = 64k
+}
+
+__ram_start__           = ORIGIN(ram);
+__ram_size__            = LENGTH(ram);
+__ram_end__             = __ram_start__ + __ram_size__;
+
+__crc_start__		= ORIGIN(flash) + 0x1AC;
+
+ENTRY(ResetHandler)
+
+SECTIONS
+{
+    . = 0;
+    _text = .;
+
+    startup : ALIGN(16) SUBALIGN(16)
+    {
+        KEEP(*(vectors))
+    } > flash
+
+    crc __crc_start__ :
+    {
+        LONG(0x55AA11EE);
+    } > flash
+
+    constructors : ALIGN(4) SUBALIGN(4)
+    {
+        PROVIDE(__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE(__init_array_end = .);
+    } > flash
+
+    destructors : ALIGN(4) SUBALIGN(4)
+    {
+        PROVIDE(__fini_array_start = .);
+        KEEP(*(.fini_array))
+        KEEP(*(SORT(.fini_array.*)))
+        PROVIDE(__fini_array_end = .);
+    } > flash
+
+    .text : ALIGN(16) SUBALIGN(16)
+    {
+        *(.text.startup.*)
+        *(.text)
+        *(.text.*)
+        *(.rodata)
+        *(.rodata.*)
+        *(.glue_7t)
+        *(.glue_7)
+        *(.gcc*)
+    } > flash
+
+    .ARM.extab :
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > flash
+
+    .ARM.exidx : {
+        PROVIDE(__exidx_start = .);
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+        PROVIDE(__exidx_end = .);
+     } > flash
+
+    .eh_frame_hdr :
+    {
+        *(.eh_frame_hdr)
+    } > flash
+
+    .eh_frame : ONLY_IF_RO
+    {
+        *(.eh_frame)
+    } > flash
+    
+    .textalign : ONLY_IF_RO
+    {
+        . = ALIGN(8);
+    } > flash
+
+    . = ALIGN(4);
+    _etext = .;
+    _textdata = _etext;
+
+    .ccm (NOLOAD):
+    {
+        PROVIDE(_ccm_start = .);
+        . = ALIGN(4);
+        *(.ccm)
+        . = ALIGN(4);
+        *(.ccm.*)
+        . = ALIGN(4);
+        PROVIDE(_ccm_end = .);
+    } > ccmram
+
+    .stacks :
+    {
+        . = ALIGN(8);
+        __main_stack_base__ = .;
+        . += __main_stack_size__;
+        . = ALIGN(8);
+        __main_stack_end__ = .;
+        __process_stack_base__ = .;
+        __main_thread_stack_base__ = .;
+        . += __process_stack_size__;
+        . = ALIGN(8);
+        __process_stack_end__ = .;
+        __main_thread_stack_end__ = .;
+    } > ram
+
+    .data ALIGN(4) : ALIGN(4)
+    {
+        . = ALIGN(4);
+        PROVIDE(_data = .);
+        *(.data)
+        *(.data.*)
+        *(.ramtext)
+        . = ALIGN(4);
+        PROVIDE(_edata = .);
+    } > ram AT > flash
+
+    .bss ALIGN(4) : ALIGN(4)
+    {
+        . = ALIGN(4);
+        PROVIDE(_bss_start = .);
+        *(.bss)
+        *(.bss.*)
+        *(COMMON)
+        . = ALIGN(4);
+        PROVIDE(_bss_end = .);
+    } > ram
+
+    .eth (NOLOAD):
+    {
+        PROVIDE(_dma_start = .);
+        . = ALIGN(4);
+        *(.eth)
+        . = ALIGN(4);
+        *(.eth.*)
+        . = ALIGN(4);
+        PROVIDE(_dma_end = .);
+    } > ethram
+}
+
+PROVIDE(end = .);
+_end            = .;
+
+__heap_base__   = _end;
+__heap_end__    = __ram_end__;
diff --git a/ports/rules.mk b/ports/rules.mk
new file mode 100644
index 0000000..45dde0a
--- /dev/null
+++ b/ports/rules.mk
@@ -0,0 +1,26 @@
+# ARM Cortex-Mx common makefile scripts and rules.
+
+##############################################################################
+# Processing options coming from the upper Makefile.
+#
+
+# Output directory and files
+SRECFILE = $(BUILDDIR)/$(PROJECT).srec
+
+#
+# Makefile rules
+#
+
+blt: $(SRECFILE)
+
+MAKE_ALL_RULE_HOOK: blt
+
+%.srec: %.elf $(LDSCRIPT)
+ifeq ($(USE_VERBOSE_COMPILE),yes)
+	$(SREC) $< $@
+else
+	@echo Creating $@
+	@$(SREC) $< $@
+endif
+
+# *** EOF ***
diff --git a/stubs.c b/stubs.c
new file mode 100644
index 0000000..f0b9564
--- /dev/null
+++ b/stubs.c
@@ -0,0 +1,26 @@
+#include <errno.h>
+#undef errno
+extern int errno;
+
+/* libc stub */
+__attribute__((used)) void *__dso_handle = 0;
+
+/* libc stub */
+__attribute__((used)) int _getpid(void) {
+  return 1;
+}
+
+/* libc stub */
+__attribute__((used)) void _exit(int i) {
+  (void)i;
+  while(1);
+}
+
+/* libc stub */
+
+__attribute__((used)) int _kill(int pid, int sig) {
+  (void)pid;
+  (void)sig;
+  errno = EINVAL;
+  return -1;
+}
