62 files changed, 13231 insertions, 0 deletions

diff --git a/lib/chibios-contrib/os/various/bitmap.c b/lib/chibios-contrib/os/various/bitmap.c
new file mode 100644
index 000000000..a17dfcb66
--- /dev/null
+++ b/lib/chibios-contrib/os/various/bitmap.c
@@ -0,0 +1,158 @@
+/*
+    ChibiOS/HAL - Copyright (C) 2015 Uladzimir Pylinsky aka barthess
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+/**
+ * @file    bitmap.c
+ * @brief   Bit map code.
+ *
+ * @addtogroup bitmap
+ * @{
+ */
+
+#include "string.h" /* for memset() */
+
+#include "hal.h"
+#include "bitmap.h"
+
+/*===========================================================================*/
+/* Module local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local types.                                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local variables.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module local functions.                                                   */
+/*===========================================================================*/
+
+/**
+ * @brief Get word number.
+ *
+ * @param[in] bit       number of the bit
+ *
+ * @return              Index of the word containing specified bit.
+ */
+static inline size_t word(size_t bit) {
+  return bit / (sizeof(bitmap_word_t) * 8);
+}
+
+/**
+ * @brief Get bit position in word.
+ *
+ * @param[in] bit       number of the bit
+ *
+ * @return              Position of the specified bit related to word start.
+ */
+static inline size_t pos_in_word(size_t bit) {
+  return bit % (sizeof(bitmap_word_t) * 8);
+}
+
+/*===========================================================================*/
+/* Module exported functions.                                                */
+/*===========================================================================*/
+/**
+ * @brief Initializes an @p bitmap_t structure.
+ *
+ * @param[out] map      the @p bitmap_t structure to be initialized
+ * @param[in] val       the value to be written in all bitmap
+ */
+void bitmapObjectInit(bitmap_t *map, bitmap_word_t val) {
+  uint8_t pattern;
+
+  osalDbgCheck(val == 1 || val == 0);
+
+  if (val == 1)
+    pattern = 0xFF;
+  else
+    pattern = 0;
+
+  memset(map->array, pattern, map->len*sizeof(bitmap_word_t));
+}
+
+/**
+ * @brief Set single bit in an @p bitmap_t structure.
+ *
+ * @param[out] map      the @p bitmap_t structure
+ * @param[in] bit       number of the bit to be set
+ */
+void bitmapSet(bitmap_t *map, size_t bit) {
+  size_t w = word(bit);
+
+  osalDbgCheck(w < map->len);
+  map->array[w] |= (bitmap_word_t)1 << pos_in_word(bit);
+}
+
+/**
+ * @brief Clear single bit in an @p bitmap_t structure.
+ *
+ * @param[out] map      the @p bitmap_t structure
+ * @param[in] bit       number of the bit to be cleared
+ */
+void bitmapClear(bitmap_t *map, size_t bit) {
+  size_t w = word(bit);
+
+  osalDbgCheck(w < map->len);
+  map->array[w] &= ~((bitmap_word_t)1 << pos_in_word(bit));
+}
+
+/**
+ * @brief Invert single bit in an @p bitmap_t structure.
+ *
+ * @param[out] map      the @p bitmap_t structure
+ * @param[in] bit       number of the bit to be inverted
+ */
+void bitmapInvert(bitmap_t *map, size_t bit) {
+  size_t w = word(bit);
+
+  osalDbgCheck(w < map->len);
+  map->array[w] ^= (bitmap_word_t)1 << pos_in_word(bit);
+}
+
+/**
+ * @brief Get bit value from an @p bitmap_t structure.
+ *
+ * @param[in] map       the @p bitmap_t structure
+ * @param[in] bit       number of the requested bit
+ *
+ * @return              Requested bit value.
+ */
+bitmap_word_t bitmapGet(const bitmap_t *map, size_t bit) {
+  size_t w = word(bit);
+
+  osalDbgCheck(w < map->len);
+  return (map->array[w] >> pos_in_word(bit)) & 1;
+}
+
+/**
+ * @brief Get total amount of bits in an @p bitmap_t structure.
+ *
+ * @param[in] map       the @p bitmap_t structure
+ *
+ * @return              Bit number.
+ */
+size_t bitmapGetBitsCount(const bitmap_t *map) {
+  return map->len * sizeof(bitmap_word_t) * 8;
+}
+/** @} */
diff --git a/lib/chibios-contrib/os/various/bitmap.h b/lib/chibios-contrib/os/various/bitmap.h
new file mode 100644
index 000000000..115b54cf8
--- /dev/null
+++ b/lib/chibios-contrib/os/various/bitmap.h
@@ -0,0 +1,77 @@
+/*
+    ChibiOS/HAL - Copyright (C) 2015 Uladzimir Pylinsky aka barthess
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+/**
+ * @file    bitmap.h
+ * @brief   Bit map structures and macros.
+ *
+ * @addtogroup bitmap
+ * @{
+ */
+
+#ifndef BITMAP_H_
+#define BITMAP_H_
+
+/*===========================================================================*/
+/* Module constants.                                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Module data structures and types.                                         */
+/*===========================================================================*/
+
+typedef unsigned int bitmap_word_t;
+
+/**
+ * @brief   Type of a event timer structure.
+ */
+typedef struct {
+  bitmap_word_t   *array;
+  size_t          len;    /* Array length in _words_ NOT bytes */
+} bitmap_t;
+
+/*===========================================================================*/
+/* Module macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void bitmapObjectInit(bitmap_t *map, bitmap_word_t val);
+  void bitmapSet(bitmap_t *map, size_t bit);
+  void bitmapClear(bitmap_t *map, size_t bit);
+  void bitmapInvert(bitmap_t *map, size_t bit);
+  bitmap_word_t bitmapGet(const bitmap_t *map, size_t bit);
+  size_t bitmapGetBitsCount(const bitmap_t *map);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BITMAP_H_ */
+
+/** @} */
diff --git a/lib/chibios-contrib/os/various/bmp.mk b/lib/chibios-contrib/os/various/bmp.mk
new file mode 100644
index 000000000..1e2e63e0f
--- /dev/null
+++ b/lib/chibios-contrib/os/various/bmp.mk
@@ -0,0 +1,57 @@
+##
+##  make targets for the Black Magic Probe
+##    See: https://github.com/blacksphere/blackmagic (Black Magic)
+##    See: https://github.com/blacksphere/blackmagic/wiki/Frequently-Asked-Questions (Black Magic udev rules)
+##
+##  Provides the following targets:
+##
+##  all:        Wrapper for the elf binary
+##  flash:      Uses GDB to flash the target via Black Magic
+##  debug:      Flashes the target and allows for debugging using gdb and Black Magic
+##  debug-tui:  Flashes the target and allows for debugging using gdb's TUI and Black Magic
+##
+
+GDB               ?= arm-none-eabi-gdb           # Path to the arm-none-eabi-gdb binary
+GDB_PROGRAM       ?= $(BUILDDIR)/$(PROJECT).elf  # Path to the elf binary
+GDB_BREAK         ?= main                        # Function to break at when starting the debugger
+
+BMP_GDB           ?= /dev/ttyBmpGdb              # Device file for the Black Magic GDB devic
+
+all: $(GDB_PROGRAM)
+
+flash: $(GDB_PROGRAM)
+        $(GDB) $(GDB_PROGRAM) -nx --batch                  \
+                -ex 'target extended-remote $(BMP_GDB)'    \
+                -ex 'monitor swdp_scan'                    \
+                -ex 'attach 1'                             \
+                -ex 'load'                                 \
+                -ex 'compare-sections'                     \
+                -ex 'kill'
+
+debug: $(GDB_PROGRAM)
+        $(GDB) $(GDB_PROGRAM) -nx                          \
+                -ex 'target extended-remote $(BMP_GDB)'    \
+                -ex 'monitor swdp_scan'                    \
+                -ex 'attach 1'                             \
+                -ex 'load'                                 \
+                -ex 'compare-sections'                     \
+                -ex 'set mem inaccessible-by-default off'  \
+                -ex 'break $(GDB_BREAK)'                   \
+                -ex 'c'
+
+debug-tui: $(GDB_PROGRAM)
+        $(GDB) $(GDB_PROGRAM) -nx                         \
+                -ex 'target extended-remote $(BMP_GDB)'   \
+                -ex 'monitor swdp_scan'                   \
+                -ex 'attach 1'                            \
+                -ex 'load'                                \
+                -ex 'compare-sections'                    \
+                -ex 'set mem inaccessible-by-default off' \
+                -ex 'set pagination off'                  \
+                -ex 'focus cmd'                           \
+                -ex 'layout src'                          \
+                -ex 'break $(GDB_BREAK)'                  \
+                -ex 'c'                                   \
+                -ex 'set pagination on'                    
+
+.PHONY: all flash debug debug-tui
diff --git a/lib/chibios-contrib/os/various/bswap.h b/lib/chibios-contrib/os/various/bswap.h
new file mode 100644
index 000000000..64484984d
--- /dev/null
+++ b/lib/chibios-contrib/os/various/bswap.h
@@ -0,0 +1,201 @@
+/*
+    Copyright (C) 2016 Stephane D'Alu
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#ifndef BSWAP_H
+#define BSWAP_H
+
+#if defined(__cplusplus)
+extern "C" {
+#endif
+
+#if !(defined(ARCH_BIG_ENDIAN) || defined(ARCH_LITTLE_ENDIAN))
+#error "Need to define one: ARCH_BIG_ENDIAN or ARCH_LITTLE_ENDIAN"
+#endif
+
+#if defined(ARCH_BIG_ENDIAN) && defined(ARCH_LITTLE_ENDIAN)
+#error "ARCH_BIG_ENDIAN and ARCH_LITTLE_ENDIAN are both set"
+#endif
+
+    
+#define BSWAP_16(x)                                                     \
+    (uint16_t)((((x) & 0xFF00) >> 8) |                                  \
+               (((x) & 0x00FF) << 8))
+#define BSWAP_32(x)                                                     \
+    (uint32_t)((((x) & 0xFF000000UL) >> 24UL) |                         \
+               (((x) & 0x00FF0000UL) >>  8UL) |                         \
+               (((x) & 0x0000FF00UL) <<  8UL) |                         \
+               (((x) & 0x000000FFUL) << 24UL))
+#define BSWAP_64(x)                                                     \
+    (uint64_t)((((x) & 0xFF00000000000000UL) >> 56UL) |                 \
+               (((x) & 0x00FF000000000000UL) >> 40UL) |                 \
+               (((x) & 0x0000FF0000000000UL) >> 24UL) |                 \
+               (((x) & 0x000000FF00000000UL) >>  8UL) |                 \
+               (((x) & 0x00000000FF000000UL) <<  8UL) |                 \
+               (((x) & 0x0000000000FF0000UL) << 24UL) |                 \
+               (((x) & 0x000000000000FF00UL) << 40UL) |                 \
+               (((x) & 0x00000000000000FFUL) << 56UL))
+
+    
+#if defined(ARCH_BIG_ENDIAN)
+#define le16_to_cpu(x)           bswap_16(x)
+#define le32_to_cpu(x)           bswap_32(x)
+#define le64_to_cpu(x)           bswap_64(x)
+#define be16_to_cpu(x)           (x)
+#define be32_to_cpu(x)           (x)
+#define be64_to_cpu(x)           (x)
+#define cpu_to_le16(x)           bswap_16(x)
+#define cpu_to_le32(x)           bswap_32(x)
+#define cpu_to_le64(x)           bswap_64(x)
+#define cpu_to_be16(x)           (x)
+#define cpu_to_be32(x)           (x)
+#define cpu_to_be64(x)           (x)
+#define LE16_TO_CPU(x)           BSWAP_16(x)
+#define LE32_TO_CPU(x)           BSWAP_32(x)
+#define LE64_TO_CPU(x)           BSWAP_64(x)
+#define BE16_TO_CPU(x)           (x)
+#define BE32_TO_CPU(x)           (x)
+#define BE64_TO_CPU(x)           (x)
+#define CPU_TO_LE16(x)           BSWAP_16(x)
+#define CPU_TO_LE32(x)           BSWAP_32(x)
+#define CPU_TO_LE64(x)           BSWAP_64(x)
+#define CPU_TO_BE16(x)           (x)
+#define CPU_TO_BE32(x)           (x)
+#define CPU_TO_BE64(x)           (x)
+#endif
+
+   
+#if defined(ARCH_LITTLE_ENDIAN)
+#define le16_to_cpu(x)           (x)
+#define le32_to_cpu(x)           (x)
+#define le64_to_cpu(x)           (x)
+#define be16_to_cpu(x)           bswap_16(x)
+#define be32_to_cpu(x)           bswap_32(x)
+#define be64_to_cpu(x)           bswap_64(x)
+#define cpu_to_le16(x)           (x)
+#define cpu_to_le32(x)           (x)
+#define cpu_to_le64(x)           (x)
+#define cpu_to_be16(x)           bswap_16(x)
+#define cpu_to_be32(x)           bswap_32(x)
+#define cpu_to_be64(x)           bswap_64(x)
+#define LE16_TO_CPU(x)           (x)
+#define LE32_TO_CPU(x)           (x)
+#define LE64_TO_CPU(x)           (x)
+#define BE16_TO_CPU(x)           BSWAP_16(x)
+#define BE32_TO_CPU(x)           BSWAP_32(x)
+#define BE64_TO_CPU(x)           BSWAP_64(x)
+#define CPU_TO_LE16(x)           (x)
+#define CPU_TO_LE32(x)           (x)
+#define CPU_TO_LE64(x)           (x)
+#define CPU_TO_BE16(x)           BSWAP_16(x)
+#define CPU_TO_BE32(x)           BSWAP_32(x)
+#define CPU_TO_BE64(x)           BSWAP_64(x)
+#endif
+
+    
+static inline uint16_t bswap_16(const uint16_t x)
+    __attribute__ ((warn_unused_result))
+    __attribute__ ((const))
+    __attribute__ ((always_inline));
+
+static inline uint16_t bswap_16(const uint16_t x) {
+    if (__builtin_constant_p(x))
+        return BSWAP_16(x);
+
+    uint8_t                             tmp;
+    union { uint16_t x; uint8_t b[2]; } data;
+    
+    data.x    = x;
+    tmp       = data.b[0];
+    data.b[0] = data.b[1];
+    data.b[1] = tmp;
+    
+    return data.x;
+}
+
+static inline uint32_t bswap_32(const uint32_t x)
+    __attribute__ ((warn_unused_result))
+    __attribute__ ((const))
+    __attribute__ ((always_inline));
+
+
+static inline uint32_t bswap_32(const uint32_t x) {
+    if (__builtin_constant_p(x))
+        return BSWAP_32(x);
+    
+    uint8_t                             tmp;
+    union { uint32_t x; uint8_t b[4]; } data;
+    
+    data.x    = x;    
+    tmp       = data.b[0];
+    data.b[0] = data.b[3];
+    data.b[3] = tmp;
+    tmp       = data.b[1];
+    data.b[1] = data.b[2];
+    data.b[2] = tmp;
+    
+    return data.x;
+}
+    
+static inline uint64_t bswap_64(const uint64_t x)
+    __attribute__ ((warn_unused_result))
+    __attribute__ ((const))
+    __attribute__ ((always_inline));
+
+
+static inline uint64_t bswap_64(const uint64_t x) {
+    if (__builtin_constant_p(x))
+        return BSWAP_64(x);
+    
+    uint8_t                             tmp;
+    union { uint64_t x; uint8_t b[8]; } data;
+
+    data.x    = x;    
+    tmp       = data.b[0];
+    data.b[0] = data.b[7];
+    data.b[7] = tmp;
+    tmp       = data.b[1];
+    data.b[1] = data.b[6];
+    data.b[6] = tmp;
+    tmp       = data.b[2];
+    data.b[2] = data.b[5];
+    data.b[5] = tmp;
+    tmp       = data.b[3];
+    data.b[3] = data.b[4];
+    data.b[4] = tmp;
+    
+    return data.x;
+}
+    
+static inline void bswap_n(void* const data, uint8_t len)
+    __attribute__ ((nonnull (1)));
+
+static inline void bswap_n(void* const data, uint8_t len) {
+    uint8_t* ptr = (uint8_t*)data;
+
+    for ( ; len > 1 ; ptr++, len -= 2 ) {
+        uint8_t tmp      = *ptr;
+        *ptr             = *(ptr + len - 1);
+        *(ptr + len - 1) = tmp;
+    }
+}
+    
+#if defined(__cplusplus)
+}
+#endif
+
+#endif
+
+
diff --git a/lib/chibios-contrib/os/various/crcsw.c b/lib/chibios-contrib/os/various/crcsw.c
new file mode 100644
index 000000000..02a64f3ef
--- /dev/null
+++ b/lib/chibios-contrib/os/various/crcsw.c
@@ -0,0 +1,338 @@
+/*
+    ChibiOS - Copyright (C) 2015 Michael D. Spradling
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+/**
+ * @file    crcsw.c
+ * @brief   CRC software driver.
+   @note    SW implementation was based from:
+ * @note    http://www.barrgroup.com/Embedded-Systems/How-To/CRC-Calculation-C-Code
+ *
+ * @addtogroup CRC
+ * @{
+ */
+
+#include "hal.h"
+
+#if HAL_USE_CRC || defined(__DOXYGEN__)
+
+#if CRCSW_USE_CRC1 || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/**
+ * @brief   CRC default configuration.
+ */
+
+#if CRCSW_CRC32_TABLE == TRUE || defined(__DOXYGEN__)
+static const uint32_t crc32_table[256] = {
+  0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
+  0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+  0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+  0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+  0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
+  0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+  0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
+  0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+  0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+  0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+  0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
+  0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+  0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
+  0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+  0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+  0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+  0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
+  0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+  0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
+  0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+  0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+  0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+  0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
+  0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+  0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
+  0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+  0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+  0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+  0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
+  0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+  0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
+  0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+  0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+  0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+  0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
+  0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+  0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
+  0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+  0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+  0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+  0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
+  0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+  0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
+  0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+  0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+  0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+  0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
+  0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+  0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
+  0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+  0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+  0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+  0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
+  0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+  0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
+  0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+  0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+  0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+  0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
+  0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+  0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
+  0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+  0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+  0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+#endif
+
+#if CRCSW_CRC16_TABLE || defined(__DOXYGEN__)
+static const uint32_t crc16_table[256] = {
+  0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
+  0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
+  0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
+  0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
+  0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
+  0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
+  0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
+  0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
+  0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
+  0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
+  0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
+  0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
+  0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
+  0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
+  0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
+  0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
+  0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
+  0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
+  0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
+  0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
+  0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
+  0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
+  0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
+  0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
+  0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
+  0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
+  0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
+  0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
+  0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
+  0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
+  0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
+  0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
+};
+#endif
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/** @brief CRC1 driver identifier.*/
+#if CRCSW_USE_CRC1 || defined(__DOXYGEN__)
+CRCDriver CRCD1;
+#endif
+
+#if CRCSW_CRC32_TABLE || defined(__DOXYGEN__)
+const CRCConfig crcsw_crc32_config = {
+  .poly_size         = 32,
+  .poly              = 0x04C11DB7,
+  .initial_val       = 0xFFFFFFFF,
+  .final_val         = 0xFFFFFFFF,
+  .reflect_data      = 1,
+  .reflect_remainder = 1,
+  .table             = crc32_table
+};
+#endif
+
+#if CRCSW_CRC16_TABLE || defined(__DOXYGEN__)
+const CRCConfig crcsw_crc16_config = {
+  .poly_size         = 16,
+  .poly              = 0x8005,
+  .initial_val       = 0x0,
+  .final_val         = 0x0,
+  .reflect_data      = 1,
+  .reflect_remainder = 1,
+  .table             = crc16_table
+};
+#endif
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+#if (CRCSW_PROGRAMMABLE == TRUE)
+static uint32_t reflect(uint32_t data, uint8_t nBits) {
+  uint32_t reflection = 0x00000000;
+  uint8_t  bit;
+
+  /* Reflect the data about the center bit.  */
+  for (bit = 0; bit < nBits; ++bit) {
+    /* If the LSB bit is set, set the reflection of it. */
+    if (data & 0x01) {
+      reflection |= (1 << ((nBits - 1) - bit));
+    }
+
+    data = (data >> 1);
+  }
+
+  return reflection;
+}
+#endif
+
+/*===========================================================================*/
+/* Driver interrupt handlers.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Low level CRC software initialization.
+ *
+ * @notapi
+ */
+void crc_lld_init(void) {
+  crcObjectInit(&CRCD1);
+  CRCD1.crc = CRCD1.config->initial_val;
+}
+
+/**
+ * @brief   Configures and activates the CRC peripheral.
+ *
+ * @param[in] crcp      pointer to the @p CRCDriver object
+ *
+ * @notapi
+ */
+void crc_lld_start(CRCDriver *crcp) {
+  osalDbgAssert(crcp->config != NULL, "config must not be NULL");
+
+#if CRCSW_PROGRAMMABLE == FALSE
+#if CRCSW_CRC32_TABLE == TRUE && CRCSW_CRC16_TABLE == TRUE
+  osalDbgAssert((crcp->config == CRCSW_CRC32_TABLE_CONFIG) ||
+      (crcp->config == CRCSW_CRC16_TABLE_CONFIG), "config must be CRCSW_CRC32_TABLE_CONFIG or CRCSW_CRC16_TABLE_CONFIG");
+#elif CRCSW_CRC32_TABLE == TRUE && CRCSW_CRC16_TABLE == FALSE
+  osalDbgAssert(crcp->config == CRCSW_CRC32_TABLE_CONFIG,
+      "config must be CRCSW_CRC32_TABLE_CONFIG");
+#else 
+  osalDbgAssert(crcp->config == CRCSW_CRC16_TABLE_CONFIG,
+      "config must be CRCSW_CRC16_TABLE_CONFIG");
+#endif
+#endif
+  crc_lld_reset(crcp);
+}
+
+
+/**
+ * @brief   Deactivates the CRC peripheral.
+ *
+ * @param[in] crcp      pointer to the @p CRCDriver object
+ *
+ * @notapi
+ */
+void crc_lld_stop(CRCDriver *crcp) {
+  (void)crcp;
+}
+
+/**
+ * @brief   Resets current CRC calculation.
+ *
+ * @param[in] crcp      pointer to the @p CRCDriver object
+ *
+ * @notapi
+ */
+void crc_lld_reset(CRCDriver *crcp) {
+  crcp->crc = crcp->config->initial_val;
+}
+
+/**
+ * @brief   Returns calculated CRC from last reset
+ *
+ * @param[in] crcp      pointer to the @p CRCDriver object
+ * @param[in] n         size of buf in bytes
+ * @param[in] buf       @p buffer location
+ *
+ * @notapi
+ */
+uint32_t crc_lld_calc(CRCDriver *crcp, size_t n, const void *buf) {
+  uint32_t i;
+  uint32_t crc;
+#if (CRCSW_CRC32_TABLE == TRUE) || (CRCSW_CRC16_TABLE == TRUE)
+  if (crcp->config->table != NULL) {
+    for (i = 0; i < n; i++) {
+      uint8_t data = *((uint8_t*)buf + i);
+      uint8_t idx = (crcp->crc ^ data);
+      crcp->crc = (crcp->config->table[idx] ^ (crcp->crc >> 8));
+    }
+    crc = crcp->crc;
+  }
+#endif
+
+#if (CRCSW_PROGRAMMABLE == TRUE)
+  // Mask off bits to poly size
+  uint32_t mask = 1 << (crcp->config->poly_size - 1);
+  mask |= (mask - 1);
+
+  crc = crcp->crc;
+  if (crcp->config->table == NULL) {
+    for (i = 0; i < n; i++) {
+      uint8_t data = *((uint8_t*)buf + i);
+      uint8_t bit;
+
+      if (crcp->config->reflect_data) {
+        data = reflect(data, 8);
+      }
+
+      /* Bring the next byte into the remainder. */
+      crc ^= (data << (crcp->config->poly_size - 8));
+
+      /* Perform modulo-2 division, a bit at a time. */
+      for (bit = 8; bit > 0; --bit) {
+        /* Try to divide the current data bit. */
+        if (crc & (1 << (crcp->config->poly_size - 1))) {
+          crc = (crc << 1) ^ crcp->config->poly;
+        } else {
+          crc <<= 1;
+        }
+      }
+    }
+
+    crcp->crc = crc;
+
+    if (crcp->config->reflect_remainder) {
+      crc = reflect(crc, crcp->config->poly_size);
+    }
+  }
+#endif
+
+  return (crc ^ crcp->config->final_val) & mask;
+}
+
+#endif /* CRCSW_USE_CRC1 */
+
+#endif /* HAL_USE_CRC */
diff --git a/lib/chibios-contrib/os/various/crcsw.h b/lib/chibios-contrib/os/various/crcsw.h
new file mode 100644
index 000000000..4483a3456
--- /dev/null
+++ b/lib/chibios-contrib/os/various/crcsw.h
@@ -0,0 +1,215 @@
+/*
+    ChibiOS - Copyright (C) 2015 Michael D. Spradling
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+/**
+ * @file    crcsw.h
+ * @brief   CRC software driver.
+ *
+ * @addtogroup CRC
+ * @{
+ */
+
+#include "hal.h"
+
+#if HAL_USE_CRC || defined(__DOXYGEN__)
+
+#if CRCSW_USE_CRC1 || defined(__DOXYGEN__)
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Configuration options
+ * @{
+ */
+/**
+ * @brief   CRC1 software driver enable switch.
+ * @details If set to @p TRUE the support for CRC1 is included.
+ * @note    The default is @p FALSE
+ */
+#if !defined(CRCSW_USE_CRC1) || defined(__DOXYGEN__)
+#define CRCSW_USE_CRC1                  FALSE
+#endif
+
+/**
+ * @brief Enables software CRC32
+ */
+#if !defined(CRCSW_CRC32_TABLE) || defined(__DOXYGEN__)
+#define CRCSW_CRC32_TABLE               FALSE
+#endif
+
+/**
+ * @brief Enables software CRC16
+ */
+#if !defined(CRCSW_CRC16_TABLE) || defined(__DOXYGEN__)
+#define CRCSW_CRC16_TABLE               FALSE
+#endif
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if CRCSW_USE_CRC1 && CRC_USE_DMA
+#error "Software CRC does not support DMA(CRC_USE_DMA)"
+#endif
+
+#if CRCSW_CRC32_TABLE == FALSE && CRCSW_CRC16_TABLE == FALSE &&                         \
+    CRCSW_PROGRAMMABLE == FALSE
+#error "At least one of CRCSW_PROGRAMMABLE, CRCSW_CRC32_TABLE, or CRCSW_CRC16_TABLE must be defined"
+#endif
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+/**
+ * @brief   Type of a structure representing an CRC driver.
+ */
+typedef struct CRCDriver CRCDriver;
+
+/**
+ * @brief   Driver configuration structure.
+ */
+typedef struct {
+  /**
+   * @brief The size of polynomial to be used for CRC.
+   */
+  uint32_t                 poly_size;
+  /**
+   * @brief The coefficients of the polynomial to be used for CRC.
+   */
+  uint32_t                 poly;
+  /**
+   * @brief The inital value
+   */
+  uint32_t                 initial_val;
+  /**
+   * @brief The final XOR value
+   */
+  uint32_t                 final_val;
+  /**
+   * @brief Reflect bit order data going into CRC
+   */
+  bool                     reflect_data;
+  /**
+   * @brief Reflect bit order of final remainder
+   */
+  bool                     reflect_remainder;
+  /* End of the mandatory fields.*/
+  /**
+   * @brief The crc lookup table to use when calculating CRC.
+   */
+  const uint32_t           *table;
+} CRCConfig;
+
+
+/**
+ * @brief   Structure representing an CRC driver.
+ */
+struct CRCDriver {
+  /**
+   * @brief Driver state.
+   */
+  crcstate_t                state;
+  /**
+   * @brief Current configuration data.
+   */
+  const CRCConfig           *config;
+#if CRC_USE_MUTUAL_EXCLUSION || defined(__DOXYGEN__)
+  /**
+   * @brief   Mutex protecting the peripheral.
+   */
+  mutex_t                   mutex;
+#endif /* CRC_USE_MUTUAL_EXCLUSION */
+  /* End of the mandatory fields.*/
+  /**
+   * @brief Current value of calculated CRC.
+   */
+  uint32_t                  crc;
+};
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+#if CRCSW_CRC32_TABLE || defined(__DOXYGEN__)
+/**
+ * @brief Configuration that represents CRC32
+ */
+#define CRCSW_CRC32_TABLE_CONFIG (&crcsw_crc32_config)
+#endif
+
+#if CRCSW_CRC16_TABLE || defined(__DOXYGEN__)
+/**
+ * @brief Configuration that represents CRC16
+ */
+#define CRCSW_CRC16_TABLE_CONFIG (&crcsw_crc16_config)
+#endif
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver interrupt handlers.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+extern CRCDriver CRCD1;
+
+#if CRCSW_CRC32_TABLE
+extern const CRCConfig crcsw_crc32_config;
+#endif
+
+#if CRCSW_CRC16_TABLE
+extern const CRCConfig crcsw_crc16_config;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void crc_lld_init(void);
+  void crc_lld_start(CRCDriver *crcp);
+  void crc_lld_stop(CRCDriver *crcp);
+  void crc_lld_reset(CRCDriver *crcp);
+  uint32_t crc_lld_calc(CRCDriver *crcp, size_t n, const void *buf);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CRCSW_USE_CRC1 */
+
+#endif /* HAL_USE_CRC */
+
+/** @} */
diff --git a/lib/chibios-contrib/os/various/dbgtrace.h b/lib/chibios-contrib/os/various/dbgtrace.h
new file mode 100644
index 000000000..b1fc2979e
--- /dev/null
+++ b/lib/chibios-contrib/os/various/dbgtrace.h
@@ -0,0 +1,41 @@
+#ifndef DBGTRACE_H_
+#define DBGTRACE_H_
+
+#include "chprintf.h"
+
+#if !defined(DEBUG_TRACE_PRINT)
+#define DEBUG_TRACE_PRINT     FALSE
+#endif
+
+#if !defined(DEBUG_TRACE_WARNING)
+#define DEBUG_TRACE_WARNING   FALSE
+#endif
+
+#if !defined(DEBUG_TRACE_ERROR)
+#define DEBUG_TRACE_ERROR     FALSE
+#endif
+
+/* user must provide correctly initialized pointer to print channel */
+#if DEBUG_TRACE_PRINT || DEBUG_TRACE_WARNING || DEBUG_TRACE_ERROR
+extern BaseSequentialStream   *GlobalDebugChannel;
+#endif
+
+#if DEBUG_TRACE_PRINT
+#define dbgprintf(fmt, ...)  chprintf(GlobalDebugChannel, fmt, ##__VA_ARGS__)
+#else
+#define dbgprintf(fmt, ...)  do {} while(0)
+#endif
+
+#if DEBUG_TRACE_WARNING
+#define warnprintf(fmt, ...)  chprintf(GlobalDebugChannel, fmt, ##__VA_ARGS__)
+#else
+#define warnprintf(fmt, ...)  do {} while(0)
+#endif
+
+#if DEBUG_TRACE_ERROR
+#define errprintf(fmt, ...)  chprintf(GlobalDebugChannel, fmt, ##__VA_ARGS__)
+#else
+#define errprintf(fmt, ...)  do {} while(0)
+#endif
+
+#endif /* DBGTRACE_H_ */
diff --git a/lib/chibios-contrib/os/various/devices_lib/lcd/ili9341.c b/lib/chibios-contrib/os/various/devices_lib/lcd/ili9341.c
new file mode 100644
index 000000000..979e502cc
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/lcd/ili9341.c
@@ -0,0 +1,418 @@
+/*
+    Copyright (C) 2013-2015 Andrea Zoppi
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+/**
+ * @file    ili9341.c
+ * @brief   ILI9341 TFT LCD diaplay controller driver.
+ * @note    Does not support multiple calling threads natively.
+ */
+
+#include "ch.h"
+#include "hal.h"
+#include "ili9341.h"
+
+/**
+ * @addtogroup ili9341
+ * @{
+ */
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+#if !ILI9341_USE_CHECKS && !defined(__DOXYGEN__)
+/* Disable checks as needed.*/
+
+#ifdef osalDbgCheck
+#undef osalDbgCheck
+#endif
+#define osalDbgCheck(c, func) {                                               \
+  (void)(c), (void)__QUOTE_THIS(func)"()";                                  \
+}
+
+#ifdef osalDbgAssert
+#undef osalDbgAssert
+#endif
+#define osalDbgAssert(c, m, r) {                                              \
+  (void)(c);                                                                \
+}
+
+#ifdef osalDbgCheckClassS
+#undef osalDbgCheckClassS
+#endif
+#define osalDbgCheckClassS() {}
+
+#ifdef osalDbgCheckClassS
+#undef osalDbgCheckClassS
+#endif
+#define osalDbgCheckClassI() {}
+
+#endif /* ILI9341_USE_CHECKS */
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/** @brief ILI9341D1 driver identifier.*/
+ILI9341Driver ILI9341D1;
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Initializes the standard part of a @p ILI9341Driver structure.
+ *
+ * @param[out] driverp  pointer to the @p ILI9341Driver object
+ *
+ * @init
+ */
+void ili9341ObjectInit(ILI9341Driver *driverp) {
+
+  osalDbgCheck(driverp != NULL);
+
+  driverp->state = ILI9341_STOP;
+  driverp->config = NULL;
+#if (TRUE == ILI9341_USE_MUTUAL_EXCLUSION)
+#if (TRUE == CH_CFG_USE_MUTEXES)
+  chMtxObjectInit(&driverp->lock);
+#else
+  chSemObjectInit(&driverp->lock, 1);
+#endif
+#endif /* (TRUE == ILI9341_USE_MUTUAL_EXCLUSION) */
+}
+
+/**
+ * @brief   Configures and activates the ILI9341 peripheral.
+ * @pre     ILI9341 is stopped.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ * @param[in] configp   pointer to the @p ILI9341Config object
+ *
+ * @api
+ */
+void ili9341Start(ILI9341Driver *driverp, const ILI9341Config *configp) {
+
+  chSysLock();
+  osalDbgCheck(driverp != NULL);
+  osalDbgCheck(configp != NULL);
+  osalDbgCheck(configp->spi != NULL);
+  osalDbgAssert(driverp->state == ILI9341_STOP, "invalid state");
+
+  spiSelectI(configp->spi);
+  spiUnselectI(configp->spi);
+  driverp->config = configp;
+  driverp->state = ILI9341_READY;
+  chSysUnlock();
+}
+
+/**
+ * @brief   Deactivates the ILI9341 peripheral.
+ * @pre     ILI9341 is ready.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @api
+ */
+void ili9341Stop(ILI9341Driver *driverp) {
+
+  chSysLock();
+  osalDbgCheck(driverp != NULL);
+  osalDbgAssert(driverp->state == ILI9341_READY, "invalid state");
+
+  driverp->state = ILI9341_STOP;
+  chSysUnlock();
+}
+
+#if ILI9341_USE_MUTUAL_EXCLUSION
+
+/**
+ * @brief   Gains exclusive access to the ILI9341 module.
+ * @details This function tries to gain ownership to the ILI9341 module, if the
+ *          module is already being used then the invoking thread is queued.
+ * @pre     In order to use this function the option
+ *          @p ILI9341_USE_MUTUAL_EXCLUSION must be enabled.
+ * @pre     ILI9341 is ready.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @sclass
+ */
+void ili9341AcquireBusS(ILI9341Driver *driverp) {
+
+  osalDbgCheckClassS();
+  osalDbgCheck(driverp == &ILI9341D1);
+  osalDbgAssert(driverp->state == ILI9341_READY, "not ready");
+
+#if (TRUE == CH_CFG_USE_MUTEXES)
+  chMtxLockS(&driverp->lock);
+#else
+  chSemWaitS(&driverp->lock);
+#endif
+}
+
+/**
+ * @brief   Gains exclusive access to the ILI9341 module.
+ * @details This function tries to gain ownership to the ILI9341 module, if the
+ *          module is already being used then the invoking thread is queued.
+ * @pre     In order to use this function the option
+ *          @p ILI9341_USE_MUTUAL_EXCLUSION must be enabled.
+ * @pre     ILI9341 is ready.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @api
+ */
+void ili9341AcquireBus(ILI9341Driver *driverp) {
+
+  chSysLock();
+  ili9341AcquireBusS(driverp);
+  chSysUnlock();
+}
+
+/**
+ * @brief   Releases exclusive access to the ILI9341 module.
+ * @pre     In order to use this function the option
+ *          @p ILI9341_USE_MUTUAL_EXCLUSION must be enabled.
+ * @pre     ILI9341 is ready.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @sclass
+ */
+void ili9341ReleaseBusS(ILI9341Driver *driverp) {
+
+  osalDbgCheckClassS();
+  osalDbgCheck(driverp == &ILI9341D1);
+  osalDbgAssert(driverp->state == ILI9341_READY, "not ready");
+
+#if (TRUE == CH_CFG_USE_MUTEXES)
+  chMtxUnlockS(&driverp->lock);
+#else
+  chSemSignalI(&driverp->lock);
+#endif
+}
+
+/**
+ * @brief   Releases exclusive access to the ILI9341 module.
+ * @pre     In order to use this function the option
+ *          @p ILI9341_USE_MUTUAL_EXCLUSION must be enabled.
+ * @pre     ILI9341 is ready.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @api
+ */
+void ili9341ReleaseBus(ILI9341Driver *driverp) {
+
+  chSysLock();
+  ili9341ReleaseBusS(driverp);
+  chSysUnlock();
+}
+
+#endif /* ILI9341_USE_MUTUAL_EXCLUSION */
+
+#if ILI9341_IM == ILI9341_IM_4LSI_1 /* 4-wire, half-duplex */
+
+/**
+ * @brief   Asserts the slave select signal and prepares for transfers.
+ * @pre     ILI9341 is ready.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @iclass
+ */
+void ili9341SelectI(ILI9341Driver *driverp) {
+
+  osalDbgCheckClassI();
+  osalDbgCheck(driverp != NULL);
+  osalDbgAssert(driverp->state == ILI9341_READY, "invalid state");
+
+  driverp->state = ILI9341_ACTIVE;
+  spiSelectI(driverp->config->spi);
+}
+
+/**
+ * @brief   Asserts the slave select signal and prepares for transfers.
+ * @pre     ILI9341 is ready.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @api
+ */
+void ili9341Select(ILI9341Driver *driverp) {
+
+  chSysLock();
+  ili9341SelectI(driverp);
+  chSysUnlock();
+}
+
+/**
+ * @brief   Deasserts the slave select signal.
+ * @details The previously selected peripheral is unselected.
+ * @pre     ILI9341 is active.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @iclass
+ */
+void ili9341UnselectI(ILI9341Driver *driverp) {
+
+  osalDbgCheckClassI();
+  osalDbgCheck(driverp != NULL);
+  osalDbgAssert(driverp->state == ILI9341_ACTIVE, "invalid state");
+
+  spiUnselectI(driverp->config->spi);
+  driverp->state = ILI9341_READY;
+}
+
+/**
+ * @brief   Deasserts the slave select signal.
+ * @details The previously selected peripheral is unselected.
+ * @pre     ILI9341 is active.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @iclass
+ */
+void ili9341Unselect(ILI9341Driver *driverp) {
+
+  chSysLock();
+  ili9341UnselectI(driverp);
+  chSysUnlock();
+}
+
+/**
+ * @brief   Write command byte.
+ * @details Sends a command byte via SPI.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ * @param[in] cmd       command byte
+ *
+ * @api
+ */
+void ili9341WriteCommand(ILI9341Driver *driverp, uint8_t cmd) {
+
+  osalDbgCheck(driverp != NULL);
+  osalDbgAssert(driverp->state == ILI9341_ACTIVE, "invalid state");
+
+  driverp->value = cmd;
+  palClearPad(driverp->config->dcx_port, driverp->config->dcx_pad);  /* !Cmd */
+  spiSend(driverp->config->spi, 1, &driverp->value);
+}
+
+/**
+ * @brief   Write data byte.
+ * @details Sends a data byte via SPI.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ * @param[in] value     data byte
+ *
+ * @api
+ */
+void ili9341WriteByte(ILI9341Driver *driverp, uint8_t value) {
+
+  osalDbgCheck(driverp != NULL);
+  osalDbgAssert(driverp->state == ILI9341_ACTIVE, "invalid state");
+
+  driverp->value = value;
+  palSetPad(driverp->config->dcx_port, driverp->config->dcx_pad);  /* Data */
+  spiSend(driverp->config->spi, 1, &driverp->value);
+}
+
+/**
+ * @brief   Read data byte.
+ * @details Receives a data byte via SPI.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ *
+ * @return              data byte
+ *
+ * @api
+ */
+uint8_t ili9341ReadByte(ILI9341Driver *driverp) {
+
+  osalDbgAssert(FALSE, "should not be used");
+
+  osalDbgCheck(driverp != NULL);
+  osalDbgAssert(driverp->state == ILI9341_ACTIVE, "invalid state");
+
+  palSetPad(driverp->config->dcx_port, driverp->config->dcx_pad);  /* Data */
+  spiReceive(driverp->config->spi, 1, &driverp->value);
+  return driverp->value;
+}
+
+/**
+ * @brief   Write data chunk.
+ * @details Sends a data chunk via SPI.
+ * @pre     The chunk must be accessed by DMA.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ * @param[in] chunk     chunk bytes
+ * @param[in] length    chunk length
+ *
+ * @api
+ */
+void ili9341WriteChunk(ILI9341Driver *driverp, const uint8_t chunk[],
+                       size_t length) {
+
+  osalDbgCheck(driverp != NULL);
+  osalDbgCheck(chunk != NULL);
+  osalDbgAssert(driverp->state == ILI9341_ACTIVE, "invalid state");
+
+  if (length != 0) {
+    palSetPad(driverp->config->dcx_port, driverp->config->dcx_pad);  /* Data */
+    spiSend(driverp->config->spi, length, chunk);
+  }
+}
+
+/**
+ * @brief   Read data chunk.
+ * @details Receives a data chunk via SPI.
+ * @pre     The chunk must be accessed by DMA.
+ *
+ * @param[in] driverp   pointer to the @p ILI9341Driver object
+ * @param[out] chunk    chunk bytes
+ * @param[in] length    chunk length
+ *
+ * @api
+ */
+void ili9341ReadChunk(ILI9341Driver *driverp, uint8_t chunk[],
+                      size_t length) {
+
+  osalDbgCheck(driverp != NULL);
+  osalDbgCheck(chunk != NULL);
+  osalDbgAssert(driverp->state == ILI9341_ACTIVE, "invalid state");
+
+  if (length != 0) {
+    palSetPad(driverp->config->dcx_port, driverp->config->dcx_pad);  /* Data */
+    spiReceive(driverp->config->spi, length, chunk);
+  }
+}
+
+#else /* ILI9341_IM == * */
+#error "Only the ILI9341_IM_4LSI_1 interface mode is currently supported"
+#endif /* ILI9341_IM == * */
+
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/lcd/ili9341.h b/lib/chibios-contrib/os/various/devices_lib/lcd/ili9341.h
new file mode 100644
index 000000000..007c4fd67
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/lcd/ili9341.h
@@ -0,0 +1,593 @@
+/*
+    Copyright (C) 2013-2015 Andrea Zoppi
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+/**
+ * @file    ili9341.h
+ * @brief   ILI9341 TFT LCD diaplay controller driver.
+ */
+
+#ifndef _ILI9341_H_
+#define _ILI9341_H_
+
+/**
+ * @addtogroup ili9341
+ * @{
+ */
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+/**
+ * @name    ILI9341 regulative commands
+ * @{
+ */
+#define ILI9341_CMD_NOP                 (0x00)  /**< No operation.*/
+#define ILI9341_CMD_RESET               (0x01)  /**< Software reset.*/
+#define ILI9341_GET_ID_INFO             (0x04)  /**< Get ID information.*/
+#define ILI9341_GET_STATUS              (0x09)  /**< Get status.*/
+#define ILI9341_GET_PWR_MODE            (0x0A)  /**< Get power mode.*/
+#define ILI9341_GET_MADCTL              (0x0B)  /**< Get MADCTL.*/
+#define ILI9341_GET_PIX_FMT             (0x0C)  /**< Get pixel format.*/
+#define ILI9341_GET_IMG_FMT             (0x0D)  /**< Get image format.*/
+#define ILI9341_GET_SIG_MODE            (0x0E)  /**< Get signal mode.*/
+#define ILI9341_GET_SELF_DIAG           (0x0F)  /**< Get self-diagnostics.*/
+#define ILI9341_CMD_SLEEP_ON            (0x10)  /**< Enter sleep mode.*/
+#define ILI9341_CMD_SLEEP_OFF           (0x11)  /**< Exist sleep mode.*/
+#define ILI9341_CMD_PARTIAL_ON          (0x12)  /**< Enter partial mode.*/
+#define ILI9341_CMD_PARTIAL_OFF         (0x13)  /**< Exit partial mode.*/
+#define ILI9341_CMD_INVERT_ON           (0x20)  /**< Enter inverted mode.*/
+#define ILI9341_CMD_INVERT_OFF          (0x21)  /**< Exit inverted mode.*/
+#define ILI9341_SET_GAMMA               (0x26)  /**< Set gamma params.*/
+#define ILI9341_CMD_DISPLAY_OFF         (0x28)  /**< Disable display.*/
+#define ILI9341_CMD_DISPLAY_ON          (0x29)  /**< Enable display.*/
+#define ILI9341_SET_COL_ADDR            (0x2A)  /**< Set column address.*/
+#define ILI9341_SET_PAGE_ADDR           (0x2B)  /**< Set page address.*/
+#define ILI9341_SET_MEM                 (0x2C)  /**< Set memory.*/
+#define ILI9341_SET_COLOR               (0x2D)  /**< Set color.*/
+#define ILI9341_GET_MEM                 (0x2E)  /**< Get memory.*/
+#define ILI9341_SET_PARTIAL_AREA        (0x30)  /**< Set partial area.*/
+#define ILI9341_SET_VSCROLL             (0x33)  /**< Set vertical scroll def.*/
+#define ILI9341_CMD_TEARING_ON          (0x34)  /**< Tearing line enabled.*/
+#define ILI9341_CMD_TEARING_OFF         (0x35)  /**< Tearing line disabled.*/
+#define ILI9341_SET_MEM_ACS_CTL         (0x36)  /**< Set mem access ctl.*/
+#define ILI9341_SET_VSCROLL_ADDR        (0x37)  /**< Set vscroll start addr.*/
+#define ILI9341_CMD_IDLE_OFF            (0x38)  /**< Exit idle mode.*/
+#define ILI9341_CMD_IDLE_ON             (0x39)  /**< Enter idle mode.*/
+#define ILI9341_SET_PIX_FMT             (0x3A)  /**< Set pixel format.*/
+#define ILI9341_SET_MEM_CONT            (0x3C)  /**< Set memory continue.*/
+#define ILI9341_GET_MEM_CONT            (0x3E)  /**< Get memory continue.*/
+#define ILI9341_SET_TEAR_SCANLINE       (0x44)  /**< Set tearing scanline.*/
+#define ILI9341_GET_TEAR_SCANLINE       (0x45)  /**< Get tearing scanline.*/
+#define ILI9341_SET_BRIGHTNESS          (0x51)  /**< Set brightness.*/
+#define ILI9341_GET_BRIGHTNESS          (0x52)  /**< Get brightness.*/
+#define ILI9341_SET_DISPLAY_CTL         (0x53)  /**< Set display ctl.*/
+#define ILI9341_GET_DISPLAY_CTL         (0x54)  /**< Get display ctl.*/
+#define ILI9341_SET_CABC                (0x55)  /**< Set CABC.*/
+#define ILI9341_GET_CABC                (0x56)  /**< Get CABC.*/
+#define ILI9341_SET_CABC_MIN            (0x5E)  /**< Set CABC min.*/
+#define ILI9341_GET_CABC_MIN            (0x5F)  /**< Set CABC max.*/
+#define ILI9341_GET_ID1                 (0xDA)  /**< Get ID1.*/
+#define ILI9341_GET_ID2                 (0xDB)  /**< Get ID2.*/
+#define ILI9341_GET_ID3                 (0xDC)  /**< Get ID3.*/
+/** @} */
+
+/**
+ * @name    ILI9341 extended commands
+ * @{
+ */
+#define ILI9341_SET_RGB_IF_SIG_CTL      (0xB0)  /**< RGB IF signal ctl.*/
+#define ILI9341_SET_FRAME_CTL_NORMAL    (0xB1)  /**< Set frame ctl (normal).*/
+#define ILI9341_SET_FRAME_CTL_IDLE      (0xB2)  /**< Set frame ctl (idle).*/
+#define ILI9341_SET_FRAME_CTL_PARTIAL   (0xB3)  /**< Set frame ctl (partial).*/
+#define ILI9341_SET_INVERSION_CTL       (0xB4)  /**< Set inversion ctl.*/
+#define ILI9341_SET_BLANKING_PORCH_CTL  (0xB5)  /**< Set blanking porch ctl.*/
+#define ILI9341_SET_FUNCTION_CTL        (0xB6)  /**< Set function ctl.*/
+#define ILI9341_SET_ENTRY_MODE          (0xB7)  /**< Set entry mode.*/
+#define ILI9341_SET_LIGHT_CTL_1         (0xB8)  /**< Set backlight ctl 1.*/
+#define ILI9341_SET_LIGHT_CTL_2         (0xB9)  /**< Set backlight ctl 2.*/
+#define ILI9341_SET_LIGHT_CTL_3         (0xBA)  /**< Set backlight ctl 3.*/
+#define ILI9341_SET_LIGHT_CTL_4         (0xBB)  /**< Set backlight ctl 4.*/
+#define ILI9341_SET_LIGHT_CTL_5         (0xBC)  /**< Set backlight ctl 5.*/
+#define ILI9341_SET_LIGHT_CTL_7         (0xBE)  /**< Set backlight ctl 7.*/
+#define ILI9341_SET_LIGHT_CTL_8         (0xBF)  /**< Set backlight ctl 8.*/
+#define ILI9341_SET_POWER_CTL_1         (0xC0)  /**< Set power ctl 1.*/
+#define ILI9341_SET_POWER_CTL_2         (0xC1)  /**< Set power ctl 2.*/
+#define ILI9341_SET_VCOM_CTL_1          (0xC5)  /**< Set VCOM ctl 1.*/
+#define ILI9341_SET_VCOM_CTL_2          (0xC6)  /**< Set VCOM ctl 2.*/
+#define ILI9341_SET_NVMEM               (0xD0)  /**< Set NVMEM data.*/
+#define ILI9341_GET_NVMEM_KEY           (0xD1)  /**< Get NVMEM protect key.*/
+#define ILI9341_GET_NVMEM_STATUS        (0xD2)  /**< Get NVMEM status.*/
+#define ILI9341_GET_ID4                 (0xD3)  /**< Get ID4.*/
+#define ILI9341_SET_PGAMMA              (0xE0)  /**< Set positive gamma.*/
+#define ILI9341_SET_NGAMMA              (0xE1)  /**< Set negative gamma.*/
+#define ILI9341_SET_DGAMMA_CTL_1        (0xE2)  /**< Set digital gamma ctl 1.*/
+#define ILI9341_SET_DGAMMA_CTL_2        (0xE3)  /**< Set digital gamma ctl 2.*/
+#define ILI9341_SET_IF_CTL              (0xF6)  /**< Set interface control.*/
+/** @} */
+
+/**
+ * @name    ILI9341 interface modes
+ * @{
+ */
+#define ILI9341_IM_3LSI_1               (0x5)   /**< 3-line serial, mode 1.*/
+#define ILI9341_IM_3LSI_2               (0xD)   /**< 3-line serial, mode 2.*/
+#define ILI9341_IM_4LSI_1               (0x6)   /**< 4-line serial, mode 1.*/
+#define ILI9341_IM_4LSI_2               (0xE)   /**< 4-line serial, mode 2.*/
+/** @} */
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    ILI9341 configuration options
+ * @{
+ */
+
+/**
+ * @brief   Enables the @p ili9341AcquireBus() and @p ili9341ReleaseBus() APIs.
+ * @note    Disabling this option saves both code and data space.
+ */
+#if !defined(ILI9341_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__)
+#define ILI9341_USE_MUTUAL_EXCLUSION        TRUE
+#endif
+
+/**
+ * @brief   ILI9341 Interface Mode.
+ */
+#if !defined(ILI9341_IM) || defined(__DOXYGEN__)
+#define ILI9341_IM                          (ILI9341_IM_4LSI_1)
+#endif
+
+/**
+ * @brief   Enables checks for ILI9341 functions.
+ * @note    Disabling this option saves both code and data space.
+ * @note    Disabling checks by ChibiOS will automatically disable ILI9341
+ *          checks.
+ */
+#if !defined(ILI9341_USE_CHECKS) || defined(__DOXYGEN__)
+#define ILI9341_USE_CHECKS                  TRUE
+#endif
+
+/** @} */
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if ((TRUE == ILI9341_USE_MUTUAL_EXCLUSION) && \
+     (TRUE != CH_CFG_USE_MUTEXES) && \
+     (TRUE != CH_CFG_USE_SEMAPHORES))
+#error "ILI9341_USE_MUTUAL_EXCLUSION requires CH_CFG_USE_MUTEXES and/or CH_CFG_USE_SEMAPHORES"
+#endif
+
+/* TODO: Add the remaining modes.*/
+#if (ILI9341_IM != ILI9341_IM_4LSI_1)
+#error "Only ILI9341_IM_4LSI_1 interface mode is supported currently"
+#endif
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/* Complex types forwarding.*/
+typedef struct ILI9341Config ILI9341Config;
+typedef enum ili9341state_t ili9341state_t;
+typedef struct ILI9341Driver ILI9341Driver;
+
+/**
+ * @brief   ILI9341 driver configuration.
+ */
+typedef struct ILI9341Config {
+  SPIDriver     *spi;               /**< SPI driver used by ILI9341.*/
+#if (ILI9341_IM == ILI9341_IM_4LSI_1)
+  ioportid_t    dcx_port;           /**< <tt>D/!C</tt> signal port.*/
+  uint16_t      dcx_pad;            /**< <tt>D/!C</tt> signal pad.*/
+#endif /* ILI9341_IM == * */ /* TODO: Add all modes.*/
+} ILI9341Config;
+
+/**
+ * @brief   ILI9341 driver state.
+ */
+typedef enum ili9341state_t {
+  ILI9341_UNINIT = (0),             /**< Not initialized.*/
+  ILI9341_STOP   = (1),             /**< Stopped.*/
+  ILI9341_READY  = (2),             /**< Ready.*/
+  ILI9341_ACTIVE = (3),             /**< Exchanging data.*/
+} ili9341state_t;
+
+/**
+ * @brief   ILI9341 driver.
+ */
+typedef struct ILI9341Driver {
+  ili9341state_t        state;      /**< Driver state.*/
+  const ILI9341Config   *config;    /**< Driver configuration.*/
+
+  /* Multithreading stuff.*/
+#if (TRUE == ILI9341_USE_MUTUAL_EXCLUSION)
+#if (TRUE == CH_CFG_USE_MUTEXES)
+  mutex_t               lock;       /**< Multithreading lock.*/
+#elif (TRUE == CH_CFG_USE_SEMAPHORES)
+  semaphore_t           lock;       /**< Multithreading lock.*/
+#endif
+#endif /* (TRUE == ILI9341_USE_MUTUAL_EXCLUSION) */
+
+  /* Temporary variables.*/
+  uint8_t               value;      /**< Non-stacked value, for SPI with CCM.*/
+} ILI9341Driver;
+
+/**
+ * @name    ILI9341 command params (little endian)
+ * @{
+ */
+#pragma pack(push, 1)
+
+typedef union {
+  struct ILI9341ParamBits_GET_ID_INFO {
+    uint8_t reserved_;
+    uint8_t ID1;
+    uint8_t ID2;
+    uint8_t ID3;
+  } bits;
+  uint8_t   bytes[4];
+} ILI9341Params_GET_ID_INFO;
+
+typedef union {
+  struct ILI9341ParamBits_GET_STATUS {
+    unsigned  _reserved_1       : 5;    /* D[ 4: 0] */
+    unsigned  tearing_mode      : 1;    /* D[ 5] */
+    unsigned  gamma_curve       : 3;    /* D[ 8: 6] */
+    unsigned  tearing           : 1;    /* D[ 9] */
+    unsigned  display           : 1;    /* D[10] */
+    unsigned  all_on            : 1;    /* D[11] */
+    unsigned  all_off           : 1;    /* D[12] */
+    unsigned  invert            : 1;    /* D[13] */
+    unsigned  _reserved_2       : 1;    /* D[14] */
+    unsigned  vscroll           : 1;    /* D[15] */
+    unsigned  normal            : 1;    /* D[16] */
+    unsigned  sleep             : 1;    /* D[17] */
+    unsigned  partial           : 1;    /* D[18] */
+    unsigned  idle              : 1;    /* D[19] */
+    unsigned  pixel_format      : 3;    /* D[22:20] */
+    unsigned  _reserved_3       : 2;    /* D[24:23] */
+    unsigned  hrefr_rtl_nltr    : 1;    /* D[25] */
+    unsigned  bgr_nrgb          : 1;    /* D[26] */
+    unsigned  vrefr_btt_nttb    : 1;    /* D[27] */
+    unsigned  transpose         : 1;    /* D[28] */
+    unsigned  coladr_rtl_nltr   : 1;    /* D[29] */
+    unsigned  rowadr_btt_nttb   : 1;    /* D[30] */
+    unsigned  booster           : 1;    /* D[31] */
+  } bits;
+  uint8_t   bytes[4];
+} ILI9341Params_GET_STATUS;
+
+typedef union {
+  struct ILI9341ParamBits_GET_PWR_MODE {
+    unsigned  _reserved_1       : 2;    /* D[1:0] */
+    unsigned  display           : 1;    /* D[2] */
+    unsigned  normal            : 1;    /* D[3] */
+    unsigned  sleep             : 1;    /* D[4] */
+    unsigned  partial           : 1;    /* D[5] */
+    unsigned  idle              : 1;    /* D[6] */
+    unsigned  booster           : 1;    /* D[7] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_PWR_MODE;
+
+typedef union {
+  struct ILI9341ParamBits_GET_MADCTL {
+    unsigned  _reserved_1       : 2;    /* D[1:0] */
+    unsigned  refr_rtl_nltr     : 1;    /* D[2] */
+    unsigned  bgr_nrgb          : 1;    /* D[3] */
+    unsigned  refr_btt_nttb     : 1;    /* D[4] */
+    unsigned  invert            : 1;    /* D[5] */
+    unsigned  rtl_nltr          : 1;    /* D[6] */
+    unsigned  btt_nttb          : 1;    /* D[7] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_MADCTL;
+
+typedef union {
+  struct ILI9341ParamBits_GET_PIX_FMT {
+    unsigned  DBI               : 3;    /* D[2:0] */
+    unsigned  _reserved_1       : 1;    /* D[3] */
+    unsigned  DPI               : 3;    /* D[6:4] */
+    unsigned  RIM               : 1;    /* D[7] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_PIX_FMT;
+
+typedef union {
+  struct ILI9341ParamBits_GET_IMG_FMT {
+    unsigned  gamma_curve       : 3;    /* D[2:0] */
+    unsigned  _reserved_1       : 5;    /* D[7:3] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_IMG_FMT;
+
+typedef union {
+  struct ILI9341ParamBits_GET_SIG_MODE {
+    unsigned  _reserved_1       : 2;    /* D[1:0] */
+    unsigned  data_enable       : 1;    /* D[2] */
+    unsigned  pixel_clock       : 1;    /* D[3] */
+    unsigned  vsync             : 1;    /* D[4] */
+    unsigned  hsync             : 1;    /* D[5] */
+    unsigned  tearing_mode      : 1;    /* D[6] */
+    unsigned  tearing           : 1;    /* D[7] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_SIG_MODE;
+
+typedef union {
+  struct ILI9341ParamBits_GET_SELF_DIAG {
+    unsigned  _reserved_1       : 6;    /* D[5:0] */
+    unsigned  func_err          : 1;    /* D[6] */
+    unsigned  reg_err           : 1;    /* D[7] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_SELF_DIAG;
+
+typedef union {
+  struct ILI9341ParamBits_SET_GAMMA {
+    uint8_t   gamma_curve;              /* D[7:0] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_SET_GAMMA;
+
+typedef union {
+  struct ILI9341ParamBits_SET_COL_ADDR {
+    uint8_t   SC_15_8;                  /* D[ 7: 0] */
+    uint8_t   SC_7_0;                   /* D[15: 8] */
+    uint8_t   EC_15_8;                  /* D[23:16] */
+    uint8_t   EC_7_0;                   /* D[31:24] */
+  } bits;
+  uint8_t   bytes[4];
+} ILI9341Params_SET_COL_ADDR;
+
+typedef union {
+  struct ILI9341ParamBits_SET_PAGE_ADDR {
+    uint8_t   SP_15_8;                  /* D[ 7: 0] */
+    uint8_t   SP_7_0;                   /* D[15: 8] */
+    uint8_t   EP_15_8;                  /* D[23:16] */
+    uint8_t   EP_7_0;                   /* D[31:24] */
+  } bits;
+  uint8_t   bytes[4];
+} ILI9341Params_SET_PAGE_ADDR;
+
+typedef union {
+  struct ILI9341ParamBits_SET_PARTIAL_AREA {
+    uint8_t   SR_15_8;                  /* D[ 7: 0] */
+    uint8_t   SR_7_0;                   /* D[15: 8] */
+    uint8_t   ER_15_8;                  /* D[23:16] */
+    uint8_t   ER_7_0;                   /* D[31:24] */
+  } bits;
+  uint8_t   bytes[4];
+} ILI9341Params_SET_PARTIAL_AREA;
+
+typedef union {
+  struct ILI9341ParamBits_SET_VSCROLL {
+    uint8_t   TFA_15_8;                 /* D[ 7: 0] */
+    uint8_t   TFA_7_0;                  /* D[15: 8] */
+    uint8_t   VSA_15_8;                 /* D[23:16] */
+    uint8_t   VSA_7_0;                  /* D[31:24] */
+    uint8_t   BFA_15_8;                 /* D[39:32] */
+    uint8_t   BFA_7_0;                  /* D[47:40] */
+  } bits;
+  uint8_t   bytes[6];
+} ILI9341Params_SET_VSCROLL;
+
+typedef union {
+  struct ILI9341ParamBits_CMD_TEARING_ON {
+    unsigned  M                 : 1;    /* D[0] */
+    unsigned  _reserved_1       : 7;    /* D[7:1] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_CMD_TEARING_ON;
+
+typedef union {
+  struct ILI9341ParamBits_SET_MEM_ACS_CTL {
+    unsigned  _reserved_1       : 2;    /* D[1:0] */
+    unsigned  MH                : 1;    /* D[2] */
+    unsigned  BGR               : 1;    /* D[3] */
+    unsigned  ML                : 1;    /* D[4] */
+    unsigned  MV                : 1;    /* D[5] */
+    unsigned  MX                : 1;    /* D[6] */
+    unsigned  MY                : 1;    /* D[7] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_SET_MEM_ACS_CTL;
+
+typedef union {
+  struct ILI9341ParamBits_SET_VSCROLL_ADDR {
+    uint8_t   VSP_15_8;                 /* D[ 7: 0] */
+    uint8_t   VSP_7_0;                  /* D[15: 8] */
+  } bits;
+  uint8_t   bytes[2];
+} ILI9341Params_SET_VSCROLL_ADDR;
+
+typedef union {
+  struct ILI9341ParamBits_SET_PIX_FMT {
+    unsigned  DBI               : 3;    /* D[2:0] */
+    unsigned  _reserved_1       : 1;    /* D[3] */
+    unsigned  DPI               : 3;    /* D[4:6] */
+    unsigned  _reserved_2       : 1;    /* D[7] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_SET_PIX_FMT;
+
+typedef union {
+  struct ILI9341ParamBits_SET_TEAR_SCANLINE {
+    uint8_t   STS_8;                    /* D[ 7: 0] */
+    uint8_t   STS_7_0;                  /* D[15: 8] */
+  } bits;
+  uint8_t   bytes[4];
+} ILI9341Params_SET_TEAR_SCANLINE;
+
+typedef union {
+  struct ILI9341ParamBits_GET_TEAR_SCANLINE {
+    uint8_t   GTS_9_8;                  /* D[ 7: 0] */
+    uint8_t   GTS_7_0;                  /* D[15: 8] */
+  } bits;
+  uint8_t   bytes[2];
+} ILI9341Params_GET_TEAR_SCANLINE;
+
+typedef union {
+  struct ILI9341ParamBits_SET_BRIGHTNESS {
+    uint8_t   DBV;                      /* D[7:0] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_SET_BRIGHTNESS;
+
+typedef union {
+  struct ILI9341ParamBits_GET_BRIGHTNESS {
+    uint8_t   DBV;                      /* D[7:0] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_BRIGHTNESS;
+
+typedef union {
+  struct ILI9341ParamBits_SET_DISPLAY_CTL {
+    unsigned  _reserved_1       : 2;    /* D[1:0] */
+    unsigned  BL                : 1;    /* D[2] */
+    unsigned  DD                : 1;    /* D[3] */
+    unsigned  _reserved_2       : 1;    /* D[4] */
+    unsigned  BCTRL             : 1;    /* D[5] */
+    unsigned  _reserved_3       : 1;    /* D[7:6] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_SET_DISPLAY_CTL;
+
+typedef union {
+  struct ILI9341ParamBits_GET_DISPLAY_CTL {
+    unsigned  _reserved_1       : 2;    /* D[1:0] */
+    unsigned  BL                : 1;    /* D[2] */
+    unsigned  DD                : 1;    /* D[3] */
+    unsigned  _reserved_2       : 1;    /* D[4] */
+    unsigned  BCTRL             : 1;    /* D[5] */
+    unsigned  _reserved_3       : 1;    /* D[7:6] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_DISPLAY_CTL;
+
+typedef union {
+  struct ILI9341ParamBits_SET_CABC {
+    unsigned  C                 : 2;    /* D[1:0] */
+    unsigned  _reserved_1       : 6;    /* D[7:2] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_SET_CABC;
+
+typedef union {
+  struct ILI9341ParamBits_GET_CABC {
+    unsigned  C                 : 2;    /* D[1:0] */
+    unsigned  _reserved_1       : 6;    /* D[7:2] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_CABC;
+
+typedef union {
+  struct ILI9341ParamBits_SET_CABC_MIN {
+    uint8_t   CMB;                      /* D[7:0] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_SET_CABC_MIN;
+
+typedef union {
+  struct ILI9341ParamBits_GET_CABC_MIN {
+    uint8_t   CMB;                      /* D[7:0] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_GET_CABC_MIN;
+
+#if 0 /* TODO: Extended command structs.*/
+
+typedef union {
+  struct ILI9341ParamBits {
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_;
+
+typedef union {
+  struct ILI9341ParamBits {
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+    unsigned    : 1;  /* D[] */
+  } bits;
+  uint8_t   bytes[1];
+} ILI9341Params_;
+
+#endif /*0*/
+
+#pragma pack(pop)
+
+/** @} */
+
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+extern ILI9341Driver ILI9341D1;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+  void ili9341ObjectInit(ILI9341Driver *driverp);
+  void ili9341Start(ILI9341Driver *driverp, const ILI9341Config *configp);
+  void ili9341Stop(ILI9341Driver *driverp);
+#if (ILI9341_USE_MUTUAL_EXCLUSION == TRUE)
+  void ili9341AcquireBusS(ILI9341Driver *driverp);
+  void ili9341AcquireBus(ILI9341Driver *driverp);
+  void ili9341ReleaseBusS(ILI9341Driver *driverp);
+  void ili9341ReleaseBus(ILI9341Driver *driverp);
+#endif /* (ILI9341_USE_MUTUAL_EXCLUSION == TRUE) */
+  void ili9341SelectI(ILI9341Driver *driverp);
+  void ili9341Select(ILI9341Driver *driverp);
+  void ili9341UnselectI(ILI9341Driver *driverp);
+  void ili9341Unselect(ILI9341Driver *driverp);
+  void ili9341WriteCommand(ILI9341Driver *driverp, uint8_t cmd);
+  void ili9341WriteByte(ILI9341Driver *driverp, uint8_t value);
+  uint8_t ili9341ReadByte(ILI9341Driver *driverp);
+  void ili9341WriteChunk(ILI9341Driver *driverp, const uint8_t chunk[],
+                         size_t length);
+  void ili9341ReadChunk(ILI9341Driver *driverp, uint8_t chunk[],
+                        size_t length);
+
+#ifdef __cplusplus
+}
+#endif
+
+/** @} */
+
+#endif /* _ILI9341_H_ */
diff --git a/lib/chibios-contrib/os/various/devices_lib/mems/l3gd20.c b/lib/chibios-contrib/os/various/devices_lib/mems/l3gd20.c
new file mode 100644
index 000000000..1cc52c931
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/mems/l3gd20.c
@@ -0,0 +1,123 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    l3gd20.c
+ * @brief   L3GD20 MEMS interface module code.
+ *
+ * @addtogroup l3gd20
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#include "l3gd20.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Reads a generic register value.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] reg       register number
+ * @return              register value.
+ */
+uint8_t l3gd20ReadRegister(SPIDriver *spip, uint8_t reg) {
+  uint8_t txbuf[2] = {L3GD20_RW | reg, 0xFF};
+  uint8_t rxbuf[2] = {0x00, 0x00};
+  spiSelect(spip);
+  spiExchange(spip, 2, txbuf, rxbuf);
+  spiUnselect(spip);
+  return rxbuf[1];
+}
+
+
+void l3gd20WriteRegister(SPIDriver *spip, uint8_t reg, uint8_t value) {
+
+  switch (reg) {
+
+    default:
+      /* Reserved register must not be written, according to the datasheet
+       * this could permanently damage the device.
+       */
+      chDbgAssert(FALSE, "lg3d20WriteRegister(), reserved register");
+    case L3GD20_AD_WHO_AM_I:
+    case L3GD20_AD_OUT_TEMP :
+    case L3GD20_AD_STATUS_REG:
+    case L3GD20_AD_OUT_X_L:
+    case L3GD20_AD_OUT_X_H:
+    case L3GD20_AD_OUT_Y_L:
+    case L3GD20_AD_OUT_Y_H:
+    case L3GD20_AD_OUT_Z_L:
+    case L3GD20_AD_OUT_Z_H:
+    case L3GD20_AD_FIFO_SRC_REG:
+    case L3GD20_AD_INT1_SRC:
+    /* Read only registers cannot be written, the command is ignored.*/
+      return;
+    case L3GD20_AD_CTRL_REG1:
+    case L3GD20_AD_CTRL_REG2:
+    case L3GD20_AD_CTRL_REG3:
+    case L3GD20_AD_CTRL_REG4:
+    case L3GD20_AD_CTRL_REG5:
+    case L3GD20_AD_REFERENCE:
+    case L3GD20_AD_FIFO_CTRL_REG:
+    case L3GD20_AD_INT1_CFG:
+    case L3GD20_AD_INT1_TSH_XH:
+    case L3GD20_AD_INT1_TSH_XL:
+    case L3GD20_AD_INT1_TSH_YH:
+    case L3GD20_AD_INT1_TSH_YL:
+    case L3GD20_AD_INT1_TSH_ZH:
+    case L3GD20_AD_INT1_TSH_ZL:
+    case L3GD20_AD_INT1_DURATION:
+      spiSelect(spip);
+      uint8_t txbuf[2] = {reg, value};
+      spiSend(spip, 2, txbuf);
+      spiUnselect(spip);
+  }
+}
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/mems/l3gd20.h b/lib/chibios-contrib/os/various/devices_lib/mems/l3gd20.h
new file mode 100644
index 000000000..08d90927e
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/mems/l3gd20.h
@@ -0,0 +1,243 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    l3gd20.h
+ * @brief   L3GD20 MEMS interface module header.
+ *
+ * @{
+ */
+
+#ifndef _L3GD20_H_
+#define _L3GD20_H_
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+#define  L3GD20_SENS_250DPS                      ((float)131.072f)          /*!< gyroscope sensitivity with 250 dps full scale [LSB/dps] */
+#define  L3GD20_SENS_500DPS                      ((float)65.536f)           /*!< gyroscope sensitivity with 500 dps full scale [LSB/dps] */
+#define  L3GD20_SENS_2000DPS                     ((float)16.384f)           /*!< gyroscope sensitivity with 2000 dps full scale [LSB/dps] */
+/**
+ * @name    L3GD20 register names
+ * @{
+ */
+/******************************************************************************/
+/*                                                                            */
+/*                        L3GD20 on board MEMS                                */
+/*                                                                            */
+/******************************************************************************/
+/*******************  Bit definition for SPI communication  *******************/
+#define  L3GD20_DI                               ((uint8_t)0xFF)            /*!< DI[7:0] Data input */
+#define  L3GD20_DI_0                             ((uint8_t)0x01)            /*!< bit 0 */
+#define  L3GD20_DI_1                             ((uint8_t)0x02)            /*!< bit 1 */
+#define  L3GD20_DI_2                             ((uint8_t)0x04)            /*!< bit 2 */
+#define  L3GD20_DI_3                             ((uint8_t)0x08)            /*!< bit 3 */
+#define  L3GD20_DI_4                             ((uint8_t)0x10)            /*!< bit 4 */
+#define  L3GD20_DI_5                             ((uint8_t)0x20)            /*!< bit 5 */
+#define  L3GD20_DI_6                             ((uint8_t)0x40)            /*!< bit 6 */
+#define  L3GD20_DI_7                             ((uint8_t)0x80)            /*!< bit 7 */
+
+#define  L3GD20_AD                               ((uint8_t)0x3F)            /*!< AD[5:0] Address Data */
+#define  L3GD20_AD_0                             ((uint8_t)0x01)            /*!< bit 0 */
+#define  L3GD20_AD_1                             ((uint8_t)0x02)            /*!< bit 1 */
+#define  L3GD20_AD_2                             ((uint8_t)0x04)            /*!< bit 2 */
+#define  L3GD20_AD_3                             ((uint8_t)0x08)            /*!< bit 3 */
+#define  L3GD20_AD_4                             ((uint8_t)0x10)            /*!< bit 4 */
+#define  L3GD20_AD_5                             ((uint8_t)0x20)            /*!< bit 5 */
+
+#define  L3GD20_MS                               ((uint8_t)0x40)            /*!< Multiple read write */
+#define  L3GD20_RW                               ((uint8_t)0x80)            /*!< Read Write, 1 0 */
+
+/******************  Bit definition for Registers Addresses *******************/
+#define  L3GD20_AD_WHO_AM_I                      ((uint8_t)0x0F)            /*!< WHO I AM */
+#define  L3GD20_AD_CTRL_REG1                     ((uint8_t)0x20)            /*!< CONTROL REGISTER 1 */
+#define  L3GD20_AD_CTRL_REG2                     ((uint8_t)0x21)            /*!< CONTROL REGISTER 2 */
+#define  L3GD20_AD_CTRL_REG3                     ((uint8_t)0x22)            /*!< CONTROL REGISTER 3 */
+#define  L3GD20_AD_CTRL_REG4                     ((uint8_t)0x23)            /*!< CONTROL REGISTER 4 */
+#define  L3GD20_AD_CTRL_REG5                     ((uint8_t)0x24)            /*!< CONTROL REGISTER 5 */
+#define  L3GD20_AD_REFERENCE                     ((uint8_t)0x25)            /*!< REFERENCE/DATACAPTURE */
+#define  L3GD20_AD_OUT_TEMP                      ((uint8_t)0x26)            /*!< MEMS ONBOARD TEMP SENSOR */
+#define  L3GD20_AD_STATUS_REG                    ((uint8_t)0x27)            /*!< STATUS REGISTER */
+#define  L3GD20_AD_OUT_X_L                       ((uint8_t)0x28)            /*!< OUTPUT X-AXIS LOW */
+#define  L3GD20_AD_OUT_X_H                       ((uint8_t)0x29)            /*!< OUTPUT X-AXIS HIGH */
+#define  L3GD20_AD_OUT_Y_L                       ((uint8_t)0x2A)            /*!< OUTPUT Y-AXIS LOW */
+#define  L3GD20_AD_OUT_Y_H                       ((uint8_t)0x2B)            /*!< OUTPUT Y-AXIS HIGH */
+#define  L3GD20_AD_OUT_Z_L                       ((uint8_t)0x2C)            /*!< OUTPUT Z-AXIS LOW */
+#define  L3GD20_AD_OUT_Z_H                       ((uint8_t)0x2D)            /*!< OUTPUT Z-AXIS HIGH */
+#define  L3GD20_AD_FIFO_CTRL_REG                 ((uint8_t)0x2E)            /*!< FIFO CONTROL REGISTER */
+#define  L3GD20_AD_FIFO_SRC_REG                  ((uint8_t)0x2F)            /*!< FIFO SOURCE REGISTER */
+#define  L3GD20_AD_INT1_CFG                      ((uint8_t)0x30)            /*!< INTERRUPT1 CONFIG REGISTER */
+#define  L3GD20_AD_INT1_SRC                      ((uint8_t)0x31)            /*!< INTERRUPT1 SOURCE REGISTER */
+#define  L3GD20_AD_INT1_TSH_XH                   ((uint8_t)0x32)            /*!< INTERRUPT1 THRESHOLD X-AXIS HIGH */
+#define  L3GD20_AD_INT1_TSH_XL                   ((uint8_t)0x33)            /*!< INTERRUPT1 THRESHOLD X-AXIS LOW */
+#define  L3GD20_AD_INT1_TSH_YH                   ((uint8_t)0x34)            /*!< INTERRUPT1 THRESHOLD Y-AXIS HIGH */
+#define  L3GD20_AD_INT1_TSH_YL                   ((uint8_t)0x35)            /*!< INTERRUPT1 THRESHOLD Y-AXIS LOW */
+#define  L3GD20_AD_INT1_TSH_ZH                   ((uint8_t)0x36)            /*!< INTERRUPT1 THRESHOLD Z-AXIS HIGH */
+#define  L3GD20_AD_INT1_TSH_ZL                   ((uint8_t)0x37)            /*!< INTERRUPT1 THRESHOLD Z-AXIS LOW */
+#define  L3GD20_AD_INT1_DURATION                 ((uint8_t)0x38)            /*!< INTERRUPT1 DURATION */
+
+/** @} */
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Gyroscope data structures and types
+ * @{
+ */
+
+/**
+ * @brief  Gyroscope Output Data Rate
+ */
+typedef enum {
+  L3GD20_ODR_95Hz_Fc_12_5  = 0x00,          /*!< Output Data Rate = 95 Hz -  LPF Cut-Off = 12.5 Hz */
+  L3GD20_ODR_95Hz_Fc_25    = 0x10,          /*!< Output Data Rate = 95 Hz -  LPF Cut-Off = 25 Hz */
+  L3GD20_ODR_190Hz_Fc_12_5 = 0x40,          /*!< Output Data Rate = 190 Hz -  LPF Cut-Off = 12.5 Hz */
+  L3GD20_ODR_190Hz_Fc_25   = 0x50,          /*!< Output Data Rate = 190 Hz -  LPF Cut-Off = 25 Hz */
+  L3GD20_ODR_190Hz_Fc_50   = 0x60,          /*!< Output Data Rate = 190 Hz -  LPF Cut-Off = 50 Hz */
+  L3GD20_ODR_190Hz_Fc_70   = 0x70,          /*!< Output Data Rate = 190 Hz -  LPF Cut-Off = 70 Hz */
+  L3GD20_ODR_380Hz_Fc_20   = 0x80,          /*!< Output Data Rate = 380 Hz -  LPF Cut-Off = 20 Hz */
+  L3GD20_ODR_380Hz_Fc_25   = 0x90,          /*!< Output Data Rate = 380 Hz -  LPF Cut-Off = 25 Hz */
+  L3GD20_ODR_380Hz_Fc_50   = 0xA0,          /*!< Output Data Rate = 380 Hz -  LPF Cut-Off = 50 Hz */
+  L3GD20_ODR_380Hz_Fc_100  = 0xB0,          /*!< Output Data Rate = 380 Hz -  LPF Cut-Off = 100 Hz */
+  L3GD20_ODR_760Hz_Fc_30   = 0xC0,          /*!< Output Data Rate = 760 Hz -  LPF Cut-Off = 30 Hz */
+  L3GD20_ODR_760Hz_Fc_35   = 0xD0,          /*!< Output Data Rate = 760 Hz -  LPF Cut-Off = 35 Hz */
+  L3GD20_ODR_760Hz_Fc_50   = 0xE0,          /*!< Output Data Rate = 760 Hz -  LPF Cut-Off = 50 Hz */
+  L3GD20_ODR_760Hz_Fc_100  = 0xF0           /*!< Output Data Rate = 760 Hz -  LPF Cut-Off = 100 Hz */
+}L3GD20_ODR_t;
+
+/**
+ * @brief  Gyroscope Power Mode
+ */
+typedef enum {
+  L3GD20_PM_POWER_DOWN   = 0x00,            /*!< Normal mode enabled */
+  L3GD20_PM_SLEEP_NORMAL = 0x08             /*!< Low Power mode enabled */
+}L3GD20_PM_t;
+
+/**
+ * @brief  Gyroscope Full Scale
+ */
+typedef enum {
+  L3GD20_FS_250DPS  = 0x00,                 /*!< �250 dps */
+  L3GD20_FS_500DPS  = 0x10,                 /*!< �500 dps */
+  L3GD20_FS_2000DPS = 0x20                  /*!< �200 dps */
+}L3GD20_FS_t;
+
+/**
+ * @brief   Gyroscope Axes Enabling
+ */
+typedef enum {
+  L3GD20_AE_DISABLED = 0x00,                /*!< All disabled */
+  L3GD20_AE_X        = 0x01,                /*!< Only X */
+  L3GD20_AE_Y        = 0x02,                /*!< Only Y */
+  L3GD20_AE_XY       = 0x03,                /*!< X & Y */
+  L3GD20_AE_Z        = 0x04,                /*!< Only Z */
+  L3GD20_AE_XZ       = 0x05,                /*!< X & Z */
+  L3GD20_AE_YZ       = 0x06,                /*!< Y & Z */
+  L3GD20_AE_XYZ      = 0x07                 /*!< All enabled */
+}L3GD20_AE_t;
+
+/**
+ * @brief  Gyroscope Block Data Update
+ */
+typedef enum {
+  L3GD20_BDU_CONTINOUS = 0x00,              /*!< Continuos Update */
+  L3GD20_BDU_BLOCKED   = 0x80               /*!< Single Update: output registers not updated until MSB and LSB reading */
+}L3GD20_BDU_t;
+
+/**
+ * @brief  Gyroscope Endianness
+ */
+typedef enum {
+  L3GD20_End_LITTLE = 0x00,                 /*!< Little Endian: data LSB @ lower address */
+  L3GD20_End_BIG    = 0x40                  /*!< Big Endian: data MSB @ lower address */
+}L3GD20_End_t;
+
+
+/**
+ * @brief   Gyroscope configuration structure.
+ */
+typedef struct {
+  /**
+   * @brief Gyroscope fullscale value.
+   */
+  L3GD20_FS_t    fullscale;
+  /**
+   * @brief Gyroscope power mode selection.
+   */
+  L3GD20_PM_t    powermode;
+  /**
+   * @brief Gyroscope output data rate selection.
+   */
+  L3GD20_ODR_t   outputdatarate;
+  /**
+   * @brief Gyroscope axes enabling.
+   */
+  L3GD20_AE_t    axesenabling;
+  /**
+   * @brief Gyroscope endianess.
+   */
+  L3GD20_End_t   endianess;
+  /**
+   * @brief Gyroscope block data update.
+   */
+  L3GD20_BDU_t   blockdataupdate;
+} L3GD20_Config;
+/** @}  */
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+  uint8_t l3gd20ReadRegister(SPIDriver *spip, uint8_t reg);
+  void l3gd20WriteRegister(SPIDriver *spip, uint8_t reg, uint8_t value);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _L3GD20_H_ */
+
+/** @} */
+
diff --git a/lib/chibios-contrib/os/various/devices_lib/mems/lis3mdl.c b/lib/chibios-contrib/os/various/devices_lib/mems/lis3mdl.c
new file mode 100644
index 000000000..99b71e45e
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/mems/lis3mdl.c
@@ -0,0 +1,151 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    lis3mdl.c
+ * @brief   LIS3MDL MEMS interface module through I2C code.
+ *
+ * @addtogroup lis3mdl
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#include "lis3mdl.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Reads a generic sub-register value.
+ * @pre     The I2C interface must be initialized and the driver started.
+ *
+ * @param[in] i2cp      pointer to the I2C interface
+ * @param[in] sad       slave address without R bit
+ * @param[in] sub       sub-register address
+ * @param[in] message   pointer to message
+ * @return              register value.
+ */
+uint8_t lis3mdlReadRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                                 msg_t* message) {
+
+  uint8_t txbuf, rxbuf[2];
+#if defined(STM32F103_MCUCONF)
+  txbuf = LSM303DLHC_SUB_MSB | sub;
+  if(message != NULL){
+    *message = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,
+                                        TIME_INFINITE);
+  }
+  else{
+    i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2, TIME_INFINITE);
+  }
+  return rxbuf[0];
+#else
+  txbuf = sub;
+  if(message != NULL){
+    *message = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 1,
+                                        TIME_INFINITE);
+  }
+  else{
+    i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 1, TIME_INFINITE);
+  }
+  return rxbuf[0];
+#endif
+}
+
+/**
+ * @brief   Writes a value into a register.
+ * @pre     The I2C interface must be initialized and the driver started.
+ *
+ * @param[in] i2cp       pointer to the I2C interface
+ * @param[in] sad        slave address without R bit
+ * @param[in] sub        sub-register address
+ * @param[in] value      the value to be written
+ * @param[out] message   pointer to message
+ */
+void lis3mdlWriteRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                               uint8_t value, msg_t* message) {
+
+  uint8_t txbuf[2];
+  uint8_t rxbuf;
+  switch (sub) {
+  default:
+    /* Reserved register must not be written, according to the datasheet
+     * this could permanently damage the device.
+     */
+    chDbgAssert(FALSE, "lis3mdlWriteRegister(), reserved register");
+  case LIS3MDL_SUB_WHO_AM_I:
+  case LIS3MDL_SUB_STATUS_REG:
+  case LIS3MDL_SUB_OUT_X_L:
+  case LIS3MDL_SUB_OUT_X_H:
+  case LIS3MDL_SUB_OUT_Y_L:
+  case LIS3MDL_SUB_OUT_Y_H:
+  case LIS3MDL_SUB_OUT_Z_L:
+  case LIS3MDL_SUB_OUT_Z_H:
+  case LIS3MDL_SUB_INT_SOURCE:
+  case LIS3MDL_SUB_INT_THS_L:
+  case LIS3MDL_SUB_INT_THS_H:
+  /* Read only registers cannot be written, the command is ignored.*/
+    return;
+  case LIS3MDL_SUB_CTRL_REG1:
+  case LIS3MDL_SUB_CTRL_REG2:
+  case LIS3MDL_SUB_CTRL_REG3:
+  case LIS3MDL_SUB_CTRL_REG4:
+  case LIS3MDL_SUB_CTRL_REG5:
+  case LIS3MDL_SUB_INT_CFG:
+    txbuf[0] = sub;
+    txbuf[1] = value;
+    if(message != NULL){
+      *message = i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0,
+                                          TIME_INFINITE);
+    }
+    else{
+      i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
+    }
+    break;
+  }
+}
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/mems/lis3mdl.h b/lib/chibios-contrib/os/various/devices_lib/mems/lis3mdl.h
new file mode 100644
index 000000000..e55978e9b
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/mems/lis3mdl.h
@@ -0,0 +1,258 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    lis3mdl.h
+ * @brief   LIS3MDL MEMS interface module header.
+ *
+ * @{
+ */
+
+#ifndef _LIS3MDL_H_
+#define _LIS3MDL_H_
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+#define  LIS3MDL_COMP_SENS_4GA           ((float)6842.0f)            /*!< compass sensitivity with 4 GA full scale [LSB  / Ga] */
+#define  LIS3MDL_COMP_SENS_8GA           ((float)3421.0f)            /*!< compass sensitivity with 8 GA full scale [LSB  / Ga] */
+#define  LIS3MDL_COMP_SENS_12GA          ((float)2281.0f)            /*!< compass sensitivity with 12 GA full scale [LSB  / Ga] */
+#define  LIS3MDL_COMP_SENS_16GA          ((float)1711.0f)            /*!< compass sensitivity with 16 GA full scale [LSB  / Ga] */
+/**
+ * @name    LIS3MDL register names
+ * @{
+ */
+/******************************************************************************/
+/*                                                                            */
+/*                        LIS3MDL on board MEMS                               */
+/*                                                                            */
+/******************************************************************************/
+/*****************  Bit definition for I2C/SPI communication  *****************/
+#define  LIS3MDL_SUB                             ((uint8_t)0x7F)            /*!< SUB[6:0] Sub-registers address Mask */
+#define  LIS3MDL_SUB_0                           ((uint8_t)0x01)            /*!< bit 0 */
+#define  LIS3MDL_SUB_1                           ((uint8_t)0x02)            /*!< bit 1 */
+#define  LIS3MDL_SUB_2                           ((uint8_t)0x08)            /*!< bit 3 */
+#define  LIS3MDL_SUB_4                           ((uint8_t)0x10)            /*!< bit 4 */
+#define  LIS3MDL_SUB_5                           ((uint8_t)0x20)            /*!< bit 5 */
+#define  LIS3MDL_SUB_6                           ((uint8_t)0x40)            /*!< bit 6 */
+
+#define  LIS3MDL_SUB_MSB                         ((uint8_t)0x80)            /*!< Multiple data read\write bit */
+
+/****************  Bit definition SUB-Registers Addresses  ********************/
+#define  LIS3MDL_SUB_WHO_AM_I                    ((uint8_t)0x0F)            /*!< CONTROL REGISTER 1 */
+#define  LIS3MDL_SUB_CTRL_REG1                   ((uint8_t)0x20)            /*!< CONTROL REGISTER 1 */
+#define  LIS3MDL_SUB_CTRL_REG2                   ((uint8_t)0x21)            /*!< CONTROL REGISTER 2 */
+#define  LIS3MDL_SUB_CTRL_REG3                   ((uint8_t)0x22)            /*!< CONTROL REGISTER 3 */
+#define  LIS3MDL_SUB_CTRL_REG4                   ((uint8_t)0x23)            /*!< CONTROL REGISTER 4 */
+#define  LIS3MDL_SUB_CTRL_REG5                   ((uint8_t)0x24)            /*!< CONTROL REGISTER 5 */
+#define  LIS3MDL_SUB_STATUS_REG                  ((uint8_t)0x27)            /*!< STATUS REGISTER */
+#define  LIS3MDL_SUB_OUT_X_L                     ((uint8_t)0x28)            /*!< OUTPUT X-AXIS LOW */
+#define  LIS3MDL_SUB_OUT_X_H                     ((uint8_t)0x29)            /*!< OUTPUT X-AXIS HIGH */
+#define  LIS3MDL_SUB_OUT_Y_L                     ((uint8_t)0x2A)            /*!< OUTPUT Y-AXIS LOW */
+#define  LIS3MDL_SUB_OUT_Y_H                     ((uint8_t)0x2B)            /*!< OUTPUT Y-AXIS HIGH */
+#define  LIS3MDL_SUB_OUT_Z_L                     ((uint8_t)0x2C)            /*!< OUTPUT Z-AXIS LOW */
+#define  LIS3MDL_SUB_OUT_Z_H                     ((uint8_t)0x2D)            /*!< OUTPUT Z-AXIS HIGH */
+#define  LIS3MDL_SUB_INT_CFG                     ((uint8_t)0x30)            /*!< INTERRUPT1 CONFIG */
+#define  LIS3MDL_SUB_INT_SOURCE                  ((uint8_t)0x31)            /*!< INTERRUPT1 SOURCE */
+#define  LIS3MDL_SUB_INT_THS_L                   ((uint8_t)0x32)            /*!< INTERRUPT1 THRESHOLD */
+#define  LIS3MDL_SUB_INT_THS_H                   ((uint8_t)0x33)            /*!< INTERRUPT1 DURATION */
+
+/** @} */
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Compass data structures and types
+ * @{
+ */
+
+/**
+ * @brief  Compass Slave Address
+ */
+typedef enum {
+  LIS3MDL_SAD_GND = 0x1C,                             /*!< COMPASS Slave Address when SA1 is to GND */
+  LIS3MDL_SAD_VCC = 0x1E                              /*!< COMPASS Slave Address when SA1 is to VCC */
+}LIS3MDL_SAD_t;
+
+/**
+ * @brief  Compass Operation Mode for X and Y axes
+ */
+typedef enum {
+  LIS3MDL_OMXY_LOW_POWER          = 0x00,             /*!< Operation Mode XY low power */
+  LIS3MDL_OMXY_MEDIUM_PERFORMANCE = 0x20,             /*!< Operation Mode XY medium performance */
+  LIS3MDL_OMXY_HIGH_PERFORMANCE   = 0x40,             /*!< Operation Mode XY high performance */
+  LIS3MDL_OMXY_ULTRA_PERFORMANCE  = 0x60              /*!< Operation Mode XY ultra performance */
+}LIS3MDL_OMXY_t;
+
+/**
+ * @brief  Compass Output Data Rate
+ */
+typedef enum {
+  LIS3MDL_ODR_0_625Hz = 0x00,                         /*!< Output Data Rate = 0.625 Hz */
+  LIS3MDL_ODR_1_25Hz  = 0x04,                         /*!< Output Data Rate = 1.25 Hz */
+  LIS3MDL_ODR_2_5Hz   = 0x08,                         /*!< Output Data Rate = 2.5 Hz */
+  LIS3MDL_ODR_5Hz     = 0x0C,                         /*!< Output Data Rate = 5 Hz */
+  LIS3MDL_ODR_10Hz    = 0x10,                         /*!< Output Data Rate = 10 Hz */
+  LIS3MDL_ODR_20Hz    = 0x14,                         /*!< Output Data Rate = 20 Hz */
+  LIS3MDL_ODR_40Hz    = 0x18,                         /*!< Output Data Rate = 40 Hz */
+  LIS3MDL_ODR_80Hz    = 0x1C                          /*!< Output Data Rate = 80 Hz */
+}LIS3MDL_ODR_t;
+
+/**
+ * @brief  Compass Full Scale
+ */
+typedef enum {
+  LIS3MDL_FS_4GA  = 0x00,                             /*!< �4 Gauss */
+  LIS3MDL_FS_8GA  = 0x02,                             /*!< �8 Gauss */
+  LIS3MDL_FS_12GA = 0x04,                             /*!< �12 Gauss */
+  LIS3MDL_FS_16GA = 0x0C                              /*!< �16 Gauss */
+}LIS3MDL_FS_t;
+
+/**
+ * @brief  Compass Low Mode configuration
+ */
+typedef enum {
+  LIS3MDL_LOW_POWER_DISABLED = 0x00,                   /*!< Low Power mode disabled */
+  LIS3MDL_LOW_POWER_ENABLED  = 0x20                    /*!< Low Power mode enabled */
+}LIS3MDL_PM_t;
+
+/**
+ * @brief  Compass Mode
+ */
+typedef enum {
+  LIS3MDL_MD_CONTINOUS_CONVERSION = 0x00,              /*!< Continous conversion mode */
+  LIS3MDL_MD_SINGLE_CONVERSION    = 0x01,              /*!< Single conversion mode */
+  LIS3MDL_MD_POWER_DOWN           = 0x02               /*!< Power down mode */
+}LIS3MDL_MD_t;
+
+
+/**
+ * @brief  Compass Operation Mode for Z axis
+ */
+typedef enum {
+  LIS3MDL_OMZ_LOW_POWER          = 0x00,               /*!< Operation Mode Z low power */
+  LIS3MDL_OMZ_MEDIUM_PERFORMANCE = 0x04,               /*!< Operation Mode Z medium performance */
+  LIS3MDL_OMZ_HIGH_PERFORMANCE   = 0x08,               /*!< Operation Mode Z high performance */
+  LIS3MDL_OMZ_ULTRA_PERFORMANCE  = 0x0C                /*!< Operation Mode Z ultra performance */
+}LIS3MDL_OMZ_t;
+
+/**
+ * @brief  Compass Endianness
+ */
+typedef enum {
+  LIS3MDL_End_LITTLE = 0x00,                 /*!< Little Endian: data LSB @ lower address */
+  LIS3MDL_End_BIG    = 0x02                  /*!< Big Endian: data MSB @ lower address */
+}LIS3MDL_End_t;
+
+/**
+ * @brief  Compass Block Data Update
+ */
+typedef enum {
+  LIS3MDL_BDU_CONTINOUS = 0x00,              /*!< Continuos Update */
+  LIS3MDL_BDU_BLOCKED   = 0x40               /*!< Single Update: output registers not updated until MSB and LSB reading */
+}LIS3MDL_BDU_t;
+
+
+
+
+/**
+ * @brief   Gyroscope configuration structure.
+ */
+typedef struct {
+  /**
+   * @brief  Compass Slave Address
+   */
+  LIS3MDL_SAD_t   slaveaddress;
+  /**
+   * @brief  Compass Operation Mode for X and Y axes
+   */
+  LIS3MDL_OMXY_t  opmodexy;
+  /**
+   * @brief  Compass Output Data Rate
+   */
+  LIS3MDL_ODR_t   outputdatarate;
+  /**
+   * @brief  Compass Full Scale
+   */
+  LIS3MDL_FS_t    fullscale;
+  /**
+   * @brief  Compass Low Mode configuration
+   */
+  LIS3MDL_PM_t    lowpowermode;
+  /**
+   * @brief  Compass Mode
+   */
+  LIS3MDL_MD_t    mode;
+  /**
+   * @brief  Compass Operation Mode for Z axis
+   */
+  LIS3MDL_OMZ_t   opmodez;
+  /**
+   * @brief  Compass Endianness
+   */
+  LIS3MDL_End_t   endianess;
+  /**
+   * @brief  Compass Block Data Update
+   */
+  LIS3MDL_BDU_t   blockdataupdate;
+} LIS3MDL_Config;
+/** @}  */
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+  uint8_t lis3mdlReadRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                                 msg_t* message);
+  void lis3mdlWriteRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                                 uint8_t value, msg_t* message);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _LIS3MDL_H_ */
+
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/mems/lsm303dlhc.c b/lib/chibios-contrib/os/various/devices_lib/mems/lsm303dlhc.c
new file mode 100644
index 000000000..070c49cc8
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/mems/lsm303dlhc.c
@@ -0,0 +1,205 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    lsm303dlhc.c
+ * @brief   LSM303DLHC MEMS interface module through I2C code.
+ *
+ * @addtogroup lsm303dlhc
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#include "lsm303dlhc.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Reads a generic sub-register value.
+ * @pre     The I2C interface must be initialized and the driver started.
+ *
+ * @param[in] i2cp      pointer to the I2C interface
+ * @param[in] sad       slave address without R bit
+ * @param[in] sub       sub-register address
+ * @param[in] message   pointer to message
+ * @return              register value.
+ */
+uint8_t lsm303dlhcReadRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                               msg_t* message) {
+
+  uint8_t txbuf, rxbuf[2];
+#if defined(STM32F103_MCUCONF)
+  txbuf = LSM303DLHC_SUB_MSB | sub;
+  if(message != NULL){
+    *message = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,
+                                        TIME_INFINITE);
+  }
+  else{
+    i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2, TIME_INFINITE);
+  }
+  return rxbuf[0];
+#else
+  txbuf = sub;
+  if(message != NULL){
+    *message = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 1,
+                                        TIME_INFINITE);
+  }
+  else{
+    i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 1, TIME_INFINITE);
+  }
+  return rxbuf[0];
+#endif
+
+
+}
+
+/**
+ * @brief   Writes a value into a register.
+ * @pre     The I2C interface must be initialized and the driver started.
+ *
+ * @param[in] i2cp       pointer to the I2C interface
+ * @param[in] sad        slave address without R bit
+ * @param[in] sub        sub-register address
+ * @param[in] value      the value to be written
+ * @param[out] message   pointer to message
+ */
+void lsm303dlhcWriteRegister(I2CDriver *i2cp,uint8_t sad, uint8_t sub,
+                             uint8_t value, msg_t* message) {
+
+  uint8_t txbuf[2];
+  uint8_t rxbuf;
+  if(sad == LSM303DLHC_SAD_ACCEL){
+    switch (sub) {
+    default:
+      /* Reserved register must not be written, according to the datasheet
+       * this could permanently damage the device.
+       */
+      chDbgAssert(FALSE, "lsm303dlhcWriteRegister(), reserved register");
+    case LSM303DLHC_SUB_ACC_STATUS_REG:
+    case LSM303DLHC_SUB_ACC_OUT_X_L:
+    case LSM303DLHC_SUB_ACC_OUT_X_H:
+    case LSM303DLHC_SUB_ACC_OUT_Y_L:
+    case LSM303DLHC_SUB_ACC_OUT_Y_H:
+    case LSM303DLHC_SUB_ACC_OUT_Z_L:
+    case LSM303DLHC_SUB_ACC_OUT_Z_H:
+    case LSM303DLHC_SUB_ACC_FIFO_SRC_REG:
+    case LSM303DLHC_SUB_ACC_INT1_SOURCE:
+    case LSM303DLHC_SUB_ACC_INT2_SOURCE:
+    case LSM303DLHC_SUB_ACC_CLICK_SRC:
+    /* Read only registers cannot be written, the command is ignored.*/
+      return;
+    case LSM303DLHC_SUB_ACC_CTRL_REG1:
+    case LSM303DLHC_SUB_ACC_CTRL_REG2:
+    case LSM303DLHC_SUB_ACC_CTRL_REG3:
+    case LSM303DLHC_SUB_ACC_CTRL_REG4:
+    case LSM303DLHC_SUB_ACC_CTRL_REG5:
+    case LSM303DLHC_SUB_ACC_CTRL_REG6:
+    case LSM303DLHC_SUB_ACC_REFERENCE:
+    case LSM303DLHC_SUB_ACC_FIFO_CTRL_REG:
+    case LSM303DLHC_SUB_ACC_INT1_CFG:
+    case LSM303DLHC_SUB_ACC_INT1_THS:
+    case LSM303DLHC_SUB_ACC_INT1_DURATION:
+    case LSM303DLHC_SUB_ACC_INT2_CFG:
+    case LSM303DLHC_SUB_ACC_INT2_THS:
+    case LSM303DLHC_SUB_ACC_INT2_DURATION:
+    case LSM303DLHC_SUB_ACC_CLICK_CFG:
+    case LSM303DLHC_SUB_ACC_CLICK_THS:
+    case LSM303DLHC_SUB_ACC_TIME_LIMIT:
+    case LSM303DLHC_SUB_ACC_TIME_LATENCY:
+    case LSM303DLHC_SUB_ACC_TIME_WINDOW:
+      txbuf[0] = sub;
+      txbuf[1] = value;
+      if(message != NULL){
+        *message = i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0,
+                                            TIME_INFINITE);
+      }
+      else{
+        i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
+      }
+      break;
+    }
+  }
+  else if(sad == LSM303DLHC_SAD_COMPASS){
+    switch (sub) {
+    default:
+      /* Reserved register must not be written, according to the datasheet
+       * this could permanently damage the device.
+       */
+      chDbgAssert(FALSE, "lsm303dlhcWriteRegister(), reserved register");
+    case LSM303DLHC_SUB_COMP_OUT_X_H:
+    case LSM303DLHC_SUB_COMP_OUT_X_L:
+    case LSM303DLHC_SUB_COMP_OUT_Z_H:
+    case LSM303DLHC_SUB_COMP_OUT_Z_L:
+    case LSM303DLHC_SUB_COMP_OUT_Y_H:
+    case LSM303DLHC_SUB_COMP_OUT_Y_L:
+    case LSM303DLHC_SUB_COMP_SR_REG:
+    case LSM303DLHC_SUB_COMP_IRA_REG:
+    case LSM303DLHC_SUB_COMP_IRB_REG:
+    case LSM303DLHC_SUB_COMP_IRC_REG:
+    case LSM303DLHC_SUB_COMP_TEMP_OUT_H:
+    case LSM303DLHC_SUB_COMP_TEMP_OUT_L:
+      /* Read only registers cannot be written, the command is ignored.*/
+      return;
+    case LSM303DLHC_SUB_COMP_CRA_REG:
+    case LSM303DLHC_SUB_COMP_CRB_REG:
+    case LSM303DLHC_SUB_COMP_MR_REG:
+      txbuf[0] = sub;
+      txbuf[1] = value;
+      if(message != NULL){
+        *message = i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0,
+                                            TIME_INFINITE);
+      }
+      else{
+        i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
+      }
+      break;
+    }
+  }
+}
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/mems/lsm303dlhc.h b/lib/chibios-contrib/os/various/devices_lib/mems/lsm303dlhc.h
new file mode 100644
index 000000000..46b51bce8
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/mems/lsm303dlhc.h
@@ -0,0 +1,352 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    lsm303dlhc.h
+ * @brief   LSM303DLHC MEMS interface module through I2C header.
+ *
+ * @addtogroup lsm303dlhc
+ * @{
+ */
+
+#ifndef _LSM303DLHC_H_
+#define _LSM303DLHC_H_
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+#define  LSM303DLHC_ACC_SENS_2G                  ((float)1671.836f)          /*!< Accelerometer sensitivity with 2 G full scale [LSB * s^2 / m] */
+#define  LSM303DLHC_ACC_SENS_4G                  ((float)835.918f)           /*!< Accelerometer sensitivity with 4 G full scale [LSB * s^2 / m] */
+#define  LSM303DLHC_ACC_SENS_8G                  ((float)417.959f)           /*!< Accelerometer sensitivity with 8 G full scale [LSB * s^2 / m] */
+#define  LSM303DLHC_ACC_SENS_16G                 ((float)208.979f)           /*!< Accelerometer sensitivity with 16 G full scale [LSB * s^2 / m] */
+
+#define  LSM303DLHC_COMP_SENS_XY_1_3GA           ((float)1100.0f)            /*!< Compass sensitivity with 1.3 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_XY_1_9GA           ((float)855.0f)             /*!< Compass sensitivity with 1.9 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_XY_2_5GA           ((float)670.0f)             /*!< Compass sensitivity with 2.5 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_XY_4_0GA           ((float)450.0f)             /*!< Compass sensitivity with 4.0 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_XY_4_7GA           ((float)400.0f)             /*!< Compass sensitivity with 4.7 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_XY_5_6GA           ((float)330.0f)             /*!< Compass sensitivity with 5.6 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_XY_8_1GA           ((float)230.0f)             /*!< Compass sensitivity with 8.1 GA full scale [LSB  / Ga] */
+
+#define  LSM303DLHC_COMP_SENS_Z_1_3GA            ((float)980.0f)             /*!< Compass sensitivity with 1.3 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_Z_1_9GA            ((float)765.0f)             /*!< Compass sensitivity with 1.9 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_Z_2_5GA            ((float)600.0f)             /*!< Compass sensitivity with 2.5 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_Z_4_0GA            ((float)400.0f)             /*!< Compass sensitivity with 4.0 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_Z_4_7GA            ((float)355.0f)             /*!< Compass sensitivity with 4.7 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_Z_5_6GA            ((float)295.0f)             /*!< Compass sensitivity with 5.6 GA full scale [LSB  / Ga] */
+#define  LSM303DLHC_COMP_SENS_Z_8_1GA            ((float)205.0f)             /*!< Compass sensitivity with 8.1 GA full scale [LSB  / Ga] */
+/**
+ * @name    LSM303DLHC register names
+ * @{
+ */
+/******************************************************************************/
+/*                                                                            */
+/*                       LSM303DLHC on board MEMS                             */
+/*                                                                            */
+/******************************************************************************/
+/*******************  Bit definition for I2C communication  *******************/
+#define  LSM303DLHC_SAD                          ((uint8_t)0x7F)            /*!< SAD[6:0] Slave Address Mask */
+#define  LSM303DLHC_SAD_ACCEL                    ((uint8_t)0x19)            /*!< ACCELEROMETER Slave Address */
+#define  LSM303DLHC_SAD_COMPASS                  ((uint8_t)0x1E)            /*!< MAGNETOMETER  Slave Address */
+
+#define  LSM303DLHC_SUB                          ((uint8_t)0x7F)            /*!< SUB[6:0] Sub-registers address Mask */
+#define  LSM303DLHC_SUB_0                        ((uint8_t)0x01)            /*!< bit 0 */
+#define  LSM303DLHC_SUB_1                        ((uint8_t)0x02)            /*!< bit 1 */
+#define  LSM303DLHC_SUB_2                        ((uint8_t)0x08)            /*!< bit 3 */
+#define  LSM303DLHC_SUB_4                        ((uint8_t)0x10)            /*!< bit 4 */
+#define  LSM303DLHC_SUB_5                        ((uint8_t)0x20)            /*!< bit 5 */
+#define  LSM303DLHC_SUB_6                        ((uint8_t)0x40)            /*!< bit 6 */
+
+#define  LSM303DLHC_SUB_MSB                      ((uint8_t)0x80)            /*!< Multiple data read\write bit */
+
+/********  Bit definition for Accelerometer SUB-Registers Addresses  **********/
+#define  LSM303DLHC_SUB_ACC_CTRL_REG1            ((uint8_t)0x20)            /*!< CONTROL REGISTER 1 FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_CTRL_REG2            ((uint8_t)0x21)            /*!< CONTROL REGISTER 2 FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_CTRL_REG3            ((uint8_t)0x22)            /*!< CONTROL REGISTER 3 FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_CTRL_REG4            ((uint8_t)0x23)            /*!< CONTROL REGISTER 4 FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_CTRL_REG5            ((uint8_t)0x24)            /*!< CONTROL REGISTER 5 FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_CTRL_REG6            ((uint8_t)0x25)            /*!< CONTROL REGISTER 6 FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_REFERENCE            ((uint8_t)0x26)            /*!< REFERENCE/DATACAPTURE FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_STATUS_REG           ((uint8_t)0x27)            /*!< STATUS REGISTER FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_OUT_X_L              ((uint8_t)0x28)            /*!< OUTPUT X-AXIS LOW FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_OUT_X_H              ((uint8_t)0x29)            /*!< OUTPUT X-AXIS HIGH FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_OUT_Y_L              ((uint8_t)0x2A)            /*!< OUTPUT Y-AXIS LOW FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_OUT_Y_H              ((uint8_t)0x2B)            /*!< OUTPUT Y-AXIS HIGH FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_OUT_Z_L              ((uint8_t)0x2C)            /*!< OUTPUT Z-AXIS LOW FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_OUT_Z_H              ((uint8_t)0x2D)            /*!< OUTPUT Z-AXIS HIGH FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_FIFO_CTRL_REG        ((uint8_t)0x2E)            /*!< FIFO CONTROL REGISTER FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_FIFO_SRC_REG         ((uint8_t)0x2F)            /*!< FIFO SOURCE REGISTER FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_INT1_CFG             ((uint8_t)0x30)            /*!< INTERRUPT1 CONFIG FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_INT1_SOURCE          ((uint8_t)0x31)            /*!< INTERRUPT1 SOURCE FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_INT1_THS             ((uint8_t)0x32)            /*!< INTERRUPT1 THRESHOLD FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_INT1_DURATION        ((uint8_t)0x33)            /*!< INTERRUPT1 DURATION FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_INT2_CFG             ((uint8_t)0x34)            /*!< INTERRUPT2 CONFIG FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_INT2_SOURCE          ((uint8_t)0x35)            /*!< INTERRUPT2 SOURCE FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_INT2_THS             ((uint8_t)0x36)            /*!< INTERRUPT2 THRESHOLD FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_INT2_DURATION        ((uint8_t)0x37)            /*!< INTERRUPT2 DURATION FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_CLICK_CFG            ((uint8_t)0x38)            /*!< CLICK CONFIG FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_CLICK_SRC            ((uint8_t)0x39)            /*!< CLICK SOURCE FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_CLICK_THS            ((uint8_t)0x3A)            /*!< CLICK THRESHOLD FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_TIME_LIMIT           ((uint8_t)0x3B)            /*!< TIME LIMIT FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_TIME_LATENCY         ((uint8_t)0x3C)            /*!< TIME LATENCY FOR ACCELEROMETER */
+#define  LSM303DLHC_SUB_ACC_TIME_WINDOW          ((uint8_t)0x3D)            /*!< TIME WINDOW FOR ACCELEROMETER */
+
+/*********  Bit definition for Compass SUB-Registers Addresses  **********/
+#define  LSM303DLHC_SUB_COMP_CRA_REG             ((uint8_t)0x00)            /*!< CONTROL REGISTER A FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_CRB_REG             ((uint8_t)0x01)            /*!< CONTROL REGISTER B FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_MR_REG              ((uint8_t)0x02)            /*!< STATUS REGISTER FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_OUT_X_H             ((uint8_t)0x03)            /*!< OUTPUT X-AXIS HIGH FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_OUT_X_L             ((uint8_t)0x04)            /*!< OUTPUT X-AXIS LOW FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_OUT_Z_H             ((uint8_t)0x05)            /*!< OUTPUT Z-AXIS HIGH FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_OUT_Z_L             ((uint8_t)0x06)            /*!< OUTPUT Z-AXIS LOW FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_OUT_Y_H             ((uint8_t)0x07)            /*!< OUTPUT Y-AXIS HIGH FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_OUT_Y_L             ((uint8_t)0x08)            /*!< OUTPUT Y-AXIS LOW FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_SR_REG              ((uint8_t)0x09)            /*!< SR REGISTER FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_IRA_REG             ((uint8_t)0x0A)            /*!< IR A REGISTER FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_IRB_REG             ((uint8_t)0x0B)            /*!< IR B REGISTER FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_IRC_REG             ((uint8_t)0x0C)            /*!< IR C REGISTER FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_TEMP_OUT_H          ((uint8_t)0x31)            /*!< OUTPUT TEMP HIGH FOR MAGNETOMETER */
+#define  LSM303DLHC_SUB_COMP_TEMP_OUT_L          ((uint8_t)0x32)            /*!< OUTPUT TEMP LOW FOR MAGNETOMETER */
+
+/** @}  */
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Accelerometer data structures and types
+ * @{
+ */
+
+/**
+ * @brief  Accelerometer Output Data Rate
+ */
+typedef enum
+{
+  LSM303DLHC_ACC_ODR_PD        = 0x00,         /*!< Power down */
+  LSM303DLHC_ACC_ODR_1Hz       = 0x10,         /*!< Output Data Rate = 1 Hz */
+  LSM303DLHC_ACC_ODR_10Hz      = 0x20,         /*!< Output Data Rate = 10 Hz */
+  LSM303DLHC_ACC_ODR_25Hz      = 0x30,         /*!< Output Data Rate = 25 Hz */
+  LSM303DLHC_ACC_ODR_50Hz      = 0x40,         /*!< Output Data Rate = 50 Hz */
+  LSM303DLHC_ACC_ODR_100Hz     = 0x50,         /*!< Output Data Rate = 100 Hz */
+  LSM303DLHC_ACC_ODR_200Hz     = 0x60,         /*!< Output Data Rate = 200 Hz */
+  LSM303DLHC_ACC_ODR_400Hz     = 0x70,         /*!< Output Data Rate = 400 Hz */
+  LSM303DLHC_ACC_ODR_1620Hz    = 0x80,         /*!< Output Data Rate = 1620 Hz Low Power mode only */
+  LSM303DLHC_ACC_ODR_1344Hz    = 0x90          /*!< Output Data Rate = 1344 Hz in Normal mode and 5376 Hz in Low Power Mode */
+}LSM303DLHC_ACC_ODR_t;
+
+/**
+ * @brief  Accelerometer Power Mode
+ */
+typedef enum
+{
+  LSM303DLHC_ACC_PM_NORMAL    = 0x00,         /*!< Normal mode enabled */
+  LSM303DLHC_ACC_PM_LOW_POWER = 0x08          /*!< Low Power mode enabled */
+}LSM303DLHC_ACC_PM_t;
+
+/**
+ * @brief  Accelerometer Full Scale
+ */
+typedef enum
+{
+  LSM303DLHC_ACC_FS_2G   = 0x00,               /*!< �2 g m/s^2 */
+  LSM303DLHC_ACC_FS_4G   = 0x10,               /*!< �4 g m/s^2 */
+  LSM303DLHC_ACC_FS_8G   = 0x20,               /*!< �8 g m/s^2 */
+  LSM303DLHC_ACC_FS_16G  = 0x30                /*!< �16 g m/s^2 */
+}LSM303DLHC_ACC_FS_t;
+
+/**
+ * @brief   Accelerometer Axes Enabling
+ */
+typedef enum{
+  LSM303DLHC_ACC_AE_DISABLED = 0x00,           /*!< Axes all disabled */
+  LSM303DLHC_ACC_AE_X        = 0x01,           /*!< Only X-axis enabled */
+  LSM303DLHC_ACC_AE_Y        = 0x02,           /*!< Only Y-axis enabled */
+  LSM303DLHC_ACC_AE_XY       = 0x03,           /*!< X & Y axes enabled */
+  LSM303DLHC_ACC_AE_Z        = 0x04,           /*!< Only Z-axis enabled */
+  LSM303DLHC_ACC_AE_XZ       = 0x05,           /*!< X & Z axes enabled  */
+  LSM303DLHC_ACC_AE_YZ       = 0x06,           /*!< Y & Z axes enabled  */
+  LSM303DLHC_ACC_AE_XYZ      = 0x07            /*!< All axes enabled */
+}LSM303DLHC_ACC_AE_t;
+
+/**
+ * @brief  Accelerometer Block Data Update
+ */
+typedef enum
+{
+  LSM303DLHC_ACC_BDU_CONTINOUS = 0x00,          /*!< Continuos Update */
+  LSM303DLHC_ACC_BDU_BLOCKED   = 0x80           /*!< Single Update: output registers not updated until MSB and LSB reading */
+}LSM303DLHC_ACC_BDU_t;
+
+/**
+ * @brief  Accelerometer Endianness
+ */
+typedef enum
+{
+  LSM303DLHC_ACC_End_LITTLE = 0x00,            /*!< Little Endian: data LSB @ lower address */
+  LSM303DLHC_ACC_End_BIG    = 0x40             /*!< Big Endian: data MSB @ lower address */
+}LSM303DLHC_ACC_End_t;
+
+/**
+ * @brief  Accelerometer High Resolution mode
+ */
+typedef enum
+{
+  LSM303DLHC_ACC_HR_Enabled  = 0x08,            /*!< High resolution output mode enabled */
+  LSM303DLHC_ACC_HR_Disabled = 0x00             /*!< High resolution output mode disabled */
+}LSM303DLHC_ACC_HR_t;
+
+/**
+ * @brief   Accelerometer configuration structure.
+ */
+typedef struct {
+  /**
+   * @brief Accelerometer fullscale value.
+   */
+  LSM303DLHC_ACC_FS_t    fullscale;
+  /**
+   * @brief Accelerometer power mode selection.
+   */
+  LSM303DLHC_ACC_PM_t    powermode;
+  /**
+   * @brief Accelerometer output data rate selection.
+   */
+  LSM303DLHC_ACC_ODR_t   outputdatarate;
+  /**
+   * @brief Accelerometer axes enabling.
+   */
+  LSM303DLHC_ACC_AE_t    axesenabling;
+  /**
+   * @brief Accelerometer block data update.
+   */
+  LSM303DLHC_ACC_BDU_t   blockdataupdate;
+  /**
+   * @brief Accelerometer block data update.
+   */
+  LSM303DLHC_ACC_HR_t    highresmode;
+} LSM303DLHC_ACC_Config;
+/** @}  */
+
+
+/**
+ * @name    Compass data types
+ * @{
+ */
+
+/**
+ * @brief  Compass Output Data Rate
+ */
+typedef enum
+{
+  LSM303DLHC_COMP_ODR_0_75_Hz = 0x00,                  /*!< Output Data Rate = 0.75 Hz */
+  LSM303DLHC_COMP_ODR_1_5_Hz  = 0x04,                  /*!< Output Data Rate = 1.5 Hz */
+  LSM303DLHC_COMP_ODR_3_0_Hz  = 0x08,                  /*!< Output Data Rate = 3 Hz */
+  LSM303DLHC_COMP_ODR_7_5_Hz  = 0x0C,                  /*!< Output Data Rate = 7.5 Hz */
+  LSM303DLHC_COMP_ODR_15_Hz   = 0x10,                  /*!< Output Data Rate = 15 Hz */
+  LSM303DLHC_COMP_ODR_30_Hz   = 0x14,                  /*!< Output Data Rate = 30 Hz */
+  LSM303DLHC_COMP_ODR_75_Hz   = 0x18,                  /*!< Output Data Rate = 75 Hz */
+  LSM303DLHC_COMP_ODR_220_Hz  = 0x1C                   /*!< Output Data Rate = 220 Hz */
+}LSM303DLHC_COMP_ODR_t;
+
+
+/**
+ * @brief  Compass Full Scale
+ */
+typedef enum
+{
+  LSM303DLHC_COMP_FS_1_3_GA  = 0x20,                   /*!< Full scale = �1.3 Gauss */
+  LSM303DLHC_COMP_FS_1_9_GA  = 0x40,                   /*!< Full scale = �1.9 Gauss */
+  LSM303DLHC_COMP_FS_2_5_GA  = 0x60,                   /*!< Full scale = �2.5 Gauss */
+  LSM303DLHC_COMP_FS_4_0_GA  = 0x80,                   /*!< Full scale = �4.0 Gauss */
+  LSM303DLHC_COMP_FS_4_7_GA  = 0xA0,                   /*!< Full scale = �4.7 Gauss */
+  LSM303DLHC_COMP_FS_5_6_GA  = 0xC0,                   /*!< Full scale = �5.6 Gauss */
+  LSM303DLHC_COMP_FS_8_1_GA  = 0xE0                    /*!< Full scale = �8.1 Gauss */
+}LSM303DLHC_COMP_FS_t;
+
+
+/**
+ * @brief  Compass Working Mode
+ */
+typedef enum
+{
+  LSM303DLHC_COMP_WM_CONTINUOS = 0x00,                 /*!< Continuous-Conversion Mode */
+  LSM303DLHC_COMP_WM_BLOCKED   = 0x01,                 /*!< Single-Conversion Mode */
+  LSM303DLHC_COMP_WM_SLEEP     = 0x02                  /*!< Sleep Mode */
+}LSM303DLHC_COMP_WM_t;
+
+/**
+ * @brief   Compass configuration structure.
+ */
+typedef struct {
+  /**
+   * @brief Compass fullscale value.
+   */
+  LSM303DLHC_COMP_FS_t    fullscale;
+  /**
+   * @brief Compass output data rate selection.
+   */
+  LSM303DLHC_COMP_ODR_t   outputdatarate;
+  /**
+   * @brief Compass working mode.
+   */
+  LSM303DLHC_COMP_WM_t    workingmode;
+} LSM303DLHC_COMP_Config;
+/** @}  */
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+  uint8_t lsm303dlhcReadRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                                 msg_t* message);
+  void lsm303dlhcWriteRegister(I2CDriver *i2cp,uint8_t sad, uint8_t sub,
+                                 uint8_t value, msg_t* message);
+
+#ifdef __cplusplus
+}
+#endif
+#endif /* _LSM303DLHC_H_ */
+/** @} */
+
diff --git a/lib/chibios-contrib/os/various/devices_lib/mems/lsm6ds0.c b/lib/chibios-contrib/os/various/devices_lib/mems/lsm6ds0.c
new file mode 100644
index 000000000..da67f126f
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/mems/lsm6ds0.c
@@ -0,0 +1,184 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    lsm6ds0.c
+ * @brief   LSM6DS0 MEMS interface module through I2C code.
+ *
+ * @addtogroup lsm6ds0
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#include "lsm6ds0.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Reads a generic sub-register value.
+ * @pre     The I2C interface must be initialized and the driver started.
+ *
+ * @param[in] i2cp      pointer to the I2C interface
+ * @param[in] sad       slave address without R bit
+ * @param[in] sub       sub-register address
+ * @param[in] message   pointer to message
+ * @return              register value.
+ */
+uint8_t lsm6ds0ReadRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                                 msg_t* message) {
+
+  uint8_t txbuf, rxbuf[2];
+#if defined(STM32F103_MCUCONF)
+  txbuf = LSM303DLHC_SUB_MSB | sub;
+  if(message != NULL){
+    *message = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2,
+                                        TIME_INFINITE);
+  }
+  else{
+    i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 2, TIME_INFINITE);
+  }
+  return rxbuf[0];
+#else
+  txbuf = sub;
+  if(message != NULL){
+    *message = i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 1,
+                                        TIME_INFINITE);
+  }
+  else{
+    i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, 1, TIME_INFINITE);
+  }
+  return rxbuf[0];
+#endif
+}
+
+/**
+ * @brief   Writes a value into a register.
+ * @pre     The I2C interface must be initialized and the driver started.
+ *
+ * @param[in] i2cp       pointer to the I2C interface
+ * @param[in] sad        slave address without R bit
+ * @param[in] sub        sub-register address
+ * @param[in] value      the value to be written
+ * @param[out] message   pointer to message
+ */
+void lsm6ds0WriteRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                               uint8_t value, msg_t* message) {
+
+  uint8_t txbuf[2];
+  uint8_t rxbuf;
+  switch (sub) {
+  default:
+    /* Reserved register must not be written, according to the datasheet
+     * this could permanently damage the device.
+     */
+    chDbgAssert(FALSE, "lsm6ds0WriteRegister(), reserved register");
+  case LSM6DS0_SUB_WHO_AM_I:
+  case LSM6DS0_SUB_INT_GEN_SRC_G:
+  case LSM6DS0_SUB_OUT_TEMP_L:
+  case LSM6DS0_SUB_OUT_TEMP_H:
+  case LSM6DS0_SUB_STATUS_REG1:
+  case LSM6DS0_SUB_OUT_X_L_G:
+  case LSM6DS0_SUB_OUT_X_H_G:
+  case LSM6DS0_SUB_OUT_Y_L_G:
+  case LSM6DS0_SUB_OUT_Y_H_G:
+  case LSM6DS0_SUB_OUT_Z_L_G:
+  case LSM6DS0_SUB_OUT_Z_H_G:
+  case LSM6DS0_SUB_INT_GEN_SRC_XL:
+  case LSM6DS0_SUB_STATUS_REG2:
+  case LSM6DS0_SUB_OUT_X_L_XL:
+  case LSM6DS0_SUB_OUT_X_H_XL:
+  case LSM6DS0_SUB_OUT_Y_L_XL:
+  case LSM6DS0_SUB_OUT_Y_H_XL:
+  case LSM6DS0_SUB_OUT_Z_L_XL:
+  case LSM6DS0_SUB_OUT_Z_H_XL:
+  case LSM6DS0_SUB_FIFO_SRC:
+  /* Read only registers cannot be written, the command is ignored.*/
+    return;
+  case LSM6DS0_SUB_ACT_THS:
+  case LSM6DS0_SUB_ACT_DUR:
+  case LSM6DS0_SUB_INT_GEN_CFG_XL:
+  case LSM6DS0_SUB_INT_GEN_THS_X_XL:
+  case LSM6DS0_SUB_INT_GEN_THS_Y_XL:
+  case LSM6DS0_SUB_INT_GEN_THS_Z_XL:
+  case LSM6DS0_SUB_INT_GEN_DUR_XL:
+  case LSM6DS0_SUB_REFERENCE_G:
+  case LSM6DS0_SUB_INT_CTRL:
+  case LSM6DS0_SUB_CTRL_REG1_G:
+  case LSM6DS0_SUB_CTRL_REG2_G:
+  case LSM6DS0_SUB_CTRL_REG3_G:
+  case LSM6DS0_SUB_ORIENT_CFG_G:
+  case LSM6DS0_SUB_CTRL_REG4:
+  case LSM6DS0_SUB_CTRL_REG5_XL:
+  case LSM6DS0_SUB_CTRL_REG6_XL:
+  case LSM6DS0_SUB_CTRL_REG7_XL:
+  case LSM6DS0_SUB_CTRL_REG8:
+  case LSM6DS0_SUB_CTRL_REG9:
+  case LSM6DS0_SUB_CTRL_REG10:
+  case LSM6DS0_SUB_FIFO_CTRL:
+  case LSM6DS0_SUB_INT_GEN_CFG_G:
+  case LSM6DS0_SUB_INT_GEN_THS_XH_G:
+  case LSM6DS0_SUB_INT_GEN_THS_XL_G:
+  case LSM6DS0_SUB_INT_GEN_THS_YH_G:
+  case LSM6DS0_SUB_INT_GEN_THS_YL_G:
+  case LSM6DS0_SUB_INT_GEN_THS_ZH_G:
+  case LSM6DS0_SUB_INT_GEN_THS_ZL_G:
+  case LSM6DS0_SUB_INT_GEN_DUR_G:
+    txbuf[0] = sub;
+    txbuf[1] = value;
+    if(message != NULL){
+      *message = i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0,
+                                          TIME_INFINITE);
+    }
+    else{
+      i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
+    }
+    break;
+  }
+}
+
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/mems/lsm6ds0.h b/lib/chibios-contrib/os/various/devices_lib/mems/lsm6ds0.h
new file mode 100644
index 000000000..57e205779
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/mems/lsm6ds0.h
@@ -0,0 +1,482 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    lsm6ds0.h
+ * @brief   LSM6DS0 MEMS interface module header.
+ *
+ * @{
+ */
+
+#ifndef _LSM6DS0_H_
+#define _LSM6DS0_H_
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+#define  LSM6DS0_ACC_SENS_2G                     ((float)1671.095f)         /*!< Accelerometer sensitivity with 2 G full scale [LSB * s^2 / m] */
+#define  LSM6DS0_ACC_SENS_4G                     ((float)835.547f)          /*!< Accelerometer sensitivity with 4 G full scale [LSB * s^2 / m] */
+#define  LSM6DS0_ACC_SENS_8G                     ((float)417.774)           /*!< Accelerometer sensitivity with 8 G full scale [LSB * s^2 / m] */
+#define  LSM6DS0_ACC_SENS_16G                    ((float)139.258f)          /*!< Accelerometer sensitivity with 16 G full scale [LSB * s^2 / m] */
+
+#define  LSM6DS0_GYRO_SENS_245DPS                ((float)114.286f)          /*!< Gyroscope sensitivity with 245 dps full scale [LSB * s / �] */
+#define  LSM6DS0_GYRO_SENS_500DPS                ((float)57.143f)           /*!< Gyroscope sensitivity with 500 dps full scale [LSB * s / �] */
+#define  LSM6DS0_GYRO_SENS_2000DPS               ((float)14.286f)           /*!< Gyroscope sensitivity with 2000 dps full scale [LSB * s / �] */
+/**
+ * @name    LSM6DS0 register names
+ * @{
+ */
+/******************************************************************************/
+/*                                                                            */
+/*                        LSM6DS0 on board MEMS                               */
+/*                                                                            */
+/******************************************************************************/
+/*****************  Bit definition for I2C/SPI communication  *****************/
+#define  LSM6DS0_SUB                             ((uint8_t)0x7F)            /*!< SUB[6:0] Sub-registers address Mask */
+#define  LSM6DS0_SUB_0                           ((uint8_t)0x01)            /*!< bit 0 */
+#define  LSM6DS0_SUB_1                           ((uint8_t)0x02)            /*!< bit 1 */
+#define  LSM6DS0_SUB_2                           ((uint8_t)0x08)            /*!< bit 3 */
+#define  LSM6DS0_SUB_4                           ((uint8_t)0x10)            /*!< bit 4 */
+#define  LSM6DS0_SUB_5                           ((uint8_t)0x20)            /*!< bit 5 */
+#define  LSM6DS0_SUB_6                           ((uint8_t)0x40)            /*!< bit 6 */
+
+#define  LSM6DS0_SUB_MSB                         ((uint8_t)0x80)            /*!< Multiple data read\write bit */
+
+/*****************  Bit definition for Registers Addresses  *******************/
+#define  LSM6DS0_SUB_ACT_THS                     ((uint8_t)0x04)            /*!< Activity threshold register */
+#define  LSM6DS0_SUB_ACT_DUR                     ((uint8_t)0x05)            /*!< Inactivity duration register */
+#define  LSM6DS0_SUB_INT_GEN_CFG_XL              ((uint8_t)0x06)            /*!< Accelerometer interrupt generator configuration register */
+#define  LSM6DS0_SUB_INT_GEN_THS_X_XL            ((uint8_t)0x07)            /*!< Accelerometer X-axis interrupt threshold register */
+#define  LSM6DS0_SUB_INT_GEN_THS_Y_XL            ((uint8_t)0x08)            /*!< Accelerometer Y-axis interrupt threshold register */
+#define  LSM6DS0_SUB_INT_GEN_THS_Z_XL            ((uint8_t)0x09)            /*!< Accelerometer Z-axis interrupt threshold register */
+#define  LSM6DS0_SUB_INT_GEN_DUR_XL              ((uint8_t)0x0A)            /*!< Accelerometer interrupt duration register */
+#define  LSM6DS0_SUB_REFERENCE_G                 ((uint8_t)0x0B)            /*!< Gyroscope reference value register for digital high-pass filter */
+#define  LSM6DS0_SUB_INT_CTRL                    ((uint8_t)0x0C)            /*!< INT pin control register */
+#define  LSM6DS0_SUB_WHO_AM_I                    ((uint8_t)0x0F)            /*!< Who_AM_I register */
+#define  LSM6DS0_SUB_CTRL_REG1_G                 ((uint8_t)0x10)            /*!< Gyroscope control register 1 */
+#define  LSM6DS0_SUB_CTRL_REG2_G                 ((uint8_t)0x11)            /*!< Gyroscope control register 2 */
+#define  LSM6DS0_SUB_CTRL_REG3_G                 ((uint8_t)0x12)            /*!< Gyroscope control register 3 */
+#define  LSM6DS0_SUB_ORIENT_CFG_G                ((uint8_t)0x13)            /*!< Gyroscope sign and orientation register */
+#define  LSM6DS0_SUB_INT_GEN_SRC_G               ((uint8_t)0x14)            /*!< Gyroscope interrupt source register */
+#define  LSM6DS0_SUB_OUT_TEMP_L                  ((uint8_t)0x15)            /*!< Temperature data output low register */
+#define  LSM6DS0_SUB_OUT_TEMP_H                  ((uint8_t)0x16)            /*!< Temperature data output high register */
+#define  LSM6DS0_SUB_STATUS_REG1                 ((uint8_t)0x17)            /*!< Status register 1 */
+#define  LSM6DS0_SUB_OUT_X_L_G                   ((uint8_t)0x18)            /*!< Gyroscope X-axis low output register */
+#define  LSM6DS0_SUB_OUT_X_H_G                   ((uint8_t)0x19)            /*!< Gyroscope X-axis high output register */
+#define  LSM6DS0_SUB_OUT_Y_L_G                   ((uint8_t)0x1A)            /*!< Gyroscope Y-axis low output register */
+#define  LSM6DS0_SUB_OUT_Y_H_G                   ((uint8_t)0x1B)            /*!< Gyroscope Y-axis high output register */
+#define  LSM6DS0_SUB_OUT_Z_L_G                   ((uint8_t)0x1C)            /*!< Gyroscope Z-axis low output register */
+#define  LSM6DS0_SUB_OUT_Z_H_G                   ((uint8_t)0x1D)            /*!< Gyroscope Z-axis high output register */
+#define  LSM6DS0_SUB_CTRL_REG4                   ((uint8_t)0x1E)            /*!< Control register 4 */
+#define  LSM6DS0_SUB_CTRL_REG5_XL                ((uint8_t)0x1F)            /*!< Accelerometer Control Register 5 */
+#define  LSM6DS0_SUB_CTRL_REG6_XL                ((uint8_t)0x20)            /*!< Accelerometer Control Register 6 */
+#define  LSM6DS0_SUB_CTRL_REG7_XL                ((uint8_t)0x21)            /*!< Accelerometer Control Register 7 */
+#define  LSM6DS0_SUB_CTRL_REG8                   ((uint8_t)0x22)            /*!< Control register 8 */
+#define  LSM6DS0_SUB_CTRL_REG9                   ((uint8_t)0x23)            /*!< Control register 9 */
+#define  LSM6DS0_SUB_CTRL_REG10                  ((uint8_t)0x24)            /*!< Control register 10 */
+#define  LSM6DS0_SUB_INT_GEN_SRC_XL              ((uint8_t)0x26)            /*!< Accelerometer interrupt source register */
+#define  LSM6DS0_SUB_STATUS_REG2                 ((uint8_t)0x27)            /*!< Status register */
+#define  LSM6DS0_SUB_OUT_X_L_XL                  ((uint8_t)0x28)            /*!< Accelerometer X-axis low output register */
+#define  LSM6DS0_SUB_OUT_X_H_XL                  ((uint8_t)0x29)            /*!< Accelerometer X-axis high output register */
+#define  LSM6DS0_SUB_OUT_Y_L_XL                  ((uint8_t)0x2A)            /*!< Accelerometer Y-axis low output register */
+#define  LSM6DS0_SUB_OUT_Y_H_XL                  ((uint8_t)0x2B)            /*!< Accelerometer Y-axis high output register */
+#define  LSM6DS0_SUB_OUT_Z_L_XL                  ((uint8_t)0x2C)            /*!< Accelerometer Z-axis low output register */
+#define  LSM6DS0_SUB_OUT_Z_H_XL                  ((uint8_t)0x2D)            /*!< Accelerometer Z-axis high output register */
+#define  LSM6DS0_SUB_FIFO_CTRL                   ((uint8_t)0x2E)            /*!< FIFO control register */
+#define  LSM6DS0_SUB_FIFO_SRC                    ((uint8_t)0x2F)            /*!< FIFO status control register */
+#define  LSM6DS0_SUB_INT_GEN_CFG_G               ((uint8_t)0x30)            /*!< Gyroscope interrupt generator configuration register */
+#define  LSM6DS0_SUB_INT_GEN_THS_XH_G            ((uint8_t)0x31)            /*!< Gyroscope X-axis low interrupt generator threshold registers */
+#define  LSM6DS0_SUB_INT_GEN_THS_XL_G            ((uint8_t)0x32)            /*!< Gyroscope X-axis high interrupt generator threshold registers  */
+#define  LSM6DS0_SUB_INT_GEN_THS_YH_G            ((uint8_t)0x33)            /*!< Gyroscope Y-axis low interrupt generator threshold registers */
+#define  LSM6DS0_SUB_INT_GEN_THS_YL_G            ((uint8_t)0x34)            /*!< Gyroscope Y-axis high interrupt generator threshold registers */
+#define  LSM6DS0_SUB_INT_GEN_THS_ZH_G            ((uint8_t)0x35)            /*!< Gyroscope Z-axis low interrupt generator threshold registers */
+#define  LSM6DS0_SUB_INT_GEN_THS_ZL_G            ((uint8_t)0x36)            /*!< Gyroscope Z-axis high interrupt generator threshold registers */
+#define  LSM6DS0_SUB_INT_GEN_DUR_G               ((uint8_t)0x37)            /*!< Gyroscope interrupt generator duration register */
+
+/** @} */
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    Generic LSM6DS0 data structures and types
+ * @{
+ */
+
+/**
+ * @brief  Accelerometer and Gyroscope Slave Address
+ */
+typedef enum {
+  LSM6DS0_SAD_GND = 0x6A,                             /*!< LSM6DS0 Slave Address when SA1 is to GND */
+  LSM6DS0_SAD_VCC = 0x6B                              /*!< LSM6DS0 Slave Address when SA1 is to VCC */
+}LSM6DS0_SAD_t;
+
+/**
+ * @brief  Accelerometer and Gyroscope Block Data Update
+ */
+typedef enum
+{
+  LSM6DS0_BDU_CONTINOUS = 0x00,                       /*!< Continuos Update */
+  LSM6DS0_BDU_BLOCKED   = 0x40                        /*!< Single Update: output registers not updated until MSB and LSB reading */
+}LSM6DS0_BDU_t;
+
+/**
+ * @brief  Accelerometer and Gyroscope Endianness
+ */
+typedef enum
+{
+  LSM6DS0_END_LITTLE = 0x00,                          /*!< Little Endian: data LSB @ lower address */
+  LSM6DS0_END_BIG    = 0x20                           /*!< Big Endian: data MSB @ lower address */
+}LSM6DS0_END_t;
+/** @}  */
+
+/**
+ * @name    Accelerometer data structures and types
+ * @{
+ */
+
+/**
+ * @brief  Accelerometer Decimation Mode
+ */
+typedef enum {
+  LSM6DS0_ACC_DEC_DISABLED = 0x00,                    /*!< NO decimation */
+  LSM6DS0_ACC_DEC_X2       = 0x40,                    /*!< Decimation update every 2 sample */
+  LSM6DS0_ACC_DEC_X4       = 0x80,                    /*!< Decimation update every 4 sample */
+  LSM6DS0_ACC_DEC_X8       = 0xC0                     /*!< Decimation update every 8 sample */
+}LSM6DS0_ACC_DEC_t;
+
+/**
+ * @brief   Accelerometer Axes Enabling
+ */
+typedef enum{
+  LSM6DS0_ACC_AE_DISABLED = 0x00,                     /*!< Axes all disabled */
+  LSM6DS0_ACC_AE_X        = 0x08,                     /*!< Only X-axis enabled */
+  LSM6DS0_ACC_AE_Y        = 0x10,                     /*!< Only Y-axis enabled */
+  LSM6DS0_ACC_AE_XY       = 0x18,                     /*!< X & Y axes enabled */
+  LSM6DS0_ACC_AE_Z        = 0x20,                     /*!< Only Z-axis enabled */
+  LSM6DS0_ACC_AE_XZ       = 0x28,                     /*!< X & Z axes enabled  */
+  LSM6DS0_ACC_AE_YZ       = 0x30,                     /*!< Y & Z axes enabled  */
+  LSM6DS0_ACC_AE_XYZ      = 0x38                      /*!< All axes enabled */
+}LSM6DS0_ACC_AE_t;
+
+/**
+ * @brief  Accelerometer Output Data Rate
+ */
+typedef enum {
+  LSM6DS0_ACC_ODR_PD    = 0x00,                       /*!< Power down */
+  LSM6DS0_ACC_ODR_10Hz  = 0x20,                       /*!< Output Data Rate = 10 Hz */
+  LSM6DS0_ACC_ODR_50Hz  = 0x40,                       /*!< Output Data Rate = 50 Hz */
+  LSM6DS0_ACC_ODR_119Hz = 0x60,                       /*!< Output Data Rate = 119 Hz */
+  LSM6DS0_ACC_ODR_238Hz = 0x80,                       /*!< Output Data Rate = 238 Hz */
+  LSM6DS0_ACC_ODR_476Hz = 0xA0,                       /*!< Output Data Rate = 476 Hz */
+  LSM6DS0_ACC_ODR_952Hz = 0xC0                        /*!< Output Data Rate = 952 Hz */
+}LSM6DS0_ACC_ODR_t;
+
+/**
+ * @brief  Accelerometer Full Scale
+ */
+typedef enum {
+  LSM6DS0_ACC_FS_2G  = 0x00,                          /*!< �2 g m/s^2 */
+  LSM6DS0_ACC_FS_4G  = 0x10,                          /*!< �4 g m/s^2 */
+  LSM6DS0_ACC_FS_8G  = 0x18,                          /*!< �8 g m/s^2 */
+  LSM6DS0_ACC_FS_16G = 0x08                           /*!< �16 g m/s^2 */
+}LSM6DS0_ACC_FS_t;
+
+/**
+ * @brief  Accelerometer Antialiasing filter Bandwidth Selection
+ */
+typedef enum {
+  LSM6DS0_ACC_BW_408Hz    = 0x00,                     /*!< AA filter bandwidth = 408 Hz  */
+  LSM6DS0_ACC_BW_211Hz    = 0x01,                     /*!< AA filter bandwidth = 211 Hz */
+  LSM6DS0_ACC_BW_105Hz    = 0x02,                     /*!< AA filter bandwidth = 105 Hz */
+  LSM6DS0_ACC_BW_50Hz     = 0x03,                     /*!< AA filter bandwidth = 50 Hz */
+  LSM6DS0_ACC_BW_ACCORDED = 0x04,                     /*!< AA filter bandwidth chosen by ODR selection */
+}LSM6DS0_ACC_BW_t;
+
+/**
+ * @brief  Accelerometer High Resolution mode
+ */
+typedef enum
+{
+  LSM6DS0_ACC_HR_Disabled = 0x00,                     /*!< High resolution output mode disabled, FDS bypassed */
+  LSM6DS0_ACC_HR_EN_9     = 0xC4,                     /*!< High resolution output mode enabled, LP cutoff = ODR/9, FDS enabled */
+  LSM6DS0_ACC_HR_EN_50    = 0x84,                     /*!< High resolution output mode enabled, LP cutoff = ODR/50, FDS enabled */
+  LSM6DS0_ACC_HR_EN_100   = 0xA4,                     /*!< High resolution output mode enabled, LP cutoff = ODR/100, FDS enabled */
+  LSM6DS0_ACC_HR_EN_400   = 0xE4,                     /*!< High resolution output mode enabled, LP cutoff = ODR/400, FDS enabled */
+}LSM6DS0_ACC_HR_t;
+
+/**
+ * @brief  HP filter for interrupt
+ */
+typedef enum
+{
+  LSM6DS0_ACC_HPIS1_BYPASSED = 0x00,                  /*!< High-pass filter bypassed */
+  LSM6DS0_ACC_HPIS1_ENABLED  = 0x01                   /*!< High-pass filter enabled for accelerometer interrupt function on interrupt */
+}LSM6DS0_ACC_HPIS1_t;
+
+/**
+ * @brief   Accelerometer configuration structure.
+ */
+typedef struct {
+
+  /**
+   * @brief  LSM6DS0 Slave Address
+   */
+  LSM6DS0_SAD_t       slaveaddress;
+  /**
+   * @brief  Accelerometer Decimation Mode
+   */
+  LSM6DS0_ACC_DEC_t   decimation;
+  /**
+   * @brief  Accelerometer Output Data Rate
+   */
+  LSM6DS0_ACC_ODR_t   outputdatarate;
+  /**
+   * @brief  Accelerometer Antialiasing filter Bandwidth Selection
+   */
+  LSM6DS0_ACC_BW_t    bandwidth;
+  /**
+   * @brief  Accelerometer Full Scale
+   */
+  LSM6DS0_ACC_FS_t    fullscale;
+  /**
+   * @brief   Accelerometer Axes Enabling
+   */
+  LSM6DS0_ACC_AE_t    axesenabling;
+  /**
+   * @brief  Accelerometer High Resolution mode
+   */
+  LSM6DS0_ACC_HR_t    highresmode;
+  /**
+   * @brief  HP filter for interrupt
+   */
+  LSM6DS0_ACC_HPIS1_t hpfirq;
+  /**
+   * @brief  LSM6DS0 Endianness
+   */
+  LSM6DS0_END_t       endianess;
+  /**
+   * @brief  LSM6DS0 Block Data Update
+   */
+  LSM6DS0_BDU_t       blockdataupdate;
+} LSM6DS0_ACC_Config;
+/** @}  */
+
+/**
+ * @name    Gyroscope data structures and types
+ * @{
+ */
+
+/**
+ * @brief  Gyroscope Output Data Rate
+ */
+typedef enum {
+  LSM6DS0_GYRO_ODR_PD             = 0x00,             /*!< Power down */
+  LSM6DS0_GYRO_ODR_14_9Hz_CO_5Hz  = 0x20,             /*!< Output Data Rate = 14.9 Hz, CutOff = 5Hz */
+  LSM6DS0_GYRO_ODR_59_5Hz_CO_16Hz = 0x40,             /*!< Output Data Rate = 59.5 Hz, CutOff = 16Hz */
+  LSM6DS0_GYRO_ODR_119Hz_CO_14Hz  = 0x60,             /*!< Output Data Rate = 119 Hz, CutOff = 14Hz */
+  LSM6DS0_GYRO_ODR_119Hz_CO_31Hz  = 0x61,             /*!< Output Data Rate = 119 Hz, CutOff = 31Hz */
+  LSM6DS0_GYRO_ODR_238Hz_CO_14Hz  = 0x80,             /*!< Output Data Rate = 238 Hz, CutOff = 14Hz */
+  LSM6DS0_GYRO_ODR_238Hz_CO_29Hz  = 0x81,             /*!< Output Data Rate = 328 Hz, CutOff = 29Hz */
+  LSM6DS0_GYRO_ODR_238Hz_CO_63Hz  = 0x82,             /*!< Output Data Rate = 238 Hz, CutOff = 63Hz */
+  LSM6DS0_GYRO_ODR_238Hz_CO_78Hz  = 0x83,             /*!< Output Data Rate = 476 Hz, CutOff = 78Hz */
+  LSM6DS0_GYRO_ODR_476Hz_CO_21Hz  = 0xA0,             /*!< Output Data Rate = 476 Hz, CutOff = 21Hz */
+  LSM6DS0_GYRO_ODR_476Hz_CO_28Hz  = 0xA1,             /*!< Output Data Rate = 238 Hz, CutOff = 28Hz */
+  LSM6DS0_GYRO_ODR_476Hz_CO_57Hz  = 0xA2,             /*!< Output Data Rate = 476 Hz, CutOff = 57Hz */
+  LSM6DS0_GYRO_ODR_476Hz_CO_100Hz = 0xA3,             /*!< Output Data Rate = 476 Hz, CutOff = 100Hz */
+  LSM6DS0_GYRO_ODR_952Hz_CO_33Hz  = 0xC0,             /*!< Output Data Rate = 952 Hz, CutOff = 33Hz */
+  LSM6DS0_GYRO_ODR_952Hz_CO_40Hz  = 0xC1,             /*!< Output Data Rate = 952 Hz, CutOff = 40Hz */
+  LSM6DS0_GYRO_ODR_952Hz_CO_58Hz  = 0xC2,             /*!< Output Data Rate = 952 Hz, CutOff = 58Hz */
+  LSM6DS0_GYRO_ODR_952Hz_CO_100Hz = 0xC3              /*!< Output Data Rate = 952 Hz, CutOff = 100Hz */
+}LSM6DS0_GYRO_ODR_t;
+
+/**
+ * @brief  Gyroscope Full Scale
+ */
+typedef enum {
+  LSM6DS0_GYRO_FS_245DSP  = 0x00,                     /*!< �245 degrees per second */
+  LSM6DS0_GYRO_FS_500DSP  = 0x08,                     /*!< �500 degrees per second */
+  LSM6DS0_GYRO_FS_2000DSP = 0x18                      /*!< �2000 degrees per second */
+}LSM6DS0_GYRO_FS_t;
+
+/**
+ * @brief  Gyroscope Output Selection
+ */
+typedef enum {
+  LSM6DS0_GYRO_OUT_SEL_BYPASS    = 0x00,              /*!< Output not filtered */
+  LSM6DS0_GYRO_OUT_SEL_FILTERED  = 0x01,              /*!< Output filtered */
+}LSM6DS0_GYRO_OUT_SEL_t;
+
+/**
+ * @brief  Gyroscope Interrupt Selection
+ */
+typedef enum {
+  LSM6DS0_GYRO_INT_SEL_BYPASS    = 0x00,              /*!< Interrupt generator signal not filtered */
+  LSM6DS0_GYRO_INT_SEL_FILTERED  = 0x08,              /*!< Interrupt generator signal filtered */
+}LSM6DS0_GYRO_INT_SEL_t;
+
+/**
+ * @brief  Gyroscope Low Power Mode
+ */
+typedef enum {
+  LSM6DS0_GYRO_LP_MODE_HIGH_PERFORMANCE = 0x00,       /*!< High performance */
+  LSM6DS0_GYRO_LP_MODE_LOW_POWER        = 0x80,       /*!< Low power */
+}LSM6DS0_GYRO_LP_MODE_t;
+
+/**
+ * @brief  Gyroscope High Pass Filter Cutoff Selection
+ */
+typedef enum {
+  LSM6DS0_GYRO_HPCF_DISABLED = 0x00,                  /*!< HP filter disabled  */
+  LSM6DS0_GYRO_HPCF_0        = 0x40,                  /*!< Config 0 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_1        = 0x41,                  /*!< Config 1 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_2        = 0x42,                  /*!< Config 2 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_3        = 0x43,                  /*!< Config 3 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_4        = 0x44,                  /*!< Config 4 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_5        = 0x45,                  /*!< Config 5 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_6        = 0x46,                  /*!< Config 6 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_7        = 0x47,                  /*!< Config 7 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_8        = 0x48,                  /*!< Config 8 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_9        = 0x49,                  /*!< Config 9 refer to table 48 of DOcID025604 Rev 3  */
+  LSM6DS0_GYRO_HPCF_10       = 0x4A                   /*!< Config 10 refer to table 48 of DOcID025604 Rev 3  */
+}LSM6DS0_GYRO_HPCF_t;
+
+/**
+ * @brief   Gyroscope Axes Enabling
+ */
+typedef enum{
+  LSM6DS0_GYRO_AE_DISABLED = 0x00,                    /*!< Axes all disabled */
+  LSM6DS0_GYRO_AE_X        = 0x08,                    /*!< Only X-axis enabled */
+  LSM6DS0_GYRO_AE_Y        = 0x10,                    /*!< Only Y-axis enabled */
+  LSM6DS0_GYRO_AE_XY       = 0x18,                    /*!< X & Y axes enabled */
+  LSM6DS0_GYRO_AE_Z        = 0x20,                    /*!< Only Z-axis enabled */
+  LSM6DS0_GYRO_AE_XZ       = 0x28,                    /*!< X & Z axes enabled  */
+  LSM6DS0_GYRO_AE_YZ       = 0x30,                    /*!< Y & Z axes enabled  */
+  LSM6DS0_GYRO_AE_XYZ      = 0x38                     /*!< All axes enabled */
+}LSM6DS0_GYRO_AE_t;
+
+/**
+ * @brief  Gyroscope Decimation Mode
+ */
+typedef enum {
+  LSM6DS0_GYRO_DEC_DISABLED = 0x00,                   /*!< NO decimation */
+  LSM6DS0_GYRO_DEC_X2       = 0x40,                   /*!< Decimation update every 2 sample */
+  LSM6DS0_GYRO_DEC_X4       = 0x80,                   /*!< Decimation update every 4 sample */
+  LSM6DS0_GYRO_DEC_X8       = 0xC0                    /*!< Decimation update every 8 sample */
+}LSM6DS0_GYRO_DEC_t;
+
+/**
+ * @brief  Gyroscope Sleep Mode
+ */
+typedef enum {
+  LSM6DS0_GYRO_SLP_DISABLED = 0x00,                   /*!< Gyroscope sleep mode disabled */
+  LSM6DS0_GYRO_SLP_ENABLED  = 0x40                    /*!< Gyroscope sleep mode enabled */
+}LSM6DS0_GYRO_SLP_t;
+/**
+ * @brief   Gyroscope configuration structure.
+ */
+typedef struct {
+  /**
+   * @brief  LSM6DS0 Slave Address
+   */
+  LSM6DS0_SAD_t   slaveaddress;
+  /**
+   * @brief  Gyroscope Output Data Rate
+   */
+  LSM6DS0_GYRO_ODR_t   outputdatarate;
+  /**
+   * @brief  Gyroscope Full Scale
+   */
+  LSM6DS0_GYRO_FS_t    fullscale;
+  /**
+   * @brief  Gyroscope Output Selection
+   */
+  LSM6DS0_GYRO_OUT_SEL_t outputselect;
+  /**
+   * @brief  Gyroscope Interrupt Selection
+   */
+  LSM6DS0_GYRO_INT_SEL_t irqselect;
+  /**
+   * @brief  Gyroscope Low Power Mode
+   */
+  LSM6DS0_GYRO_LP_MODE_t lowpowermode;
+  /**
+   * @brief  Gyroscope High Pass Filter Cutoff Selection
+   */
+  LSM6DS0_GYRO_HPCF_t HPCfrequency;
+  /**
+   * @brief   Gyroscope Axes Enabling
+   */
+  LSM6DS0_GYRO_AE_t axesenabling;
+  /**
+   * @brief  Gyroscope Decimation Mode
+   */
+  LSM6DS0_GYRO_DEC_t decimation;
+  /**
+   * @brief  LSM6DS0 Endianness
+   */
+  LSM6DS0_END_t   endianess;
+  /**
+   * @brief  LSM6DS0 Block Data Update
+   */
+  LSM6DS0_BDU_t   blockdataupdate;
+} LSM6DS0_GYRO_Config;
+/** @}  */
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+  uint8_t lsm6ds0ReadRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                                 msg_t* message);
+  void lsm6ds0WriteRegister(I2CDriver *i2cp, uint8_t sad, uint8_t sub,
+                                 uint8_t value, msg_t* message);
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _LSM6DS0_H_ */
+
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/others/max7219.c b/lib/chibios-contrib/os/various/devices_lib/others/max7219.c
new file mode 100644
index 000000000..0e511671c
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/others/max7219.c
@@ -0,0 +1,94 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    max7219.c
+ * @brief   MAX7219 display driver module code.
+ *
+ * @addtogroup max7219
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#include "max7219.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Reads a generic register value.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] adr       address number
+ * @param[in] data      data value.
+ */
+void max7219WriteRegister(SPIDriver *spip, uint16_t adr, uint8_t data) {
+
+  switch (adr) {
+    default:
+      return;
+    case MAX7219_AD_DIGIT_0:
+    case MAX7219_AD_DIGIT_1:
+    case MAX7219_AD_DIGIT_2:
+    case MAX7219_AD_DIGIT_3:
+    case MAX7219_AD_DIGIT_4:
+    case MAX7219_AD_DIGIT_5:
+    case MAX7219_AD_DIGIT_6:
+    case MAX7219_AD_DIGIT_7:
+    case MAX7219_AD_DECODE_MODE:
+    case MAX7219_AD_INTENSITY:
+    case MAX7219_AD_SCAN_LIMIT:
+    case MAX7219_AD_SHUTDOWN:
+    case MAX7219_AD_DISPLAY_TEST:
+      spiSelect(spip);
+      uint16_t txbuf = {adr | data};
+      spiSend(spip, 1, &txbuf);
+      spiUnselect(spip);
+  }
+}
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/others/max7219.h b/lib/chibios-contrib/os/various/devices_lib/others/max7219.h
new file mode 100644
index 000000000..e672be946
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/others/max7219.h
@@ -0,0 +1,187 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    MAX7219.h
+ * @brief   MAX7219 display driver module header.
+ *
+ * @{
+ */
+
+#ifndef _MAX7219_H_
+#define _MAX7219_H_
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+/**
+ * @name    MAX7219 register names
+ * @{
+ */
+/******************************************************************************/
+/*                                                                            */
+/*                        MAX7219 display driver                              */
+/*                                                                            */
+/******************************************************************************/
+/*******************  Bit definition for SPI communication  *******************/
+#define  MAX7219_DI                              ((uint16_t)0x00FF)          /*!< DI[7:0] Data input */
+#define  MAX7219_DI_0                            ((uint16_t)0x0001)          /*!< bit 0 */
+#define  MAX7219_DI_1                            ((uint16_t)0x0002)          /*!< bit 1 */
+#define  MAX7219_DI_2                            ((uint16_t)0x0004)          /*!< bit 2 */
+#define  MAX7219_DI_3                            ((uint16_t)0x0008)          /*!< bit 3 */
+#define  MAX7219_DI_4                            ((uint16_t)0x0010)          /*!< bit 4 */
+#define  MAX7219_DI_5                            ((uint16_t)0x0020)          /*!< bit 5 */
+#define  MAX7219_DI_6                            ((uint16_t)0x0040)          /*!< bit 6 */
+#define  MAX7219_DI_7                            ((uint16_t)0x0080)          /*!< bit 7 */
+
+#define  MAX7219_AD                              ((uint16_t)0x0F00)          /*!< AD[11:8] Data input */
+#define  MAX7219_AD_0                            ((uint16_t)0x0100)          /*!< bit 8 */
+#define  MAX7219_AD_1                            ((uint16_t)0x0200)          /*!< bit 9 */
+#define  MAX7219_AD_2                            ((uint16_t)0x0400)          /*!< bit 10 */
+#define  MAX7219_AD_3                            ((uint16_t)0x0800)          /*!< bit 11 */
+
+/******************  Bit definition for Registers Addresses *******************/
+#define  MAX7219_AD_NOP                          ((uint16_t)0x0000)          /*!< No operation */
+#define  MAX7219_AD_DIGIT_0                      ((uint16_t)0x0100)          /*!< Digit 0 */
+#define  MAX7219_AD_DIGIT_1                      ((uint16_t)0x0200)          /*!< Digit 1 */
+#define  MAX7219_AD_DIGIT_2                      ((uint16_t)0x0300)          /*!< Digit 2 */
+#define  MAX7219_AD_DIGIT_3                      ((uint16_t)0x0400)          /*!< Digit 3 */
+#define  MAX7219_AD_DIGIT_4                      ((uint16_t)0x0500)          /*!< Digit 4 */
+#define  MAX7219_AD_DIGIT_5                      ((uint16_t)0x0600)          /*!< Digit 5 */
+#define  MAX7219_AD_DIGIT_6                      ((uint16_t)0x0700)          /*!< Digit 6 */
+#define  MAX7219_AD_DIGIT_7                      ((uint16_t)0x0800)          /*!< Digit 7 */
+#define  MAX7219_AD_DECODE_MODE                  ((uint16_t)0x0900)          /*!< Decode mode */
+#define  MAX7219_AD_INTENSITY                    ((uint16_t)0x0A00)          /*!< Intensity */
+#define  MAX7219_AD_SCAN_LIMIT                   ((uint16_t)0x0B00)          /*!< Scan limit */
+#define  MAX7219_AD_SHUTDOWN                     ((uint16_t)0x0C00)          /*!< Shutdown */
+#define  MAX7219_AD_DISPLAY_TEST                 ((uint16_t)0x0F00)          /*!< Display test */
+
+/***************  Bit definition for Registers Configuration  *****************/
+/** @} */
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if !HAL_USE_SPI
+#error "MAX7219 requires HAL_USE_SPI"
+#endif
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    MAX7219 data structures and types
+ * @{
+ *
+ */
+
+/**
+ * @brief  MAX7219 operation mode
+ */
+typedef enum {
+  MAX7219_OM_Shutdown  = 0x00,                 /*!< Shutdown mode */
+  MAX7219_OM_Normal    = 0x01                  /*!< Normal mode */
+} MAX7219_OM_t;
+
+/**
+ * @brief  MAX7219 decoder mode
+ */
+typedef enum {
+  MAX7219_DM_No_decode  = 0x00,                 /*!< No decode */
+  MAX7219_DM_CodeB_0    = 0x01,                 /*!< Code B on Digit 0 */
+  MAX7219_DM_CodeB_1    = 0x03,                 /*!< Code B on Digits 0-1 */
+  MAX7219_DM_CodeB_2    = 0x07,                 /*!< Code B on Digits from 0 to 2 */
+  MAX7219_DM_CodeB_3    = 0x0F,                 /*!< Code B on Digits from 0 to 3 */
+  MAX7219_DM_CodeB_4    = 0x1F,                 /*!< Code B on Digits from 0 to 4 */
+  MAX7219_DM_CodeB_5    = 0x3F,                 /*!< Code B on Digits from 0 to 5 */
+  MAX7219_DM_CodeB_6    = 0x7F,                 /*!< Code B on Digits from 0 to 6 */
+  MAX7219_DM_CodeB_7    = 0xFF                  /*!< Code B on every digit */
+} MAX7219_DM_t;
+
+/**
+ * @brief  MAX7219 intensity mode
+ */
+typedef enum {
+  MAX7219_IM_1_32       = 0x00,                 /*!< 1/32 intensity */
+  MAX7219_IM_3_32       = 0x01,                 /*!< 3/32 intensity */
+  MAX7219_IM_5_32       = 0x02,                 /*!< 5/32 intensity */
+  MAX7219_IM_7_32       = 0x03,                 /*!< 7/32 intensity */
+  MAX7219_IM_9_32       = 0x04,                 /*!< 9/32 intensity */
+  MAX7219_IM_11_32      = 0x05,                 /*!< 11/32 intensity */
+  MAX7219_IM_13_32      = 0x06,                 /*!< 13/32 intensity */
+  MAX7219_IM_15_32      = 0x07,                 /*!< 15/32 intensity */
+  MAX7219_IM_17_32      = 0x08,                 /*!< 17/32 intensity */
+  MAX7219_IM_19_32      = 0x09,                 /*!< 19/32 intensity */
+  MAX7219_IM_21_32      = 0x0A,                 /*!< 21/32 intensity */
+  MAX7219_IM_23_32      = 0x0B,                 /*!< 23/32 intensity */
+  MAX7219_IM_25_32      = 0x0C,                 /*!< 25/32 intensity */
+  MAX7219_IM_27_32      = 0x0D,                 /*!< 27/32 intensity */
+  MAX7219_IM_29_32      = 0x0E,                 /*!< 29/32 intensity */
+  MAX7219_IM_31_32      = 0x0F                  /*!< 31/32 intensity */
+} MAX7219_IM_t;
+
+/**
+ * @brief  MAX7219 scan line mode
+ */
+typedef enum {
+  MAX7219_SL_0          = 0x00,                 /*!< Scanned digit 0 only */
+  MAX7219_SL_1          = 0x01,                 /*!< Scanned digit 0 & 1 */
+  MAX7219_SL_2          = 0x02,                 /*!< Scanned digit 0 - 2 */
+  MAX7219_SL_3          = 0x03,                 /*!< Scanned digit 0 - 3 */
+  MAX7219_SL_4          = 0x04,                 /*!< Scanned digit 0 - 4 */
+  MAX7219_SL_5          = 0x05,                 /*!< Scanned digit 0 - 5 */
+  MAX7219_SL_6          = 0x06,                 /*!< Scanned digit 0 - 6 */
+  MAX7219_SL_7          = 0x07                  /*!< Scanned digit 0 - 7 */
+} MAX7219_SL_t;
+/** @}  */
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+  void max7219WriteRegister(SPIDriver *spip, uint16_t adr, uint8_t data);
+#ifdef __cplusplus
+}
+#endif
+#endif /* _MAX7219_H_ */
+
+/** @} */
+
diff --git a/lib/chibios-contrib/os/various/devices_lib/rf/nrf24l01.c b/lib/chibios-contrib/os/various/devices_lib/rf/nrf24l01.c
new file mode 100644
index 000000000..f526fbeff
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/rf/nrf24l01.c
@@ -0,0 +1,440 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    nrf24l01.c
+ * @brief   NRF24L01 interface module code.
+ *
+ * @addtogroup nrf24l01
+ * @{
+ */
+
+#include "ch.h"
+#include "hal.h"
+
+#include "nrf24l01.h"
+
+/*===========================================================================*/
+/* Driver local definitions.                                                 */
+/*===========================================================================*/
+
+#define ACTIVATE                  0x73
+/*===========================================================================*/
+/* Driver exported variables.                                                */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local variables and types.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver local functions.                                                   */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Driver exported functions.                                                */
+/*===========================================================================*/
+
+/**
+ * @brief   Gets the status register value.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01GetStatus(SPIDriver *spip) {
+  uint8_t txbuf = NRF24L01_CMD_NOP;
+  uint8_t status;
+  spiSelect(spip);
+  spiExchange(spip, 1, &txbuf, &status);
+  spiUnselect(spip);
+  return status;
+}
+
+/**
+ * @brief   Reads a generic register value.
+ *
+ * @note    Cannot be used to set addresses
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] reg       register number
+ * @param[out] pvalue   pointer to a data buffer
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01ReadRegister(SPIDriver *spip, uint8_t reg,
+                                       uint8_t* pvalue) {
+  uint8_t txbuf = (NRF24L01_CMD_READ | reg);
+  uint8_t status = 0xFF;
+  spiSelect(spip);
+  spiExchange(spip, 1, &txbuf, &status);
+  spiReceive(spip, 1, pvalue);
+  spiUnselect(spip);
+  return status;
+}
+
+/**
+ * @brief   Writes a generic register value.
+ *
+ * @note    Cannot be used to set addresses
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] reg       register number
+ * @param[in] value     data value
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01WriteRegister(SPIDriver *spip, uint8_t reg,
+                                        uint8_t value) {
+
+  uint8_t txbuf[2] = {(NRF24L01_CMD_WRITE | reg), value};
+  uint8_t rxbuf[2] = {0xFF, 0xFF};
+  switch (reg) {
+
+    default:
+      /* Reserved register must not be written, according to the datasheet
+       * this could permanently damage the device.
+       */
+      chDbgAssert(FALSE, "lg3d20WriteRegister(), reserved register");
+    case NRF24L01_AD_OBSERVE_TX:
+    case NRF24L01_AD_CD:
+    case NRF24L01_AD_RX_ADDR_P0:
+    case NRF24L01_AD_RX_ADDR_P1:
+    case NRF24L01_AD_RX_ADDR_P2:
+    case NRF24L01_AD_RX_ADDR_P3:
+    case NRF24L01_AD_RX_ADDR_P4:
+    case NRF24L01_AD_RX_ADDR_P5:
+    case NRF24L01_AD_TX_ADDR:
+    /* Read only or addresses registers cannot be written,
+     * the command is ignored.
+     */
+      return 0;
+    case NRF24L01_AD_CONFIG:
+    case NRF24L01_AD_EN_AA:
+    case NRF24L01_AD_EN_RXADDR:
+    case NRF24L01_AD_SETUP_AW:
+    case NRF24L01_AD_SETUP_RETR:
+    case NRF24L01_AD_RF_CH:
+    case NRF24L01_AD_RF_SETUP:
+    case NRF24L01_AD_STATUS:
+    case NRF24L01_AD_RX_PW_P0:
+    case NRF24L01_AD_RX_PW_P1:
+    case NRF24L01_AD_RX_PW_P2:
+    case NRF24L01_AD_RX_PW_P3:
+    case NRF24L01_AD_RX_PW_P4:
+    case NRF24L01_AD_RX_PW_P5:
+    case NRF24L01_AD_FIFO_STATUS:
+    case NRF24L01_AD_DYNPD:
+    case NRF24L01_AD_FEATURE:
+      spiSelect(spip);
+      spiExchange(spip, 2, txbuf, rxbuf);
+      spiUnselect(spip);
+      return rxbuf[0];
+  }
+}
+
+
+/**
+ * @brief   Writes an address.
+ *
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] reg       register number
+ * @param[in] pvalue    pointer to address value
+ * @param[in] addlen    address len
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01WriteAddress(SPIDriver *spip, uint8_t reg,
+                                       uint8_t *pvalue, uint8_t addlen) {
+
+  uint8_t txbuf[NRF24L01_MAX_ADD_LENGHT + 1];
+  uint8_t rxbuf[NRF24L01_MAX_ADD_LENGHT + 1];
+  unsigned i;
+
+  if(addlen > NRF24L01_MAX_ADD_LENGHT) {
+    chDbgAssert(FALSE, "nrf24l01WriteAddress(), wrong address length");
+    return 0;
+  }
+  txbuf[0] = (NRF24L01_CMD_WRITE | reg);
+  rxbuf[0] = 0xFF;
+  for(i = 1; i <= addlen; i++) {
+    txbuf[i] = *(pvalue + (i - 1));
+    rxbuf[i] = 0xFF;
+  }
+  switch (reg) {
+
+    default:
+      /* Reserved register must not be written, according to the datasheet
+       * this could permanently damage the device.
+       */
+      chDbgAssert(FALSE, "nrf24l01WriteAddress(), reserved register");
+    case NRF24L01_AD_OBSERVE_TX:
+    case NRF24L01_AD_CD:
+    case NRF24L01_AD_CONFIG:
+    case NRF24L01_AD_EN_AA:
+    case NRF24L01_AD_EN_RXADDR:
+    case NRF24L01_AD_SETUP_AW:
+    case NRF24L01_AD_SETUP_RETR:
+    case NRF24L01_AD_RF_CH:
+    case NRF24L01_AD_RF_SETUP:
+    case NRF24L01_AD_STATUS:
+    case NRF24L01_AD_RX_PW_P0:
+    case NRF24L01_AD_RX_PW_P1:
+    case NRF24L01_AD_RX_PW_P2:
+    case NRF24L01_AD_RX_PW_P3:
+    case NRF24L01_AD_RX_PW_P4:
+    case NRF24L01_AD_RX_PW_P5:
+    case NRF24L01_AD_FIFO_STATUS:
+    case NRF24L01_AD_DYNPD:
+    case NRF24L01_AD_FEATURE:
+    /* Not address registers cannot be written, the command is ignored.*/
+      return 0;
+    case NRF24L01_AD_RX_ADDR_P0:
+    case NRF24L01_AD_RX_ADDR_P1:
+    case NRF24L01_AD_RX_ADDR_P2:
+    case NRF24L01_AD_RX_ADDR_P3:
+    case NRF24L01_AD_RX_ADDR_P4:
+    case NRF24L01_AD_RX_ADDR_P5:
+    case NRF24L01_AD_TX_ADDR:
+      spiSelect(spip);
+      spiExchange(spip, addlen + 1, txbuf, rxbuf);
+      spiUnselect(spip);
+      return rxbuf[0];
+  }
+}
+/**
+ * @brief   Reads RX payload from FIFO.
+ *
+ * @note    Payload is deleted from FIFO after it is read. Used in RX mode.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] paylen    payload length
+ * @param[in] rxbuf     pointer to a buffer
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01GetRxPl(SPIDriver *spip, uint8_t paylen,
+                                  uint8_t* rxbuf) {
+
+  uint8_t txbuf = NRF24L01_CMD_R_RX_PAYLOAD;
+  uint8_t status;
+  if(paylen > NRF24L01_MAX_PL_LENGHT) {
+    return 0;
+  }
+  spiSelect(spip);
+  spiExchange(spip, 1, &txbuf, &status);
+  spiReceive(spip, paylen, rxbuf);
+  spiUnselect(spip);
+  return status;
+}
+
+/**
+ * @brief   Writes TX payload on FIFO.
+ *
+ * @note    Used in TX mode.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] paylen    payload length
+ * @param[in] rxbuf     pointer to a buffer
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01WriteTxPl(SPIDriver *spip, uint8_t paylen,
+                                    uint8_t* txbuf) {
+
+  uint8_t cmd = NRF24L01_CMD_W_TX_PAYLOAD;
+  uint8_t status;
+  if(paylen > NRF24L01_MAX_PL_LENGHT) {
+    return 0;
+  }
+  spiSelect(spip);
+  spiExchange(spip, 1, &cmd, &status);
+  spiSend(spip, paylen, txbuf);
+  spiUnselect(spip);
+  return status;
+}
+
+/**
+ * @brief   Flush TX FIFO.
+ *
+ * @note    Used in TX mode.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01FlushTx(SPIDriver *spip) {
+
+  uint8_t txbuf = NRF24L01_CMD_FLUSH_TX;
+  uint8_t status;
+  spiSelect(spip);
+  spiExchange(spip, 1, &txbuf, &status);
+  spiUnselect(spip);
+  return status;
+}
+
+/**
+ * @brief   Flush RX FIFO.
+ *
+ * @note    Used in RX mode. Should not be executed during transmission of
+            acknowledge, that is, acknowledge package will not be completed.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01FlushRx(SPIDriver *spip) {
+
+  uint8_t txbuf = NRF24L01_CMD_FLUSH_RX;
+  uint8_t status;
+  spiSelect(spip);
+  spiExchange(spip, 1, &txbuf, &status);
+  spiUnselect(spip);
+  return status;
+}
+
+#if NRF24L01_USE_FEATURE || defined(__DOXYGEN__)
+/**
+ * @brief   Activates the following features:
+ *          R_RX_PL_WID        -> (In order to enable DPL the EN_DPL bit in the
+ *                                 FEATURE register must be set)
+ *          W_ACK_PAYLOAD      -> (In order to enable PL with ACK the EN_ACK_PAY
+ *                                 bit in the FEATURE register must be set)
+ *          W_TX_PAYLOAD_NOACK -> (In order to send a PL without ACK
+ *                                 the EN_DYN_ACK it in the FEATURE register
+ *                                 must be set)
+ *
+ * @note    A new ACTIVATE command with the same data deactivates them again.
+ *          This is executable in power down or stand by modes only.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01Activate(SPIDriver *spip) {
+
+  uint8_t txbuf[2] = {NRF24L01_CMD_FLUSH_RX, ACTIVATE};
+  uint8_t rxbuf[2];
+  spiSelect(spip);
+  spiExchange(spip, 2, txbuf, rxbuf);
+  spiUnselect(spip);
+  return rxbuf[0];
+}
+
+/**
+ * @brief   Reads RX payload lenght for the top R_RX_PAYLOAD
+ *          in the RX FIFO when Dynamic Payload Length is activated.
+ *
+ * @note    R_RX_PL_WID must be set and activated.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] ppaylen   pointer to the payload length variable
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01ReadRxPlWid(SPIDriver *spip, uint8_t *ppaylen) {
+
+  uint8_t txbuf[2] = {NRF24L01_CMD_R_RX_PL_WID, 0xFF};
+  uint8_t rxbuf[2];
+  spiSelect(spip);
+  spiExchange(spip, 2, txbuf, rxbuf);
+  spiUnselect(spip);
+  *ppaylen = rxbuf[1];
+  return rxbuf[0];
+}
+
+/**
+ * @brief   Writes TX payload associateted to ACK.
+ *
+ * @note    Used in RX mode. Write Payload to be transmitted together with
+ *          ACK packet on PIPE PPP. (PPP valid in the range from 000 to 101).
+ * @note    EN_ACK_PAY must be set and activated.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] paylen    payload length
+ * @param[in] rxbuf     pointer to a buffer
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01WriteAckPl(SPIDriver *spip, uint8_t ppp, uint8_t paylen,
+                                       uint8_t* payload){
+
+  payload[0] = NRF24L01_CMD_W_ACK_PAYLOAD | NRF24L01_MAX_PPP;
+  uint8_t status;
+  if((paylen > NRF24L01_MAX_PL_LENGHT) || (ppp > NRF24L01_MAX_PPP)) {
+    return 0;
+  }
+  spiSelect(spip);
+  spiExchange(spip, 1, payload, &status);
+  spiSend(spip, paylen, payload);
+  spiUnselect(spip);
+  return status;
+}
+
+/**
+ * @brief   Writes next TX payload without ACK.
+ *
+ * @note    Used in TX mode.
+ * @note    EN_DYN_ACK must be set and activated.
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ * @param[in] paylen    payload length
+ * @param[in] rxbuf     pointer to a buffer
+ *
+ * @return              the status register value
+ */
+NRF24L01_status_t nrf24l01WriteTxPlNoAck(SPIDriver *spip, uint8_t paylen,
+                                    uint8_t* txbuf) {
+
+  txbuf[0] = NRF24L01_CMD_W_TX_PAYLOAD_NOACK;
+  uint8_t status;
+  if(paylen > NRF24L01_MAX_PL_LENGHT) {
+    return 0;
+  }
+  spiSelect(spip);
+  spiExchange(spip, 1, txbuf, &status);
+  spiSend(spip, paylen, txbuf);
+  spiUnselect(spip);
+  return status;
+}
+#endif /* NRF24L01_USE_FEATURE */
+
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/rf/nrf24l01.h b/lib/chibios-contrib/os/various/devices_lib/rf/nrf24l01.h
new file mode 100644
index 000000000..86ba1279d
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/rf/nrf24l01.h
@@ -0,0 +1,575 @@
+/*
+    Pretty LAYer for ChibiOS/RT - Copyright (C) 2015 Rocco Marco Guglielmi
+        
+    This file is part of PLAY for ChibiOS/RT.
+
+    PLAY 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.
+
+    PLAY 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/>.
+*/
+
+/*
+    Special thanks to Giovanni Di Sirio for teachings, his moral support and
+    friendship. Note that some or every piece of this file could be part of
+    the ChibiOS project that is intellectual property of Giovanni Di Sirio.
+    Please refer to ChibiOS/RT license before use this file.
+        
+        For suggestion or Bug report - [email protected]
+ */
+
+/**
+ * @file    nrf24l01.h
+ * @brief   NRF24L01 Radio frequency module interface module header.
+ *
+ * @{
+ */
+
+#ifndef _NRF24L01_H_
+#define _NRF24L01_H_
+
+/*===========================================================================*/
+/* Driver constants.                                                         */
+/*===========================================================================*/
+
+#define  NRF24L01_MAX_ADD_LENGHT                 ((uint8_t)  5)
+#define  NRF24L01_MAX_PL_LENGHT                  ((uint8_t) 32)
+#define  NRF24L01_MAX_PPP                        ((uint8_t)  5)
+
+/**
+ * @brief   Enables Advanced Features.
+ */
+#if !defined(NRF24L01_USE_FEATURE) || defined(__DOXYGEN__)
+#define NRF24L01_USE_FEATURE                     TRUE
+#endif
+
+/**
+ * @name    NRF24L01 register names
+ * @{
+ */
+/******************************************************************************/
+/*                                                                            */
+/*                        NRF24L01 RF Transceiver                             */
+/*                                                                            */
+/******************************************************************************/
+/******************  Bit definition for SPI communication  ********************/
+#define  NRF24L01_DI                             ((uint8_t)0xFF)             /*!< DI[7:0] Data input */
+#define  NRF24L01_DI_0                           ((uint8_t)0x01)             /*!< bit 0 */
+#define  NRF24L01_DI_1                           ((uint8_t)0x02)             /*!< bit 1 */
+#define  NRF24L01_DI_2                           ((uint8_t)0x04)             /*!< bit 2 */
+#define  NRF24L01_DI_3                           ((uint8_t)0x08)             /*!< bit 3 */
+#define  NRF24L01_DI_4                           ((uint8_t)0x10)             /*!< bit 4 */
+#define  NRF24L01_DI_5                           ((uint8_t)0x20)             /*!< bit 5 */
+#define  NRF24L01_DI_6                           ((uint8_t)0x40)             /*!< bit 6 */
+#define  NRF24L01_DI_7                           ((uint8_t)0x80)             /*!< bit 7 */
+
+#define  NRF24L01_AD                             ((uint8_t)0x1F)             /*!< AD[4:0] Address Data */
+#define  NRF24L01_AD_0                           ((uint8_t)0x01)             /*!< bit 0 */
+#define  NRF24L01_AD_1                           ((uint8_t)0x02)             /*!< bit 1 */
+#define  NRF24L01_AD_2                           ((uint8_t)0x04)             /*!< bit 2 */
+#define  NRF24L01_AD_3                           ((uint8_t)0x08)             /*!< bit 3 */
+#define  NRF24L01_AD_4                           ((uint8_t)0x10)             /*!< bit 4 */
+
+#define  NRF24L01_CMD_READ                       ((uint8_t)0x00)             /*!< Read command */
+#define  NRF24L01_CMD_WRITE                      ((uint8_t)0x20)             /*!< Write command */
+#define  NRF24L01_CMD_R_RX_PAYLOAD               ((uint8_t)0x61)             /*!< Read RX-payload*/
+#define  NRF24L01_CMD_W_TX_PAYLOAD               ((uint8_t)0xA0)             /*!< Write TX-payload */
+#define  NRF24L01_CMD_FLUSH_TX                   ((uint8_t)0xE1)             /*!< Flush TX FIFO */
+#define  NRF24L01_CMD_FLUSH_RX                   ((uint8_t)0xE2)             /*!< Flush RX FIFO */
+#define  NRF24L01_CMD_REUSE_TX_PL                ((uint8_t)0xE3)             /*!< Used for a PTX device */
+#define  NRF24L01_CMD_ACTIVATE                   ((uint8_t)0x50)             /*!< Activate command */
+#define  NRF24L01_CMD_R_RX_PL_WID                ((uint8_t)0x60)             /*!< Read RX-payload width */
+#define  NRF24L01_CMD_W_ACK_PAYLOAD              ((uint8_t)0xA8)             /*!< Write Payload for ACK */
+#define  NRF24L01_CMD_W_TX_PAYLOAD_NOACK         ((uint8_t)0xB0)             /*!< Disables AUTOACK*/
+#define  NRF24L01_CMD_NOP                        ((uint8_t)0xFF)             /*!< No Operation */
+
+/******************  Bit definition for Registers Addresses *******************/
+#define  NRF24L01_AD_CONFIG                      ((uint8_t)0x00)             /*!< Configuration Register */
+#define  NRF24L01_AD_EN_AA                       ((uint8_t)0x01)             /*!< Enable �Auto Acknowledgment� */
+#define  NRF24L01_AD_EN_RXADDR                   ((uint8_t)0x02)             /*!< Enabled RX Addresses */
+#define  NRF24L01_AD_SETUP_AW                    ((uint8_t)0x03)             /*!< Setup of Address Widths */
+#define  NRF24L01_AD_SETUP_RETR                  ((uint8_t)0x04)             /*!< Setup of Automatic Retransmission */
+#define  NRF24L01_AD_RF_CH                       ((uint8_t)0x05)             /*!< RF Channel */
+#define  NRF24L01_AD_RF_SETUP                    ((uint8_t)0x06)             /*!< RF Setup Register */
+#define  NRF24L01_AD_STATUS                      ((uint8_t)0x07)             /*!< Status Register */
+#define  NRF24L01_AD_OBSERVE_TX                  ((uint8_t)0x08)             /*!< Transmit observe register */
+#define  NRF24L01_AD_CD                          ((uint8_t)0x09)             /*!< CD */
+#define  NRF24L01_AD_RX_ADDR_P0                  ((uint8_t)0x0A)             /*!< Receive address data pipe 0 */
+#define  NRF24L01_AD_RX_ADDR_P1                  ((uint8_t)0x0B)             /*!< Receive address data pipe 1 */
+#define  NRF24L01_AD_RX_ADDR_P2                  ((uint8_t)0x0C)             /*!< Receive address data pipe 2 */
+#define  NRF24L01_AD_RX_ADDR_P3                  ((uint8_t)0x0D)             /*!< Receive address data pipe 3 */
+#define  NRF24L01_AD_RX_ADDR_P4                  ((uint8_t)0x0E)             /*!< Receive address data pipe 4 */
+#define  NRF24L01_AD_RX_ADDR_P5                  ((uint8_t)0x0F)             /*!< Receive address data pipe 5 */
+#define  NRF24L01_AD_TX_ADDR                     ((uint8_t)0x10)             /*!< Transmit address */
+#define  NRF24L01_AD_RX_PW_P0                    ((uint8_t)0x11)             /*!< Number of bytes in RX payload in data pipe 0 */
+#define  NRF24L01_AD_RX_PW_P1                    ((uint8_t)0x12)             /*!< Number of bytes in RX payload in data pipe 1 */
+#define  NRF24L01_AD_RX_PW_P2                    ((uint8_t)0x13)             /*!< Number of bytes in RX payload in data pipe 2 */
+#define  NRF24L01_AD_RX_PW_P3                    ((uint8_t)0x14)             /*!< Number of bytes in RX payload in data pipe 3 */
+#define  NRF24L01_AD_RX_PW_P4                    ((uint8_t)0x15)             /*!< Number of bytes in RX payload in data pipe 4 */
+#define  NRF24L01_AD_RX_PW_P5                    ((uint8_t)0x16)             /*!< Number of bytes in RX payload in data pipe 5 */
+#define  NRF24L01_AD_FIFO_STATUS                 ((uint8_t)0x17)             /*!< FIFO Status Register */
+#define  NRF24L01_AD_DYNPD                       ((uint8_t)0x1C)             /*!< Enable dynamic payload length */
+#define  NRF24L01_AD_FEATURE                     ((uint8_t)0x1D)             /*!< Feature Register */
+
+/***************  Bit definition for Registers Configuration  *****************/
+#define  NRF24L01_DI_CONFIG                      ((uint8_t)0x7F)             /*!< CONTROL REGISTER BIT MASK*/
+#define  NRF24L01_DI_CONFIG_PRIM_RX              ((uint8_t)0x01)             /*!< RX/TX control - 1: PRX, 0: PTX */
+#define  NRF24L01_DI_CONFIG_PWR_UP               ((uint8_t)0x02)             /*!< 1: POWER UP, 0:POWER DOWN */
+#define  NRF24L01_DI_CONFIG_CRCO                 ((uint8_t)0x04)             /*!< CRC encoding scheme - 1:two bytes, 0:one byte */
+#define  NRF24L01_DI_CONFIG_EN_CRC               ((uint8_t)0x08)             /*!< Enable CRC. Forced high if one of the bits in the EN_AA is high */
+#define  NRF24L01_DI_CONFIG_MASK_MAX_RT          ((uint8_t)0x10)             /*!< Mask interrupt caused by MAX_RT - 1: Interrupt disabled, 0: Interrupt reflected on IRQ pin */
+#define  NRF24L01_DI_CONFIG_MASK_TX_DS           ((uint8_t)0x20)             /*!< Mask interrupt caused by TX_DS - 1: Interrupt disabled, 0: Interrupt reflected on IRQ pin */
+#define  NRF24L01_DI_CONFIG_MASK_RX_DR           ((uint8_t)0x40)             /*!< Mask interrupt caused by RX_DR - 1: Interrupt disabled, 0: Interrupt reflected on IRQ pin */
+
+#define  NRF24L01_DI_EN_AA                       ((uint8_t)0x3F)             /*!< ENABLE AUTO ACKNOLEDGMENT REGISTER BIT MASK */
+#define  NRF24L01_DI_EN_AA_P0                    ((uint8_t)0x01)             /*!< Enable auto acknowledgement data pipe 0 */
+#define  NRF24L01_DI_EN_AA_P1                    ((uint8_t)0x02)             /*!< Enable auto acknowledgement data pipe 1 */
+#define  NRF24L01_DI_EN_AA_P2                    ((uint8_t)0x04)             /*!< Enable auto acknowledgement data pipe 2 */
+#define  NRF24L01_DI_EN_AA_P3                    ((uint8_t)0x08)             /*!< Enable auto acknowledgement data pipe 3 */
+#define  NRF24L01_DI_EN_AA_P4                    ((uint8_t)0x10)             /*!< Enable auto acknowledgement data pipe 4 */
+#define  NRF24L01_DI_EN_AA_P5                    ((uint8_t)0x20)             /*!< Enable auto acknowledgement data pipe 5 */
+
+#define  NRF24L01_DI_EN_RXADDR                   ((uint8_t)0x3F)             /*!< ENABLE RX ADDRESSES REGISTER BIT MASK */
+#define  NRF24L01_DI_EN_RXADDR_P0                ((uint8_t)0x01)             /*!< Enable data pipe 0 */
+#define  NRF24L01_DI_EN_RXADDR_P1                ((uint8_t)0x02)             /*!< Enable data pipe 1 */
+#define  NRF24L01_DI_EN_RXADDR_P2                ((uint8_t)0x04)             /*!< Enable data pipe 2 */
+#define  NRF24L01_DI_EN_RXADDR_P3                ((uint8_t)0x08)             /*!< Enable data pipe 3 */
+#define  NRF24L01_DI_EN_RXADDR_P4                ((uint8_t)0x10)             /*!< Enable data pipe 4 */
+#define  NRF24L01_DI_EN_RXADDR_P5                ((uint8_t)0x20)             /*!< Enable data pipe 5 */
+
+#define  NRF24L01_DI_SETUP_AW                    ((uint8_t)0x03)             /*!< SETUP OF ADDRESSES WIDTHS REGISTER BIT MASK */
+#define  NRF24L01_DI_SETUP_AW_0                  ((uint8_t)0x01)             /*!< Addressed widths bit 0 */
+#define  NRF24L01_DI_SETUP_AW_1                  ((uint8_t)0x02)             /*!< Addressed widths bit 1 */
+
+#define  NRF24L01_DI_SETUP_RETR                  ((uint8_t)0xFF)             /*!< SETUP OF AUTOMATIC RETRANSMISSION REGISTER BIT MASK */
+#define  NRF24L01_DI_SETUP_RETR_ARC_0            ((uint8_t)0x01)             /*!< Auto Retransmit Count bit 0 */
+#define  NRF24L01_DI_SETUP_RETR_ARC_1            ((uint8_t)0x02)             /*!< Auto Retransmit Count bit 1 */
+#define  NRF24L01_DI_SETUP_RETR_ARC_2            ((uint8_t)0x04)             /*!< Auto Retransmit Count bit 2 */
+#define  NRF24L01_DI_SETUP_RETR_ARC_3            ((uint8_t)0x08)             /*!< Auto Retransmit Count bit 3 */
+#define  NRF24L01_DI_SETUP_RETR_ARD_0            ((uint8_t)0x10)             /*!< Auto Retransmit Delay bit 0 */
+#define  NRF24L01_DI_SETUP_RETR_ARD_1            ((uint8_t)0x20)             /*!< Auto Retransmit Delay bit 1 */
+#define  NRF24L01_DI_SETUP_RETR_ARD_2            ((uint8_t)0x40)             /*!< Auto Retransmit Delay bit 2 */
+#define  NRF24L01_DI_SETUP_RETR_ARD_3            ((uint8_t)0x80)             /*!< Auto Retransmit Delay bit 3 */
+
+
+#define  NRF24L01_DI_RF_CH                       ((uint8_t)0x7F)             /*!< RF CHANNEL REGISTER BIT MASK */
+#define  NRF24L01_DI_RF_CH_0                     ((uint8_t)0x01)             /*!< RF channel bit 0 */
+#define  NRF24L01_DI_RF_CH_1                     ((uint8_t)0x02)             /*!< RF channel bit 1 */
+#define  NRF24L01_DI_RF_CH_2                     ((uint8_t)0x04)             /*!< RF channel bit 2 */
+#define  NRF24L01_DI_RF_CH_3                     ((uint8_t)0x08)             /*!< RF channel bit 3 */
+#define  NRF24L01_DI_RF_CH_4                     ((uint8_t)0x10)             /*!< RF channel bit 4 */
+#define  NRF24L01_DI_RF_CH_5                     ((uint8_t)0x20)             /*!< RF channel bit 5 */
+#define  NRF24L01_DI_RF_CH_6                     ((uint8_t)0x40)             /*!< RF channel bit 6 */
+
+
+#define  NRF24L01_DI_RF_SETUP                    ((uint8_t)0x1F)             /*!< RF SETUP REGISTER BIT MASK */
+#define  NRF24L01_DI_RF_SETUP_LNA_HCURR          ((uint8_t)0x01)             /*!< Setup LNA gain */
+#define  NRF24L01_DI_RF_SETUP_RF_PWR_0           ((uint8_t)0x02)             /*!< RF output power bit 0 */
+#define  NRF24L01_DI_RF_SETUP_RF_PWR_1           ((uint8_t)0x04)             /*!< RF output power bit 1 */
+#define  NRF24L01_DI_RF_SETUP_RF_DR              ((uint8_t)0x08)             /*!< Air Data rate - 0: 1Mbps, 1: 2Mbps */
+#define  NRF24L01_DI_RF_SETUP_PLL_LOCK           ((uint8_t)0x10)             /*!< Force PLL lock signal */
+
+#define  NRF24L01_DI_STATUS                      ((uint8_t)0x7F)             /*!< STATUS REGISTER BIT MASK */
+#define  NRF24L01_DI_STATUS_TX_FULL              ((uint8_t)0x01)             /*!< TX FIFO full flag - 0: Available locations, 1: Full */
+#define  NRF24L01_DI_STATUS_RX_P_NO_0            ((uint8_t)0x02)             /*!< RX payload number bit 0 */
+#define  NRF24L01_DI_STATUS_RX_P_NO_1            ((uint8_t)0x04)             /*!< RX payload number bit 1 */
+#define  NRF24L01_DI_STATUS_RX_P_NO_2            ((uint8_t)0x08)             /*!< RX payload number bit 2 */
+#define  NRF24L01_DI_STATUS_MAX_RT               ((uint8_t)0x10)             /*!< Maximum number of TX retransmits interrupt */
+#define  NRF24L01_DI_STATUS_TX_DS                ((uint8_t)0x20)             /*!< Data Sent TX FIFO interrupt */
+#define  NRF24L01_DI_STATUS_RX_DR                ((uint8_t)0x40)             /*!< Data Ready RX FIFO interrupt */
+
+#define  NRF24L01_DI_OBSERVE_TX                  ((uint8_t)0xFF)             /*!< TRANSMIT OBSERVE REGISTER BIT MASK */
+#define  NRF24L01_DI_ARC_CNT_0                   ((uint8_t)0x01)             /*!< Count retransmitted packets bit 0 */
+#define  NRF24L01_DI_ARC_CNT_1                   ((uint8_t)0x02)             /*!< Count retransmitted packets bit 1 */
+#define  NRF24L01_DI_ARC_CNT_2                   ((uint8_t)0x04)             /*!< Count retransmitted packets bit 2 */
+#define  NRF24L01_DI_ARC_CNT_3                   ((uint8_t)0x08)             /*!< Count retransmitted packets bit 3 */
+#define  NRF24L01_DI_PLOS_CNT_0                  ((uint8_t)0x10)             /*!< Count lost packets bit 0 */
+#define  NRF24L01_DI_PLOS_CNT_1                  ((uint8_t)0x20)             /*!< Count lost packets bit 1 */
+#define  NRF24L01_DI_PLOS_CNT_2                  ((uint8_t)0x40)             /*!< Count lost packets bit 2 */
+#define  NRF24L01_DI_PLOS_CNT_3                  ((uint8_t)0x80)             /*!< Count lost packets bit 3 */
+
+#define  NRF24L01_DI_CD                          ((uint8_t)0x01)             /*!< REGISTER BIT MASK */
+#define  NRF24L01_DI_CARRIER_DETECT              ((uint8_t)0x01)             /*!< Carrier detect */
+
+#define  NRF24L01_DI_RX_PW_P0                    ((uint8_t)0x3F)             /*!< RX PAYLOAD WIDTH FOR PIPE 0 REGISTER BIT MASK */
+#define  NRF24L01_DI_RX_PW_P0_0                  ((uint8_t)0x01)             /*!< Bit 0 */
+#define  NRF24L01_DI_RX_PW_P0_1                  ((uint8_t)0x02)             /*!< Bit 1 */
+#define  NRF24L01_DI_RX_PW_P0_2                  ((uint8_t)0x04)             /*!< Bit 2 */
+#define  NRF24L01_DI_RX_PW_P0_3                  ((uint8_t)0x08)             /*!< Bit 3 */
+#define  NRF24L01_DI_RX_PW_P0_4                  ((uint8_t)0x10)             /*!< Bit 4 */
+#define  NRF24L01_DI_RX_PW_P0_5                  ((uint8_t)0x20)             /*!< Bit 5 */
+
+#define  NRF24L01_DI_RX_PW_P1                    ((uint8_t)0x3F)             /*!< RX PAYLOAD WIDTH FOR PIPE 1 REGISTER BIT MASK */
+#define  NRF24L01_DI_RX_PW_P1_0                  ((uint8_t)0x01)             /*!< Bit 0 */
+#define  NRF24L01_DI_RX_PW_P1_1                  ((uint8_t)0x02)             /*!< Bit 1 */
+#define  NRF24L01_DI_RX_PW_P1_2                  ((uint8_t)0x04)             /*!< Bit 2 */
+#define  NRF24L01_DI_RX_PW_P1_3                  ((uint8_t)0x08)             /*!< Bit 3 */
+#define  NRF24L01_DI_RX_PW_P1_4                  ((uint8_t)0x10)             /*!< Bit 4 */
+#define  NRF24L01_DI_RX_PW_P1_5                  ((uint8_t)0x20)             /*!< Bit 5 */
+
+#define  NRF24L01_DI_RX_PW_P2                    ((uint8_t)0x3F)             /*!< RX PAYLOAD WIDTH FOR PIPE 2 REGISTER BIT MASK */
+#define  NRF24L01_DI_RX_PW_P2_0                  ((uint8_t)0x01)             /*!< Bit 0 */
+#define  NRF24L01_DI_RX_PW_P2_1                  ((uint8_t)0x02)             /*!< Bit 1 */
+#define  NRF24L01_DI_RX_PW_P2_2                  ((uint8_t)0x04)             /*!< Bit 2 */
+#define  NRF24L01_DI_RX_PW_P2_3                  ((uint8_t)0x08)             /*!< Bit 3 */
+#define  NRF24L01_DI_RX_PW_P2_4                  ((uint8_t)0x10)             /*!< Bit 4 */
+#define  NRF24L01_DI_RX_PW_P2_5                  ((uint8_t)0x20)             /*!< Bit 5 */
+
+#define  NRF24L01_DI_RX_PW_P3                    ((uint8_t)0x3F)             /*!< RX PAYLOAD WIDTH FOR PIPE 3 REGISTER BIT MASK */
+#define  NRF24L01_DI_RX_PW_P3_0                  ((uint8_t)0x01)             /*!< Bit 0 */
+#define  NRF24L01_DI_RX_PW_P3_1                  ((uint8_t)0x02)             /*!< Bit 1 */
+#define  NRF24L01_DI_RX_PW_P3_2                  ((uint8_t)0x04)             /*!< Bit 2 */
+#define  NRF24L01_DI_RX_PW_P3_3                  ((uint8_t)0x08)             /*!< Bit 3 */
+#define  NRF24L01_DI_RX_PW_P3_4                  ((uint8_t)0x10)             /*!< Bit 4 */
+#define  NRF24L01_DI_RX_PW_P3_5                  ((uint8_t)0x20)             /*!< Bit 5 */
+
+#define  NRF24L01_DI_RX_PW_P4                    ((uint8_t)0x3F)             /*!< RX PAYLOAD WIDTH FOR PIPE 4 REGISTER BIT MASK */
+#define  NRF24L01_DI_RX_PW_P4_0                  ((uint8_t)0x01)             /*!< Bit 0 */
+#define  NRF24L01_DI_RX_PW_P4_1                  ((uint8_t)0x02)             /*!< Bit 1 */
+#define  NRF24L01_DI_RX_PW_P4_2                  ((uint8_t)0x04)             /*!< Bit 2 */
+#define  NRF24L01_DI_RX_PW_P4_3                  ((uint8_t)0x08)             /*!< Bit 3 */
+#define  NRF24L01_DI_RX_PW_P4_4                  ((uint8_t)0x10)             /*!< Bit 4 */
+#define  NRF24L01_DI_RX_PW_P4_5                  ((uint8_t)0x20)             /*!< Bit 5 */
+
+#define  NRF24L01_DI_RX_PW_P5                    ((uint8_t)0x3F)             /*!< RX PAYLOAD WIDTH FOR PIPE 5 REGISTER BIT MASK */
+#define  NRF24L01_DI_RX_PW_P5_0                  ((uint8_t)0x01)             /*!< Bit 0 */
+#define  NRF24L01_DI_RX_PW_P5_1                  ((uint8_t)0x02)             /*!< Bit 1 */
+#define  NRF24L01_DI_RX_PW_P5_2                  ((uint8_t)0x04)             /*!< Bit 2 */
+#define  NRF24L01_DI_RX_PW_P5_3                  ((uint8_t)0x08)             /*!< Bit 3 */
+#define  NRF24L01_DI_RX_PW_P5_4                  ((uint8_t)0x10)             /*!< Bit 4 */
+#define  NRF24L01_DI_RX_PW_P5_5                  ((uint8_t)0x20)             /*!< Bit 5 */
+
+#define  NRF24L01_DI_FIFO_STATUS                 ((uint8_t)0x73)             /*!< FIFO STATUS REGISTER BIT MASK*/
+#define  NRF24L01_DI_FIFO_STATUS_RX_EMPTY        ((uint8_t)0x01)             /*!< RX FIFO empty flag - 0:Data in RX FIFO, 1:RX FIFO empty */
+#define  NRF24L01_DI_FIFO_STATUS_RX_FULL         ((uint8_t)0x02)             /*!< RX FIFO full flag - 0:Available locations in RX FIFO, 1:RX FIFO empty */
+#define  NRF24L01_DI_FIFO_STATUS_TX_EMPTY        ((uint8_t)0x10)             /*!< TX FIFO empty flag - 0:Data in TX FIFO, 1:TX FIFO empty */
+#define  NRF24L01_DI_FIFO_STATUS_TX_FULL         ((uint8_t)0x20)             /*!< TX FIFO full flag - 0:Available locations in TX FIFO, 1:TX FIFO empty */
+#define  NRF24L01_DI_FIFO_STATUS_TX_REUSE        ((uint8_t)0x40)             /*!< Reuse last transmitted data packet if set high */
+
+#define  NRF24L01_DI_DYNPD                       ((uint8_t)0x3F)             /*!< ENABLE DYNAMIC PAYLOAD LENGHT REGISTER BIT MASK */
+#define  NRF24L01_DI_DYNPD_DPL_P0                ((uint8_t)0x01)             /*!< Enable dyn. payload length data pipe 0 */
+#define  NRF24L01_DI_DYNPD_DPL_P1                ((uint8_t)0x02)             /*!< Enable dyn. payload length data pipe 1 */
+#define  NRF24L01_DI_DYNPD_DPL_P2                ((uint8_t)0x04)             /*!< Enable dyn. payload length data pipe 2 */
+#define  NRF24L01_DI_DYNPD_DPL_P3                ((uint8_t)0x08)             /*!< Enable dyn. payload length data pipe 3 */
+#define  NRF24L01_DI_DYNPD_DPL_P4                ((uint8_t)0x10)             /*!< Enable dyn. payload length data pipe 4 */
+#define  NRF24L01_DI_DYNPD_DPL_P5                ((uint8_t)0x20)             /*!< Enable dyn. payload length data pipe 5 */
+
+#define  NRF24L01_DI_FEATURE                     ((uint8_t)0x07)             /*!< FEATURE REGISTER REGISTER BIT MASK */
+#define  NRF24L01_DI_FEATURE_EN_DYN_ACK          ((uint8_t)0x01)             /*!< Enables the W_TX_PAYLOAD_NOACK command */
+#define  NRF24L01_DI_FEATURE_EN_ACK_PAY          ((uint8_t)0x02)             /*!< Enables Payload with ACK */
+#define  NRF24L01_DI_FEATURE_EN_DPL              ((uint8_t)0x04)             /*!< Enables Dynamic Payload Length */
+/** @} */
+
+/*===========================================================================*/
+/* Driver pre-compile time settings.                                         */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* Derived constants and error checks.                                       */
+/*===========================================================================*/
+
+#if !(HAL_USE_SPI)
+#error "RF_NRF24L01 requires HAL_USE_SPI."
+#endif
+
+#if !(HAL_USE_EXT)
+#error "RF_NRF24L01 requires HAL_USE_EXT."
+#endif
+/*===========================================================================*/
+/* Driver data structures and types.                                         */
+/*===========================================================================*/
+
+/**
+ * @name    RF Transceiver data structures and types
+ * @{
+ */
+
+/**
+ * @brief   RF Transceiver RX/TX Address field width
+ */
+typedef enum {
+
+  NRF24L01_AW_3_bytes = 0x01,               /*!< 3 bytes width */
+  NRF24L01_AW_4_bytes = 0x02,               /*!< 4 bytes width */
+  NRF24L01_AW_5_bytes = 0x03                /*!< 5 bytes width */
+} NRF24L01_AW_t;
+
+/**
+ * @brief   RF Transceiver Auto Retransmit Delay
+ */
+typedef enum {
+
+  NRF24L01_ARD_250us =  0x00,               /*!< Wait 250us */
+  NRF24L01_ARD_500us =  0x10,               /*!< Wait 500us */
+  NRF24L01_ARD_750us =  0x20,               /*!< Wait 750us */
+  NRF24L01_ARD_1000us = 0x30,               /*!< Wait 1000us */
+  NRF24L01_ARD_1250us = 0x40,               /*!< Wait 1250us */
+  NRF24L01_ARD_1500us = 0x50,               /*!< Wait 1500us */
+  NRF24L01_ARD_1750us = 0x60,               /*!< Wait 1750us */
+  NRF24L01_ARD_2000us = 0x70,               /*!< Wait 2000us */
+  NRF24L01_ARD_2250us = 0x80,               /*!< Wait 2250us */
+  NRF24L01_ARD_2500us = 0x90,               /*!< Wait 2500us */
+  NRF24L01_ARD_2750us = 0xA0,               /*!< Wait 2750us */
+  NRF24L01_ARD_3000us = 0xB0,               /*!< Wait 3000us */
+  NRF24L01_ARD_3250us = 0xC0,               /*!< Wait 3250us */
+  NRF24L01_ARD_3500us = 0xD0,               /*!< Wait 3500us */
+  NRF24L01_ARD_3750us = 0xE0,               /*!< Wait 3750us */
+  NRF24L01_ARD_4000us = 0xF0                /*!< Wait 4000us */
+} NRF24L01_ARD_t;
+
+/**
+ * @brief   RF Transceiver Auto Retransmit Count
+ */
+typedef enum {
+
+  NRF24L01_ARC_disabled = 0x00,             /*!< Re-Transmit disabled */
+  NRF24L01_ARC_1_time   = 0x01,             /*!< Up to 1 Re-Transmit on fail of AA */
+  NRF24L01_ARC_2_times  = 0x02,             /*!< Up to 2 Re-Transmit on fail of AA */
+  NRF24L01_ARC_3_times  = 0x03,             /*!< Up to 3 Re-Transmit on fail of AA */
+  NRF24L01_ARC_4_times  = 0x04,             /*!< Up to 4 Re-Transmit on fail of AA */
+  NRF24L01_ARC_5_times  = 0x05,             /*!< Up to 5 Re-Transmit on fail of AAs */
+  NRF24L01_ARC_6_times  = 0x06,             /*!< Up to 6 Re-Transmit on fail of AA */
+  NRF24L01_ARC_7_times  = 0x07,             /*!< Up to 7 Re-Transmit on fail of AA */
+  NRF24L01_ARC_8_times  = 0x08,             /*!< Up to 8 Re-Transmit on fail of AA */
+  NRF24L01_ARC_9_times  = 0x09,             /*!< Up to 9 Re-Transmit on fail of AA */
+  NRF24L01_ARC_10_times = 0x0A,             /*!< Up to 10 Re-Transmit on fail of AA */
+  NRF24L01_ARC_11_times = 0x0B,             /*!< Up to 11 Re-Transmit on fail of AA */
+  NRF24L01_ARC_12_times = 0x0C,             /*!< Up to 12 Re-Transmit on fail of AA */
+  NRF24L01_ARC_13_times = 0x0D,             /*!< Up to 13 Re-Transmit on fail of AA */
+  NRF24L01_ARC_14_times = 0x0E,             /*!< Up to 14 Re-Transmit on fail of AA */
+  NRF24L01_ARC_15_times = 0x0F              /*!< Up to 15 Re-Transmit on fail of AA */
+} NRF24L01_ARC_t;
+
+
+/**
+ * @brief   RF Transceiver configuration typedef.
+ *
+ * @detail  This will select frequency channel beetween 2,4 GHz and 2,525 GHz
+ * @detail  according to formula 2,4GHz + RF_CH[MHz]. This value must be included
+ * @detail  between 0 and 125.
+ */
+typedef uint8_t NRF24L01_RF_CH_t;
+
+/**
+ * @brief   RF Transceiver Air Data Rate
+ */
+typedef enum {
+
+  NRF24L01_ADR_1Mbps = 0x00,             /*!< Air data rate 1 Mbps */
+  NRF24L01_ADR_2Mbps = 0x08              /*!< Air data rate 2 Mbps */
+} NRF24L01_ADR_t;
+
+/**
+ * @brief   RF Transceiver Output Power
+ */
+typedef enum {
+
+  NRF24L01_PWR_0dBm     = 0x06,          /*!< RF output power 0 dBm */
+  NRF24L01_PWR_neg6dBm  = 0x04,          /*!< RF output power -6 dBm */
+  NRF24L01_PWR_neg12dBm = 0x02,          /*!< RF output power -12 dBm */
+  NRF24L01_PWR_neg18dBm = 0x00           /*!< RF output power -18 dBm */
+} NRF24L01_PWR_t;
+
+/**
+ * @brief   RF Transceiver Low Noise Amplifier
+ *
+ * @details Reduce current consumption in RX mode with 0.8 mA at cost of 1.5dB
+ *          reduction in receiver sensitivity.
+ */
+typedef  enum {
+  NRF24L01_LNA_enabled =  0x01,          /*!< LNA_CURR enabled */
+  NRF24L01_LNA_disabled = 0x00           /*!< LNA_CURR disabled */
+} NRF24L01_LNA_t;
+
+/**
+ * @brief   RF Transceiver Backward Compatibility
+ *
+ * @details This type specifies if trasmission must be compatible to receive
+ *          from an nRF2401/nRF2402/nRF24E1/nRF24E.
+ */
+typedef  bool_t                   NRF24L01_bckwrdcmp_t;
+
+#if NRF24L01_USE_FEATURE || defined(__doxigen__)
+/**
+ * @brief   RF Transceiver Dynamic Payload enabler
+ *
+ * @details Enables Dynamic Payload Length
+ */
+typedef  enum {
+  NRF24L01_DPL_enabled =  0x04,          /*!< EN_DPL enabled */
+  NRF24L01_DPL_disabled = 0x00           /*!< EN_DPL disabled */
+} NRF24L01_DPL_t;
+
+/**
+ * @brief   RF Transceiver Dynamic Acknowledge with Payload enabler
+ *
+ * @details Enables Payload with ACK
+ */
+typedef  enum {
+  NRF24L01_ACK_PAY_enabled =  0x02,          /*!< EN_ACK_PAY enabled */
+  NRF24L01_ACK_PAY_disabled = 0x00           /*!< EN_ACK_PAY disabled */
+} NRF24L01_ACK_PAY_t;
+
+/**
+ * @brief   RF Transceiver Dynamic Acknowledge enabler
+ *
+ * @details Enables the W_TX_PAYLOAD_NOACK command
+ */
+typedef  enum {
+  NRF24L01_DYN_ACK_enabled =  0x01,          /*!< EN_DYN_ACK enabled */
+  NRF24L01_DYN_ACK_disabled = 0x00           /*!< EN_DYN_ACK disabled */
+} NRF24L01_DYN_ACK_t;
+#endif /* NRF24L01_USE_FEATURE */
+
+/**
+ * @brief   RF Transceiver configuration structure.
+ */
+typedef struct {
+
+  /**
+   * @brief The chip enable line port.
+   */
+  ioportid_t                ceport;
+  /**
+   * @brief The chip enable line pad number.
+   */
+  uint16_t                  cepad;
+  /**
+   * @brief The interrupt line port.
+   */
+  ioportid_t                irqport;
+  /**
+   * @brief The interrupt line pad number.
+   */
+  uint16_t                  irqpad;
+  /**
+   * @brief Pointer to the SPI driver associated to this RF.
+   */
+  SPIDriver                 *spip;
+  /**
+   * @brief Pointer to the SPI configuration .
+   */
+  const SPIConfig           *spicfg;
+  /**
+   * @brief Pointer to the EXT driver associated to this RF.
+   */
+  EXTDriver                 *extp;
+  /**
+   * @brief EXT configuration.
+   */
+  EXTConfig                 *extcfg;
+  /**
+   * @brief RF Transceiver auto retransmit count.
+   */
+  NRF24L01_ARC_t            auto_retr_count;
+  /**
+   * @brief RF Transceiver auto retransmit delay.
+   */
+  NRF24L01_ARD_t            auto_retr_delay;
+  /**
+   * @brief RF Transceiver address width.
+   */
+  NRF24L01_AW_t             address_width;
+  /**
+   * @brief RF Transceiver channel frequency.
+   */
+  NRF24L01_RF_CH_t          channel_freq;
+  /**
+   * @brief RF Transceiver air data rate.
+   */
+  NRF24L01_ADR_t            data_rate;
+  /**
+   * @brief RF Transceiver output power.
+   */
+  NRF24L01_PWR_t            out_pwr;
+  /**
+   * @brief   RF Transceiver Low Noise Amplifier
+   */
+  NRF24L01_LNA_t            lna;
+#if NRF24L01_USE_FEATURE || defined(__doxigen__)
+  /**
+  * @brief    RF Transceiver Dynamic Payload enabler
+  */
+  NRF24L01_DPL_t            en_dpl;
+
+  /**
+   * @brief   RF Transceiver Dynamic Acknowledge with Payload enabler
+   */
+  NRF24L01_ACK_PAY_t        en_ack_pay;
+
+  /**
+   * @brief   RF Transceiver Dynamic Acknowledge enabler
+   */
+  NRF24L01_DYN_ACK_t        en_dyn_ack;
+#endif /* NRF24L01_USE_FEATURE */
+} NRF24L01_Config;
+
+/**
+ * @brief   RF Transceiver status register value.
+ */
+typedef  uint8_t             NRF24L01_status_t;
+/** @}  */
+/*===========================================================================*/
+/* Driver macros.                                                            */
+/*===========================================================================*/
+
+/*===========================================================================*/
+/* External declarations.                                                    */
+/*===========================================================================*/
+
+/**
+ * @brief   Flushes FIFOs and resets all Status flags.
+ *
+ * @pre     The SPI interface must be initialized and the driver started.
+ *
+ * @param[in] spip      pointer to the SPI interface
+ *
+ * @return              the status register value
+ */
+#define  nrf24l01Reset(spip) {                                               \
+                                                                             \
+  nrf24l01WriteRegister(spip, NRF24L01_AD_STATUS,                            \
+                                NRF24L01_DI_STATUS_MAX_RT |                  \
+                                NRF24L01_DI_STATUS_RX_DR |                   \
+                                NRF24L01_DI_STATUS_TX_DS);                   \
+}
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+NRF24L01_status_t nrf24l01GetStatus(SPIDriver *spip);
+NRF24L01_status_t nrf24l01ReadRegister(SPIDriver *spip, uint8_t reg,
+                                       uint8_t* pvalue);
+NRF24L01_status_t nrf24l01WriteRegister(SPIDriver *spip, uint8_t reg,
+                                        uint8_t value);
+NRF24L01_status_t nrf24l01WriteAddress(SPIDriver *spip, uint8_t reg,
+                                       uint8_t *pvalue, uint8_t addlen);
+NRF24L01_status_t nrf24l01GetRxPl(SPIDriver *spip, uint8_t paylen,
+                                  uint8_t* rxbuf);
+NRF24L01_status_t nrf24l01WriteTxPl(SPIDriver *spip, uint8_t paylen,
+                                    uint8_t* txbuf);
+NRF24L01_status_t nrf24l01FlushTx(SPIDriver *spip);
+NRF24L01_status_t nrf24l01FlushRx(SPIDriver *spip);
+#if NRF24L01_USE_FEATURE || defined(__DOXYGEN__)
+NRF24L01_status_t nrf24l01Activate(SPIDriver *spip);
+NRF24L01_status_t nrf24l01ReadRxPlWid(SPIDriver *spip, uint8_t* ppaylen);
+NRF24L01_status_t nrf24l01WriteAckPl(SPIDriver *spip, uint8_t ppp, uint8_t paylen,
+                                     uint8_t* payload);
+NRF24L01_status_t nrf24l01WriteTxPlNoAck(SPIDriver *spip, uint8_t paylen,
+                                         uint8_t* txbuf);
+#endif /* NRF24L01_USE_FEATURE */
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _NRF24L01_H_ */
+
+/** @} */
diff --git a/lib/chibios-contrib/os/various/devices_lib/rf/nrf52_radio.c b/lib/chibios-contrib/os/various/devices_lib/rf/nrf52_radio.c
new file mode 100644
index 000000000..9829935e7
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/rf/nrf52_radio.c
@@ -0,0 +1,1114 @@
+/* Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
+ *
+ * The information contained herein is property of Nordic Semiconductor ASA.
+ * Terms and conditions of usage are described in detail in NORDIC
+ * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
+ *
+ * Licensees are granted free, non-transferable use of the information. NO
+ * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
+ * the file.
+ *
+ * Enhanced ShockBurst proprietary protocol to ChibiOS port
+ *
+ *  ported on: 25/10/2018, by andru
+ *
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include "ch.h"
+#include "hal.h"
+
+#include "nrf52_radio.h"
+
+
+#define BIT_MASK_UINT_8(x)                                        (0xFF >> (8 - (x)))
+#define NRF52_PIPE_COUNT                                          9
+
+#define RADIO_SHORTS_COMMON ( RADIO_SHORTS_READY_START_Msk | RADIO_SHORTS_END_DISABLE_Msk | \
+            RADIO_SHORTS_ADDRESS_RSSISTART_Msk | RADIO_SHORTS_DISABLED_RSSISTOP_Msk )
+
+// Constant parameters
+#define RX_WAIT_FOR_ACK_TIMEOUT_US_2MBPS      (48)        /**< 2MBit RX wait for ack timeout value. Smallest reliable value - 43 */
+#define RX_WAIT_FOR_ACK_TIMEOUT_US_1MBPS      (64)        /**< 1MBit RX wait for ack timeout value. Smallest reliable value - 59 */
+
+#define NRF52_ADDR_UPDATE_MASK_BASE0          (1 << 0)    /*< Mask value to signal updating BASE0 radio address. */
+#define NRF52_ADDR_UPDATE_MASK_BASE1          (1 << 1)    /*< Mask value to signal updating BASE1 radio address. */
+#define NRF52_ADDR_UPDATE_MASK_PREFIX         (1 << 2)    /*< Mask value to signal updating radio prefixes */
+
+#define NRF52_PID_RESET_VALUE                 0xFF        /**< Invalid PID value which is guaranteed to not collide with any valid PID value. */
+#define NRF52_PID_MAX                         3           /**< Maximum value for PID. */
+#define NRF52_CRC_RESET_VALUE                 0xFFFF      /**< CRC reset value*/
+
+#ifndef NRF52_RADIO_USE_TIMER0
+#define NRF52_RADIO_USE_TIMER0                FALSE
+#endif
+
+#ifndef NRF52_RADIO_USE_TIMER1
+#define NRF52_RADIO_USE_TIMER1                FALSE
+#endif
+
+#ifndef NRF52_RADIO_USE_TIMER2
+#define NRF52_RADIO_USE_TIMER2                FALSE
+#endif
+
+#ifndef NRF52_RADIO_USE_TIMER3
+#define NRF52_RADIO_USE_TIMER3                FALSE
+#endif
+
+#ifndef NRF52_RADIO_USE_TIMER4
+#define NRF52_RADIO_USE_TIMER4                FALSE
+#endif
+
+#ifndef NRF52_RADIO_IRQ_PRIORITY
+#define NRF52_RADIO_IRQ_PRIORITY                          3                   /**< RADIO interrupt priority. */
+#endif
+
+#ifndef NRF52_RADIO_PPI_TIMER_START
+#error "PPI channel NRF52_RADIO_PPI_TIMER_START need to be defined"
+#endif
+
+#ifndef NRF52_RADIO_PPI_TIMER_STOP
+#error "PPI channel NRF52_RADIO_PPI_TIMER_STOP need to be defined"
+#endif
+
+#ifndef NRF52_RADIO_PPI_RX_TIMEOUT
+#error "PPI channel NRF52_RADIO_PPI_RX_TIMEOUT need to be defined"
+#endif
+
+#ifndef NRF52_RADIO_PPI_TX_START
+#error "PPI channel NRF52_RADIO_PPI_TX_START need to be defined"
+#endif
+
+#if (NRF52_RADIO_USE_TIMER0 == FALSE) && (NRF52_RADIO_USE_TIMER1 == FALSE) && \
+        (NRF52_RADIO_USE_TIMER2 == FALSE) && (NRF52_RADIO_USE_TIMER3 == FALSE) && \
+        (NRF52_RADIO_USE_TIMER4 == FALSE)
+#error "At least one hardware TIMER must be defined"
+#endif
+
+#ifndef NRF52_RADIO_INTTHD_PRIORITY
+#error "Interrupt handle thread priority need to be defined"
+#endif
+
+#ifndef NRF52_RADIO_EVTTHD_PRIORITY
+#error "Event thread priority need to be defined"
+#endif
+
+#define VERIFY_PAYLOAD_LENGTH(p)                            \
+do                                                          \
+{                                                           \
+    if(p->length == 0 ||                                    \
+       p->length > NRF52_MAX_PAYLOAD_LENGTH ||              \
+       (RFD1.config.protocol == NRF52_PROTOCOL_ESB &&       \
+        p->length > RFD1.config.payload_length))            \
+    {                                                       \
+        return NRF52_ERROR_INVALID_LENGTH;                  \
+    }                                                       \
+}while(0)
+
+//Structure holding pipe info PID and CRC and ack payload.
+typedef struct
+{
+    uint16_t            m_crc;
+    uint8_t             m_pid;
+    uint8_t             m_ack_payload;
+} pipe_info_t;
+
+// First in first out queue of payloads to be transmitted.
+typedef struct
+{
+    nrf52_payload_t *   p_payload[NRF52_TX_FIFO_SIZE];      /**< Pointer to the actual queue. */
+    uint32_t            entry_point;                        /**< Current start of queue. */
+    uint32_t            exit_point;                         /**< Current end of queue. */
+    uint32_t            count;                              /**< Current number of elements in the queue. */
+} nrf52_payload_tx_fifo_t;
+
+// First in first out queue of received payloads.
+typedef struct
+{
+    nrf52_payload_t *   p_payload[NRF52_RX_FIFO_SIZE];      /**< Pointer to the actual queue. */
+    uint32_t            entry_point;                        /**< Current start of queue. */
+    uint32_t            exit_point;                         /**< Current end of queue. */
+    uint32_t            count;                              /**< Current number of elements in the queue. */
+} nrf52_payload_rx_fifo_t;
+
+// These function pointers are changed dynamically, depending on protocol configuration and state.
+//static void (*on_radio_end)(RFDriver *rfp) = NULL;
+static void (*set_rf_payload_format)(RFDriver *rfp, uint32_t payload_length) = NULL;
+
+// The following functions are assigned to the function pointers above.
+static void on_radio_disabled_tx_noack(RFDriver *rfp);
+static void on_radio_disabled_tx(RFDriver *rfp);
+static void on_radio_disabled_tx_wait_for_ack(RFDriver *rfp);
+static void on_radio_disabled_rx(RFDriver *rfp);
+static void on_radio_disabled_rx_ack(RFDriver *rfp);
+
+static volatile uint16_t wait_for_ack_timeout_us;
+static nrf52_payload_t * p_current_payload;
+
+// TX FIFO
+static nrf52_payload_t            tx_fifo_payload[NRF52_TX_FIFO_SIZE];
+static nrf52_payload_tx_fifo_t    tx_fifo;
+
+// RX FIFO
+static nrf52_payload_t            rx_fifo_payload[NRF52_RX_FIFO_SIZE];
+static nrf52_payload_rx_fifo_t    rx_fifo;
+
+// Payload buffers
+static uint8_t                    tx_payload_buffer[NRF52_MAX_PAYLOAD_LENGTH + 2];
+static uint8_t                    rx_payload_buffer[NRF52_MAX_PAYLOAD_LENGTH + 2];
+
+static uint8_t                    pids[NRF52_PIPE_COUNT];
+static pipe_info_t                rx_pipe_info[NRF52_PIPE_COUNT];
+
+ // disable and events semaphores.
+static binary_semaphore_t disable_sem;
+static binary_semaphore_t events_sem;
+
+RFDriver RFD1;
+
+// Function to do bytewise bit-swap on a unsigned 32 bit value
+static uint32_t bytewise_bit_swap(uint8_t const * p_inp) {
+    uint32_t inp = (*(uint32_t*)p_inp);
+
+    return __REV((uint32_t)__RBIT(inp)); //lint -esym(628, __rev) -esym(526, __rev) -esym(628, __rbit) -esym(526, __rbit) */
+}
+
+// Internal function to convert base addresses from nRF24L type addressing to nRF52 type addressing
+static uint32_t addr_conv(uint8_t const* p_addr) {
+    return __REV(bytewise_bit_swap(p_addr)); //lint -esym(628, __rev) -esym(526, __rev) */
+}
+
+static thread_t *rfEvtThread_p;
+static THD_WORKING_AREA(waRFEvtThread, 128);
+static THD_FUNCTION(rfEvtThread, arg) {
+    (void)arg;
+
+    chRegSetThreadName("rfevent");
+
+    while (!chThdShouldTerminateX()) {
+        chBSemWait(&events_sem);
+
+        nrf52_int_flags_t interrupts = RFD1.flags;
+        RFD1.flags = 0;
+
+        if (interrupts & NRF52_INT_TX_SUCCESS_MSK) {
+            chEvtBroadcastFlags(&RFD1.eventsrc, (eventflags_t) NRF52_EVENT_TX_SUCCESS);
+        }
+        if (interrupts & NRF52_INT_TX_FAILED_MSK) {
+                chEvtBroadcastFlags(&RFD1.eventsrc, (eventflags_t) NRF52_EVENT_TX_FAILED);
+        }
+        if (interrupts & NRF52_INT_RX_DR_MSK) {
+                chEvtBroadcastFlags(&RFD1.eventsrc, (eventflags_t) NRF52_EVENT_RX_RECEIVED);
+        }
+    }
+    chThdExit((msg_t) 0);
+}
+
+static thread_t *rfIntThread_p;
+static THD_WORKING_AREA(waRFIntThread, 128);
+static THD_FUNCTION(rfIntThread, arg) {
+    (void)arg;
+
+    chRegSetThreadName("rfint");
+
+    while (!chThdShouldTerminateX()) {
+        chBSemWait(&disable_sem);
+        switch (RFD1.state) {
+          case NRF52_STATE_PTX_TX:
+                  on_radio_disabled_tx_noack(&RFD1);
+                  break;
+          case NRF52_STATE_PTX_TX_ACK:
+                  on_radio_disabled_tx(&RFD1);
+                  break;
+          case NRF52_STATE_PTX_RX_ACK:
+                  on_radio_disabled_tx_wait_for_ack(&RFD1);
+                  break;
+          case NRF52_STATE_PRX:
+                  on_radio_disabled_rx(&RFD1);
+                  break;
+          case NRF52_STATE_PRX_SEND_ACK:
+                  on_radio_disabled_rx_ack(&RFD1);
+                  break;
+          default:
+                  break;
+        }
+    }
+        chThdExit((msg_t) 0);
+}
+
+static void serve_radio_interrupt(RFDriver *rfp) {
+    if ((NRF_RADIO->INTENSET & RADIO_INTENSET_READY_Msk) && NRF_RADIO->EVENTS_READY) {
+        NRF_RADIO->EVENTS_READY = 0;
+        (void) NRF_RADIO->EVENTS_READY;
+    }
+    if ((NRF_RADIO->INTENSET & RADIO_INTENSET_DISABLED_Msk) && NRF_RADIO->EVENTS_DISABLED) {
+        NRF_RADIO->EVENTS_DISABLED = 0;
+        (void) NRF_RADIO->EVENTS_DISABLED;
+        chSysLockFromISR();
+        chBSemSignalI(&disable_sem);
+        chSysUnlockFromISR();
+    }
+}
+
+/**
+ * @brief   RADIO events interrupt handler.
+ *
+ * @isr
+ */
+OSAL_IRQ_HANDLER(Vector44) {
+
+  OSAL_IRQ_PROLOGUE();
+
+  serve_radio_interrupt(&RFD1);
+
+  OSAL_IRQ_EPILOGUE();
+}
+
+static void set_rf_payload_format_esb_dpl(RFDriver *rfp, uint32_t payload_length) {
+#if (NRF52_MAX_PAYLOAD_LENGTH <= 32)
+    // Using 6 bits for length
+    NRF_RADIO->PCNF0 = (0 << RADIO_PCNF0_S0LEN_Pos) |
+                       (6 << RADIO_PCNF0_LFLEN_Pos) |
+                       (3 << RADIO_PCNF0_S1LEN_Pos) ;
+#else
+    // Using 8 bits for length
+    NRF_RADIO->PCNF0 = (0 << RADIO_PCNF0_S0LEN_Pos) |
+                       (8 << RADIO_PCNF0_LFLEN_Pos) |
+                       (3 << RADIO_PCNF0_S1LEN_Pos) ;
+#endif
+    NRF_RADIO->PCNF1 = (RADIO_PCNF1_WHITEEN_Disabled    << RADIO_PCNF1_WHITEEN_Pos) |
+                       (RADIO_PCNF1_ENDIAN_Big          << RADIO_PCNF1_ENDIAN_Pos)  |
+                       ((rfp->config.address.addr_length - 1)  << RADIO_PCNF1_BALEN_Pos)   |
+                       (0                               << RADIO_PCNF1_STATLEN_Pos) |
+                       (NRF52_MAX_PAYLOAD_LENGTH        << RADIO_PCNF1_MAXLEN_Pos);
+}
+
+static void set_rf_payload_format_esb(RFDriver *rfp, uint32_t payload_length) {
+    NRF_RADIO->PCNF0 = (1 << RADIO_PCNF0_S0LEN_Pos) |
+                       (0 << RADIO_PCNF0_LFLEN_Pos) |
+                       (1 << RADIO_PCNF0_S1LEN_Pos);
+
+    NRF_RADIO->PCNF1 = (RADIO_PCNF1_WHITEEN_Disabled    << RADIO_PCNF1_WHITEEN_Pos) |
+                       (RADIO_PCNF1_ENDIAN_Big          << RADIO_PCNF1_ENDIAN_Pos)  |
+                       ((rfp->config.address.addr_length - 1)  << RADIO_PCNF1_BALEN_Pos)   |
+                       (payload_length                  << RADIO_PCNF1_STATLEN_Pos) |
+                       (payload_length                  << RADIO_PCNF1_MAXLEN_Pos);
+}
+
+/* Set BASE0 and BASE1 addresses & prefixes registers
+ *   NRF52 { prefixes[0], base0_addr[0], base0_addr[1], base0_addr[2], base0_addr[3] } ==
+ *   NRF24 { addr[0], addr[1], addr[2], addr[3], addr[4] }
+ */
+static void set_addresses(RFDriver *rfp, uint8_t update_mask) {
+    if (update_mask & NRF52_ADDR_UPDATE_MASK_BASE0) {
+        NRF_RADIO->BASE0 = addr_conv(rfp->config.address.base_addr_p0);
+        NRF_RADIO->DAB[0] = addr_conv(rfp->config.address.base_addr_p0);
+    }
+
+    if (update_mask & NRF52_ADDR_UPDATE_MASK_BASE1) {
+        NRF_RADIO->BASE1 = addr_conv(rfp->config.address.base_addr_p1);
+        NRF_RADIO->DAB[1] = addr_conv(rfp->config.address.base_addr_p1);
+    }
+
+    if (update_mask & NRF52_ADDR_UPDATE_MASK_PREFIX) {
+        NRF_RADIO->PREFIX0 = bytewise_bit_swap(&rfp->config.address.pipe_prefixes[0]);
+        NRF_RADIO->DAP[0] = bytewise_bit_swap(&rfp->config.address.pipe_prefixes[0]);
+        NRF_RADIO->PREFIX1 = bytewise_bit_swap(&rfp->config.address.pipe_prefixes[4]);
+        NRF_RADIO->DAP[1] = bytewise_bit_swap(&rfp->config.address.pipe_prefixes[4]);
+    }
+}
+
+static void set_tx_power(RFDriver *rfp) {
+    NRF_RADIO->TXPOWER = rfp->config.tx_power << RADIO_TXPOWER_TXPOWER_Pos;
+}
+
+static void set_bitrate(RFDriver *rfp) {
+    NRF_RADIO->MODE = rfp->config.bitrate << RADIO_MODE_MODE_Pos;
+
+    switch (rfp->config.bitrate) {
+        case NRF52_BITRATE_2MBPS:
+            wait_for_ack_timeout_us = RX_WAIT_FOR_ACK_TIMEOUT_US_2MBPS;
+            break;
+        case NRF52_BITRATE_1MBPS:
+            wait_for_ack_timeout_us = RX_WAIT_FOR_ACK_TIMEOUT_US_1MBPS;
+            break;
+    }
+}
+
+static void set_protocol(RFDriver *rfp) {
+    switch (rfp->config.protocol) {
+        case NRF52_PROTOCOL_ESB_DPL:
+            set_rf_payload_format = set_rf_payload_format_esb_dpl;
+            break;
+        case NRF52_PROTOCOL_ESB:
+            set_rf_payload_format = set_rf_payload_format_esb;
+            break;
+    }
+}
+
+static void set_crc(RFDriver *rfp) {
+    NRF_RADIO->CRCCNF = rfp->config.crc << RADIO_CRCCNF_LEN_Pos;
+
+    if (rfp->config.crc == RADIO_CRCCNF_LEN_Two)
+    {
+        NRF_RADIO->CRCINIT = 0xFFFFUL;      // Initial value
+        NRF_RADIO->CRCPOLY = 0x11021UL;     // CRC poly: x^16+x^12^x^5+1
+    }
+    else if (rfp->config.crc == RADIO_CRCCNF_LEN_One)
+    {
+        NRF_RADIO->CRCINIT = 0xFFUL;        // Initial value
+        NRF_RADIO->CRCPOLY = 0x107UL;       // CRC poly: x^8+x^2^x^1+1
+    }
+}
+
+static void ppi_init(RFDriver *rfp) {
+    NRF_PPI->CH[NRF52_RADIO_PPI_TIMER_START].EEP = (uint32_t)&NRF_RADIO->EVENTS_READY;
+    NRF_PPI->CH[NRF52_RADIO_PPI_TIMER_START].TEP = (uint32_t)&rfp->timer->TASKS_START;
+
+    NRF_PPI->CH[NRF52_RADIO_PPI_TIMER_STOP].EEP  = (uint32_t)&NRF_RADIO->EVENTS_ADDRESS;
+    NRF_PPI->CH[NRF52_RADIO_PPI_TIMER_STOP].TEP  = (uint32_t)&rfp->timer->TASKS_STOP;
+
+    NRF_PPI->CH[NRF52_RADIO_PPI_RX_TIMEOUT].EEP  = (uint32_t)&rfp->timer->EVENTS_COMPARE[0];
+    NRF_PPI->CH[NRF52_RADIO_PPI_RX_TIMEOUT].TEP  = (uint32_t)&NRF_RADIO->TASKS_DISABLE;
+
+    NRF_PPI->CH[NRF52_RADIO_PPI_TX_START].EEP    = (uint32_t)&rfp->timer->EVENTS_COMPARE[1];
+    NRF_PPI->CH[NRF52_RADIO_PPI_TX_START].TEP    = (uint32_t)&NRF_RADIO->TASKS_TXEN;
+}
+
+static void set_parameters(RFDriver *rfp) {
+    set_tx_power(rfp);
+    set_bitrate(rfp);
+    set_protocol(rfp);
+    set_crc(rfp);
+    set_rf_payload_format(rfp, rfp->config.payload_length);
+}
+
+static void reset_fifo(void) {
+    tx_fifo.entry_point = 0;
+    tx_fifo.exit_point  = 0;
+    tx_fifo.count       = 0;
+
+    rx_fifo.entry_point = 0;
+    rx_fifo.exit_point  = 0;
+    rx_fifo.count       = 0;
+}
+
+static void init_fifo(void) {
+    reset_fifo();
+
+    for (int i = 0; i < NRF52_TX_FIFO_SIZE; i++) {
+        tx_fifo.p_payload[i] = &tx_fifo_payload[i];
+    }
+
+    for (int i = 0; i < NRF52_RX_FIFO_SIZE; i++) {
+        rx_fifo.p_payload[i] = &rx_fifo_payload[i];
+    }
+}
+
+static void tx_fifo_remove_last(void) {
+    if (tx_fifo.count > 0) {
+        nvicDisableVector(RADIO_IRQn);
+
+        tx_fifo.count--;
+        if (++tx_fifo.exit_point >= NRF52_TX_FIFO_SIZE) {
+            tx_fifo.exit_point = 0;
+        }
+
+        nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+    }
+}
+
+/** @brief  Function to push the content of the rx_buffer to the RX FIFO.
+ *
+ *  The module will point the register NRF_RADIO->PACKETPTR to a buffer for receiving packets.
+ *  After receiving a packet the module will call this function to copy the received data to
+ *  the RX FIFO.
+ *
+ *  @param  pipe Pipe number to set for the packet.
+ *  @param  pid  Packet ID.
+ *
+ *  @retval true   Operation successful.
+ *  @retval false  Operation failed.
+ */
+static bool rx_fifo_push_rfbuf(RFDriver *rfp, uint8_t pipe, uint8_t pid) {
+    if (rx_fifo.count < NRF52_RX_FIFO_SIZE) {
+        if (rfp->config.protocol == NRF52_PROTOCOL_ESB_DPL) {
+            if (rx_payload_buffer[0] > NRF52_MAX_PAYLOAD_LENGTH) {
+                return false;
+            }
+
+            rx_fifo.p_payload[rx_fifo.entry_point]->length = rx_payload_buffer[0];
+        }
+        else if (rfp->state == NRF52_STATE_PTX_RX_ACK) {
+            // Received packet is an acknowledgment
+            rx_fifo.p_payload[rx_fifo.entry_point]->length = 0;
+        }
+        else {
+            rx_fifo.p_payload[rx_fifo.entry_point]->length = rfp->config.payload_length;
+        }
+
+        memcpy(rx_fifo.p_payload[rx_fifo.entry_point]->data, &rx_payload_buffer[2],
+               rx_fifo.p_payload[rx_fifo.entry_point]->length);
+
+        rx_fifo.p_payload[rx_fifo.entry_point]->pipe = pipe;
+        rx_fifo.p_payload[rx_fifo.entry_point]->rssi = NRF_RADIO->RSSISAMPLE;
+        rx_fifo.p_payload[rx_fifo.entry_point]->pid = pid;
+        if (++rx_fifo.entry_point >= NRF52_RX_FIFO_SIZE) {
+            rx_fifo.entry_point = 0;
+        }
+        rx_fifo.count++;
+
+        return true;
+    }
+
+    return false;
+}
+
+static void timer_init(RFDriver *rfp) {
+    // Configure the system timer with a 1 MHz base frequency
+    rfp->timer->PRESCALER = 4;
+    rfp->timer->BITMODE   = TIMER_BITMODE_BITMODE_16Bit;
+    rfp->timer->SHORTS    = TIMER_SHORTS_COMPARE1_CLEAR_Msk | TIMER_SHORTS_COMPARE1_STOP_Msk;
+}
+
+static void start_tx_transaction(RFDriver *rfp) {
+    bool ack;
+
+    rfp->tx_attempt = 1;
+    rfp->tx_remaining = rfp->config.retransmit.count;
+
+    // Prepare the payload
+    p_current_payload = tx_fifo.p_payload[tx_fifo.exit_point];
+
+    // Handling ack if noack is set to false or if selctive auto ack is turned turned off
+    ack = !p_current_payload->noack || !rfp->config.selective_auto_ack;
+
+    switch (rfp->config.protocol) {
+        case NRF52_PROTOCOL_ESB:
+            set_rf_payload_format(rfp, p_current_payload->length);
+            tx_payload_buffer[0] = p_current_payload->pid;
+            tx_payload_buffer[1] = 0;
+            memcpy(&tx_payload_buffer[2], p_current_payload->data, p_current_payload->length);
+
+            NRF_RADIO->SHORTS   = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_RXEN_Msk;
+            NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk | RADIO_INTENSET_READY_Msk;
+
+            // Configure the retransmit counter
+            rfp->tx_remaining = rfp->config.retransmit.count;
+            rfp->state = NRF52_STATE_PTX_TX_ACK;
+            break;
+
+        case NRF52_PROTOCOL_ESB_DPL:
+            tx_payload_buffer[0] = p_current_payload->length;
+            tx_payload_buffer[1] = p_current_payload->pid << 1;
+            tx_payload_buffer[1] |= ack ? 0x00 : 0x01;
+            memcpy(&tx_payload_buffer[2], p_current_payload->data, p_current_payload->length);
+
+            if (ack) {
+                NRF_RADIO->SHORTS   = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_RXEN_Msk;
+                NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk | RADIO_INTENSET_READY_Msk;
+
+                // Configure the retransmit counter
+                rfp->tx_remaining = rfp->config.retransmit.count;
+                rfp->state = NRF52_STATE_PTX_TX_ACK;
+            }
+            else {
+                NRF_RADIO->SHORTS   = RADIO_SHORTS_COMMON;
+                NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+                rfp->state = NRF52_STATE_PTX_TX;
+            }
+            break;
+    }
+
+    NRF_RADIO->TXADDRESS    = p_current_payload->pipe;
+    NRF_RADIO->RXADDRESSES  = 1 << p_current_payload->pipe;
+
+    NRF_RADIO->FREQUENCY    = rfp->config.address.rf_channel;
+    NRF_RADIO->PACKETPTR    = (uint32_t)tx_payload_buffer;
+
+    NRF_RADIO->EVENTS_READY = 0;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void)NRF_RADIO->EVENTS_READY;
+    (void)NRF_RADIO->EVENTS_DISABLED;
+
+    nvicClearPending(RADIO_IRQn);
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    NRF_RADIO->TASKS_TXEN  = 1;
+}
+
+static void on_radio_disabled_tx_noack(RFDriver *rfp) {
+    rfp->flags |= NRF52_INT_TX_SUCCESS_MSK;
+    tx_fifo_remove_last();
+
+        chBSemSignal(&events_sem);
+
+        if (tx_fifo.count == 0) {
+        rfp->state = NRF52_STATE_IDLE;
+    }
+    else {
+        start_tx_transaction(rfp);
+    }
+}
+
+static void on_radio_disabled_tx(RFDriver *rfp) {
+    // Remove the DISABLED -> RXEN shortcut, to make sure the radio stays
+    // disabled after the RX window
+    NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON;
+
+    // Make sure the timer is started the next time the radio is ready,
+    // and that it will disable the radio automatically if no packet is
+    // received by the time defined in m_wait_for_ack_timeout_us
+    rfp->timer->CC[0]    = wait_for_ack_timeout_us + 130;
+    rfp->timer->CC[1]    = rfp->config.retransmit.delay - 130;
+    rfp->timer->TASKS_CLEAR = 1;
+    rfp->timer->EVENTS_COMPARE[0] = 0;
+    rfp->timer->EVENTS_COMPARE[1] = 0;
+    (void)rfp->timer->EVENTS_COMPARE[0];
+    (void)rfp->timer->EVENTS_COMPARE[1];
+
+    NRF_PPI->CHENSET = (1 << NRF52_RADIO_PPI_TIMER_START) |
+                       (1 << NRF52_RADIO_PPI_RX_TIMEOUT) |
+                       (1 << NRF52_RADIO_PPI_TIMER_STOP);
+    NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TX_START);
+
+    NRF_RADIO->EVENTS_END = 0;
+    (void)NRF_RADIO->EVENTS_END;
+
+    if (rfp->config.protocol == NRF52_PROTOCOL_ESB) {
+        set_rf_payload_format(rfp, 0);
+    }
+
+    NRF_RADIO->PACKETPTR = (uint32_t)rx_payload_buffer;
+    rfp->state = NRF52_STATE_PTX_RX_ACK;
+}
+
+static void on_radio_disabled_tx_wait_for_ack(RFDriver *rfp) {
+    // This marks the completion of a TX_RX sequence (TX with ACK)
+
+    // Make sure the timer will not deactivate the radio if a packet is received
+    NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TIMER_START) |
+                       (1 << NRF52_RADIO_PPI_RX_TIMEOUT)  |
+                       (1 << NRF52_RADIO_PPI_TIMER_STOP);
+
+    // If the radio has received a packet and the CRC status is OK
+    if (NRF_RADIO->EVENTS_END && NRF_RADIO->CRCSTATUS != 0) {
+        rfp->timer->TASKS_STOP = 1;
+        NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TX_START);
+        rfp->flags |= NRF52_INT_TX_SUCCESS_MSK;
+        rfp->tx_attempt++;// = rfp->config.retransmit.count - rfp->tx_remaining + 1;
+
+        tx_fifo_remove_last();
+
+        if (rfp->config.protocol != NRF52_PROTOCOL_ESB && rx_payload_buffer[0] > 0) {
+            if (rx_fifo_push_rfbuf(rfp, (uint8_t)NRF_RADIO->TXADDRESS, 0)) {
+                rfp->flags |= NRF52_INT_RX_DR_MSK;
+            }
+        }
+
+        chBSemSignal(&events_sem);
+
+        if ((tx_fifo.count == 0) || (rfp->config.tx_mode == NRF52_TXMODE_MANUAL)) {
+            rfp->state = NRF52_STATE_IDLE;
+        }
+        else {
+            start_tx_transaction(rfp);
+        }
+    }
+    else {
+        if (rfp->tx_remaining-- == 0) {
+            rfp->timer->TASKS_STOP = 1;
+            NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TX_START);
+            // All retransmits are expended, and the TX operation is suspended
+            rfp->tx_attempt = rfp->config.retransmit.count + 1;
+            rfp->flags |= NRF52_INT_TX_FAILED_MSK;
+
+            chBSemSignal(&events_sem);
+
+            rfp->state = NRF52_STATE_IDLE;
+        }
+        else {
+            // There are still have more retransmits left, TX mode should be
+            // entered again as soon as the system timer reaches CC[1].
+            NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_RXEN_Msk;
+            set_rf_payload_format(rfp, p_current_payload->length);
+            NRF_RADIO->PACKETPTR = (uint32_t)tx_payload_buffer;
+            rfp->state = NRF52_STATE_PTX_TX_ACK;
+            rfp->timer->TASKS_START = 1;
+            NRF_PPI->CHENSET = (1 << NRF52_RADIO_PPI_TX_START);
+            if (rfp->timer->EVENTS_COMPARE[1])
+                NRF_RADIO->TASKS_TXEN = 1;
+        }
+    }
+}
+
+static void clear_events_restart_rx(RFDriver *rfp) {
+    NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON;
+    set_rf_payload_format(rfp, rfp->config.payload_length);
+    NRF_RADIO->PACKETPTR = (uint32_t)rx_payload_buffer;
+
+    NRF_RADIO->INTENCLR = RADIO_INTENCLR_DISABLED_Msk;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+
+    NRF_RADIO->TASKS_DISABLE = 1;
+    while (NRF_RADIO->EVENTS_DISABLED == 0);
+
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+    NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk;
+
+    NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_TXEN_Msk;
+    NRF_RADIO->TASKS_RXEN = 1;
+}
+
+static void on_radio_disabled_rx(RFDriver *rfp) {
+    bool            ack                = false;
+    bool            retransmit_payload = false;
+    bool            send_rx_event      = true;
+    pipe_info_t *   p_pipe_info;
+
+    if (NRF_RADIO->CRCSTATUS == 0) {
+        clear_events_restart_rx(rfp);
+        return;
+    }
+
+    if(rx_fifo.count >= NRF52_RX_FIFO_SIZE) {
+        clear_events_restart_rx(rfp);
+        return;
+    }
+
+    p_pipe_info = &rx_pipe_info[NRF_RADIO->RXMATCH];
+    if (NRF_RADIO->RXCRC           == p_pipe_info->m_crc &&
+       (rx_payload_buffer[1] >> 1) == p_pipe_info->m_pid  ) {
+        retransmit_payload = true;
+        send_rx_event = false;
+    }
+
+    p_pipe_info->m_pid = rx_payload_buffer[1] >> 1;
+    p_pipe_info->m_crc = NRF_RADIO->RXCRC;
+
+    if(rfp->config.selective_auto_ack == false || ((rx_payload_buffer[1] & 0x01) == 0))
+        ack = true;
+
+    if(ack) {
+        NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_RXEN_Msk;
+
+        switch(rfp->config.protocol) {
+            case NRF52_PROTOCOL_ESB_DPL:
+                {
+                    if (tx_fifo.count > 0 &&
+                        (tx_fifo.p_payload[tx_fifo.exit_point]->pipe == NRF_RADIO->RXMATCH))
+                    {
+                        // Pipe stays in ACK with payload until TX fifo is empty
+                        // Do not report TX success on first ack payload or retransmit
+                        if (p_pipe_info->m_ack_payload != 0 && !retransmit_payload) {
+                            if(++tx_fifo.exit_point >= NRF52_TX_FIFO_SIZE) {
+                                tx_fifo.exit_point = 0;
+                            }
+
+                            tx_fifo.count--;
+
+                            // ACK payloads also require TX_DS
+                            // (page 40 of the 'nRF24LE1_Product_Specification_rev1_6.pdf').
+                            rfp->flags |= NRF52_INT_TX_SUCCESS_MSK;
+                        }
+
+                        p_pipe_info->m_ack_payload = 1;
+
+                        p_current_payload = tx_fifo.p_payload[tx_fifo.exit_point];
+
+                        set_rf_payload_format(rfp, p_current_payload->length);
+                        tx_payload_buffer[0] = p_current_payload->length;
+                        memcpy(&tx_payload_buffer[2],
+                               p_current_payload->data,
+                               p_current_payload->length);
+                    }
+                    else {
+                        p_pipe_info->m_ack_payload = 0;
+                        set_rf_payload_format(rfp, 0);
+                        tx_payload_buffer[0] = 0;
+                    }
+
+                    tx_payload_buffer[1] = rx_payload_buffer[1];
+                }
+                break;
+
+            case NRF52_PROTOCOL_ESB:
+                {
+                    set_rf_payload_format(rfp, 0);
+                    tx_payload_buffer[0] = rx_payload_buffer[0];
+                    tx_payload_buffer[1] = 0;
+                }
+                break;
+        }
+
+        rfp->state = NRF52_STATE_PRX_SEND_ACK;
+        NRF_RADIO->TXADDRESS = NRF_RADIO->RXMATCH;
+        NRF_RADIO->PACKETPTR = (uint32_t)tx_payload_buffer;
+    }
+    else {
+        clear_events_restart_rx(rfp);
+    }
+
+    if (send_rx_event) {
+        // Push the new packet to the RX buffer and trigger a received event if the operation was
+        // successful.
+        if (rx_fifo_push_rfbuf(rfp, NRF_RADIO->RXMATCH, p_pipe_info->m_pid)) {
+            rfp->flags |= NRF52_INT_RX_DR_MSK;
+            chBSemSignal(&events_sem);
+        }
+    }
+}
+
+static void on_radio_disabled_rx_ack(RFDriver *rfp) {
+    NRF_RADIO->SHORTS = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_TXEN_Msk;
+    set_rf_payload_format(rfp, rfp->config.payload_length);
+
+    NRF_RADIO->PACKETPTR = (uint32_t)rx_payload_buffer;
+
+    rfp->state = NRF52_STATE_PRX;
+}
+
+nrf52_error_t radio_disable(void) {
+        NRF_RADIO->SHORTS = 0;
+        NRF_RADIO->INTENCLR = 0xFFFFFFFF;
+
+    nvicDisableVector(RADIO_IRQn);
+
+        NRF_RADIO->EVENTS_DISABLED = 0;
+        (void) NRF_RADIO->EVENTS_DISABLED;
+
+    NRF_RADIO->TASKS_STOP = 1;
+        NRF_RADIO->TASKS_DISABLE = 1;
+
+        RFD1.state = NRF52_STATE_IDLE;
+
+    // Clear PPI
+    NRF_PPI->CHENCLR = (1 << NRF52_RADIO_PPI_TIMER_START) |
+                       (1 << NRF52_RADIO_PPI_TIMER_STOP)  |
+                       (1 << NRF52_RADIO_PPI_RX_TIMEOUT)  |
+                                           (1 << NRF52_RADIO_PPI_TX_START);
+
+    reset_fifo();
+
+    memset(rx_pipe_info, 0, sizeof(rx_pipe_info));
+    memset(pids, 0, sizeof(pids));
+
+    // Terminate interrupts handle thread
+    chThdTerminate(rfIntThread_p);
+    chBSemSignal(&disable_sem);
+    chThdWait(rfIntThread_p);
+
+    // Terminate events handle thread
+    chThdTerminate(rfEvtThread_p);
+    RFD1.flags = 0;
+    chBSemSignal(&events_sem);
+    chThdWait(rfEvtThread_p);
+
+    RFD1.state = NRF52_STATE_UNINIT;
+
+    return NRF52_SUCCESS;
+}
+
+//
+nrf52_error_t radio_init(nrf52_config_t const *config) {
+        osalDbgAssert(config != NULL,
+                "config must be defined");
+        osalDbgAssert(&config->address != NULL,
+                "address must be defined");
+        osalDbgAssert(NRF52_RADIO_IRQ_PRIORITY <= 7,
+                "wrong radio irq priority");
+
+    if (RFD1.state != NRF52_STATE_UNINIT) {
+        nrf52_error_t err = radio_disable();
+        if (err != NRF52_SUCCESS)
+            return err;
+    }
+
+    RFD1.radio = NRF_RADIO;
+        RFD1.config = *config;
+    RFD1.flags    = 0;
+
+    init_fifo();
+
+#if NRF52_RADIO_USE_TIMER0
+    RFD1.timer = NRF_TIMER0;
+#endif
+#if NRF52_RADIO_USE_TIMER1
+    RFD1.timer = NRF_TIMER1;
+#endif
+#if NRF52_RADIO_USE_TIMER2
+    RFD1.timer = NRF_TIMER2;
+#endif
+#if NRF52_RADIO_USE_TIMER3
+    RFD1.timer = NRF_TIMER3;
+#endif
+#if NRF52_RADIO_USE_TIMER4
+    RFD1.timer = NRF_TIMER4;
+#endif
+
+    set_parameters(&RFD1);
+
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_BASE0);
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_BASE1);
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_PREFIX);
+
+    ppi_init(&RFD1);
+    timer_init(&RFD1);
+
+    chBSemObjectInit(&disable_sem, TRUE);
+    chBSemObjectInit(&events_sem, TRUE);
+
+    chEvtObjectInit(&RFD1.eventsrc);
+
+    // interrupt handle thread
+    rfIntThread_p = chThdCreateStatic(waRFIntThread, sizeof(waRFIntThread),
+                NRF52_RADIO_INTTHD_PRIORITY, rfIntThread, NULL);
+
+    // events handle thread
+    rfEvtThread_p = chThdCreateStatic(waRFEvtThread, sizeof(waRFEvtThread),
+                NRF52_RADIO_EVTTHD_PRIORITY, rfEvtThread, NULL);
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    RFD1.state = NRF52_STATE_IDLE;
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_write_payload(nrf52_payload_t const * p_payload) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+        return NRF52_INVALID_STATE;
+    if(p_payload == NULL)
+        return NRF52_ERROR_NULL;
+    VERIFY_PAYLOAD_LENGTH(p_payload);
+    if (tx_fifo.count >= NRF52_TX_FIFO_SIZE)
+        return NRF52_ERROR_INVALID_LENGTH;
+
+    if (RFD1.config.mode == NRF52_MODE_PTX &&
+        p_payload->noack && !RFD1.config.selective_auto_ack )
+    {
+        return NRF52_ERROR_NOT_SUPPORTED;
+    }
+
+    nvicDisableVector(RADIO_IRQn);
+
+    memcpy(tx_fifo.p_payload[tx_fifo.entry_point], p_payload, sizeof(nrf52_payload_t));
+
+    pids[p_payload->pipe] = (pids[p_payload->pipe] + 1) % (NRF52_PID_MAX + 1);
+    tx_fifo.p_payload[tx_fifo.entry_point]->pid = pids[p_payload->pipe];
+
+    if (++tx_fifo.entry_point >= NRF52_TX_FIFO_SIZE) {
+        tx_fifo.entry_point = 0;
+    }
+
+    tx_fifo.count++;
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    if (RFD1.config.mode == NRF52_MODE_PTX &&
+        RFD1.config.tx_mode == NRF52_TXMODE_AUTO &&
+        RFD1.state == NRF52_STATE_IDLE)
+    {
+        start_tx_transaction(&RFD1);
+    }
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_read_rx_payload(nrf52_payload_t * p_payload) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+        return NRF52_INVALID_STATE;
+    if (p_payload == NULL)
+        return NRF52_ERROR_NULL;
+
+    if (rx_fifo.count == 0) {
+        return NRF52_ERROR_INVALID_LENGTH;
+    }
+
+    nvicDisableVector(RADIO_IRQn);
+
+    p_payload->length = rx_fifo.p_payload[rx_fifo.exit_point]->length;
+    p_payload->pipe   = rx_fifo.p_payload[rx_fifo.exit_point]->pipe;
+    p_payload->rssi   = rx_fifo.p_payload[rx_fifo.exit_point]->rssi;
+    p_payload->pid    = rx_fifo.p_payload[rx_fifo.exit_point]->pid;
+    memcpy(p_payload->data, rx_fifo.p_payload[rx_fifo.exit_point]->data, p_payload->length);
+
+    if (++rx_fifo.exit_point >= NRF52_RX_FIFO_SIZE) {
+        rx_fifo.exit_point = 0;
+    }
+
+    rx_fifo.count--;
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_start_tx(void) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+        return NRF52_ERROR_BUSY;
+
+    if (tx_fifo.count == 0) {
+        return NRF52_ERROR_INVALID_LENGTH;
+    }
+
+    start_tx_transaction(&RFD1);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_start_rx(void) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+        return NRF52_ERROR_BUSY;
+
+    NRF_RADIO->INTENCLR = 0xFFFFFFFF;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+
+    NRF_RADIO->SHORTS      = RADIO_SHORTS_COMMON | RADIO_SHORTS_DISABLED_TXEN_Msk;
+    NRF_RADIO->INTENSET    = RADIO_INTENSET_DISABLED_Msk;
+    RFD1.state             = NRF52_STATE_PRX;
+
+    NRF_RADIO->RXADDRESSES  = RFD1.config.address.rx_pipes;
+    NRF_RADIO->FREQUENCY    = RFD1.config.address.rf_channel;
+    NRF_RADIO->PACKETPTR    = (uint32_t)rx_payload_buffer;
+
+    nvicClearPending(RADIO_IRQn);
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    NRF_RADIO->EVENTS_ADDRESS = 0;
+    NRF_RADIO->EVENTS_PAYLOAD = 0;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_ADDRESS;
+    (void) NRF_RADIO->EVENTS_PAYLOAD;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+
+    NRF_RADIO->TASKS_RXEN  = 1;
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_stop_rx(void) {
+    if (RFD1.state != NRF52_STATE_PRX) {
+        return NRF52_INVALID_STATE;
+    }
+
+    NRF_RADIO->SHORTS = 0;
+    NRF_RADIO->INTENCLR = 0xFFFFFFFF;
+    NRF_RADIO->EVENTS_DISABLED = 0;
+    (void) NRF_RADIO->EVENTS_DISABLED;
+    NRF_RADIO->TASKS_DISABLE = 1;
+    while (NRF_RADIO->EVENTS_DISABLED == 0);
+    RFD1.state = NRF52_STATE_IDLE;
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_flush_tx(void) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+        return NRF52_INVALID_STATE;
+
+    nvicDisableVector(RADIO_IRQn);
+
+    tx_fifo.count = 0;
+    tx_fifo.entry_point = 0;
+    tx_fifo.exit_point = 0;
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_pop_tx(void) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+        return NRF52_INVALID_STATE;
+    if (tx_fifo.count == 0)
+        return NRF52_ERROR_INVALID_LENGTH;
+
+    nvicDisableVector(RADIO_IRQn);
+
+    if (++tx_fifo.entry_point >= NRF52_TX_FIFO_SIZE) {
+        tx_fifo.entry_point = 0;
+    }
+    tx_fifo.count--;
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_flush_rx(void) {
+    if (RFD1.state == NRF52_STATE_UNINIT)
+        return NRF52_INVALID_STATE;
+
+    nvicDisableVector(RADIO_IRQn);
+
+    rx_fifo.count = 0;
+    rx_fifo.entry_point = 0;
+    rx_fifo.exit_point = 0;
+
+    memset(rx_pipe_info, 0, sizeof(rx_pipe_info));
+
+    nvicEnableVector(RADIO_IRQn, NRF52_RADIO_IRQ_PRIORITY);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_set_base_address_0(uint8_t const * p_addr) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+        return NRF52_ERROR_BUSY;
+    if (p_addr == NULL)
+        return NRF52_ERROR_NULL;
+
+    memcpy(RFD1.config.address.base_addr_p0, p_addr, 4);
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_BASE0);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_set_base_address_1(uint8_t const * p_addr) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+        return NRF52_ERROR_BUSY;
+    if (p_addr == NULL)
+        return NRF52_ERROR_NULL;
+
+    memcpy(RFD1.config.address.base_addr_p1, p_addr, 4);
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_BASE1);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_set_prefixes(uint8_t const * p_prefixes, uint8_t num_pipes) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+        return NRF52_ERROR_BUSY;
+    if (p_prefixes == NULL)
+        return NRF52_ERROR_NULL;
+    if (num_pipes > 8)
+        return NRF52_ERROR_INVALID_PARAM;
+
+    memcpy(RFD1.config.address.pipe_prefixes, p_prefixes, num_pipes);
+    RFD1.config.address.num_pipes = num_pipes;
+    RFD1.config.address.rx_pipes = BIT_MASK_UINT_8(num_pipes);
+
+    set_addresses(&RFD1, NRF52_ADDR_UPDATE_MASK_PREFIX);
+
+    return NRF52_SUCCESS;
+}
+
+nrf52_error_t radio_set_prefix(uint8_t pipe, uint8_t prefix) {
+    if (RFD1.state != NRF52_STATE_IDLE)
+        return NRF52_ERROR_BUSY;
+    if (pipe > 8)
+        return NRF52_ERROR_INVALID_PARAM;
+
+    RFD1.config.address.pipe_prefixes[pipe] = prefix;
+
+    NRF_RADIO->PREFIX0 = bytewise_bit_swap(&RFD1.config.address.pipe_prefixes[0]);
+    NRF_RADIO->PREFIX1 = bytewise_bit_swap(&RFD1.config.address.pipe_prefixes[4]);
+
+    return NRF52_SUCCESS;
+}
diff --git a/lib/chibios-contrib/os/various/devices_lib/rf/nrf52_radio.h b/lib/chibios-contrib/os/various/devices_lib/rf/nrf52_radio.h
new file mode 100644
index 000000000..9cdea6fc2
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/rf/nrf52_radio.h
@@ -0,0 +1,256 @@
+/* Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
+ *
+ * The information contained herein is property of Nordic Semiconductor ASA.
+ * Terms and conditions of usage are described in detail in NORDIC
+ * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
+ *
+ * Licensees are granted free, non-transferable use of the information. NO
+ * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
+ * the file.
+ *
+ * @brief Enhanced ShockBurst (ESB) is a basic protocol supporting two-way data
+ *        packet communication including packet buffering, packet acknowledgment
+ *        and automatic retransmission of lost packets.
+ *
+ * ported on: 25/10/2018, by andru
+ *
+ */
+
+#ifndef NRF52_RADIO_H_
+#define NRF52_RADIO_H_
+
+// Hard coded parameters - change if necessary
+#ifndef NRF52_MAX_PAYLOAD_LENGTH
+#define NRF52_MAX_PAYLOAD_LENGTH            32                  /**< The max size of the payload. Valid values are 1 to 252 */
+#endif
+
+#define NRF52_CRC_RESET_VALUE                   0xFFFF              /**< CRC reset value*/
+
+#define NRF52_TX_FIFO_SIZE                  8                   /**< The size of the transmission first in first out buffer. */
+#define NRF52_RX_FIFO_SIZE                  8                   /**< The size of the reception first in first out buffer. */
+
+#define NRF52_RADIO_USE_TIMER0                  FALSE               /**< TIMER0 will be used by the module. */
+#define NRF52_RADIO_USE_TIMER1                  TRUE                /**< TIMER1 will be used by the module. */
+#define NRF52_RADIO_USE_TIMER2                  FALSE               /**< TIMER2 will be used by the module. */
+#define NRF52_RADIO_USE_TIMER3                  FALSE               /**< TIMER3 will be used by the module. */
+#define NRF52_RADIO_USE_TIMER4                  FALSE               /**< TIMER4 will be used by the module. */
+
+#define NRF52_RADIO_IRQ_PRIORITY                        3                   /**< RADIO interrupt priority. */
+#define NRF52_RADIO_INTTHD_PRIORITY         (NORMALPRIO+4)      /**< Interrupts handle thread priority. */
+#define NRF52_RADIO_EVTTHD_PRIORITY         (NORMALPRIO+3)      /**< Events handle thread priority */
+
+#define NRF52_RADIO_PPI_TIMER_START         10                  /**< The PPI channel used for timer start. */
+#define NRF52_RADIO_PPI_TIMER_STOP          11                  /**< The PPI channel used for timer stop. */
+#define NRF52_RADIO_PPI_RX_TIMEOUT          12                  /**< The PPI channel used for RX timeout. */
+#define NRF52_RADIO_PPI_TX_START            13                  /**< The PPI channel used for starting TX. */
+
+
+typedef enum {
+        NRF52_SUCCESS,                                        /* Call was successful.                  */
+        NRF52_INVALID_STATE,                                  /* Module is not initialized.            */
+        NRF52_ERROR_BUSY,                                     /* Module was not in idle state.         */
+        NRF52_ERROR_NULL,                                     /* Required parameter was NULL.          */
+        NRF52_ERROR_INVALID_PARAM,                            /* Required parameter is invalid         */
+        NRF52_ERROR_NOT_SUPPORTED,                            /* p_payload->noack was false while selective ack was not enabled. */
+        NRF52_ERROR_INVALID_LENGTH,                           /* Payload length was invalid (zero or larger than max allowed).   */
+} nrf52_error_t;
+
+// Internal radio module state.
+typedef enum {
+    NRF52_STATE_UNINIT,                                   /**< Module not initialized. */
+    NRF52_STATE_IDLE,                                     /**< Module idle. */
+    NRF52_STATE_PTX_TX,                                   /**< Module transmitting without ack. */
+    NRF52_STATE_PTX_TX_ACK,                               /**< Module transmitting with ack. */
+    NRF52_STATE_PTX_RX_ACK,                               /**< Module transmitting with ack and reception of payload with the ack response. */
+    NRF52_STATE_PRX,                                      /**< Module receiving packets without ack. */
+    NRF52_STATE_PRX_SEND_ACK,                             /**< Module transmitting ack in RX mode. */
+} nrf52_state_t;
+
+/**@brief Events to indicate the last transmission/receiving status. */
+typedef enum {
+    NRF52_EVENT_TX_SUCCESS  = 0x01,   /**< Event triggered on TX success.     */
+    NRF52_EVENT_TX_FAILED   = 0x02,   /**< Event triggered on TX failed.      */
+    NRF52_EVENT_RX_RECEIVED = 0x04,   /**< Event triggered on RX Received.    */
+} nrf52_event_t;
+
+// Interrupt flags
+typedef enum {
+        NRF52_INT_TX_SUCCESS_MSK = 0x01,  /**< The flag used to indicate a success since last event. */
+        NRF52_INT_TX_FAILED_MSK  = 0x02,  /**< The flag used to indicate a failiure since last event. */
+        NRF52_INT_RX_DR_MSK      = 0x04,  /**< The flag used to indicate a received packet since last event. */
+} nrf52_int_flags_t;
+
+/**Macro to create initializer for a TX data packet.
+ *
+ * @details This macro generates an initializer. It is more efficient
+ *          than setting the individual parameters dynamically.
+ *
+ * @param[in]   _pipe   The pipe to use for the data packet.
+ * @param[in]   ...     Comma separated list of character data to put in the TX buffer.
+ *                      Supported values are from 1 to 63 characters.
+ *
+ * @return  Initializer that sets up pipe, length and the byte array for content of the TX data.
+ */
+#define NRF52_CREATE_PAYLOAD(_pipe, ...)                                                  \
+        {.pipe = _pipe, .length = NUM_VA_ARGS(__VA_ARGS__), .data = {__VA_ARGS__}};         \
+        STATIC_ASSERT(NUM_VA_ARGS(__VA_ARGS__) > 0 && NUM_VA_ARGS(__VA_ARGS__) <= 63)
+
+/**@brief Enhanced ShockBurst protocol. */
+typedef enum {
+    NRF52_PROTOCOL_ESB,      /*< Enhanced ShockBurst with fixed payload length.                                            */
+    NRF52_PROTOCOL_ESB_DPL   /*< Enhanced ShockBurst with dynamic payload length.                                          */
+} nrf52_protocol_t;
+
+/**@brief Enhanced ShockBurst mode. */
+typedef enum {
+    NRF52_MODE_PTX,          /*< Primary transmitter mode. */
+    NRF52_MODE_PRX           /*< Primary receiver mode.    */
+} nrf52_mode_t;
+
+/**@brief Enhanced ShockBurst bitrate mode. */
+typedef enum {
+    NRF52_BITRATE_2MBPS     = RADIO_MODE_MODE_Nrf_2Mbit,      /**< 2Mbit radio mode.                                             */
+    NRF52_BITRATE_1MBPS     = RADIO_MODE_MODE_Nrf_1Mbit,      /**< 1Mbit radio mode.                                             */
+} nrf52_bitrate_t;
+
+/**@brief Enhanced ShockBurst CRC modes. */
+typedef enum {
+    NRF52_CRC_16BIT         = RADIO_CRCCNF_LEN_Two,                   /**< Use two byte CRC. */
+    NRF52_CRC_8BIT          = RADIO_CRCCNF_LEN_One,                   /**< Use one byte CRC. */
+    NRF52_CRC_OFF           = RADIO_CRCCNF_LEN_Disabled               /**< Disable CRC.      */
+} nrf52_crc_t;
+
+/**@brief Enhanced ShockBurst radio transmission power modes. */
+typedef enum {
+    NRF52_TX_POWER_4DBM     = RADIO_TXPOWER_TXPOWER_Pos4dBm,  /**< 4 dBm radio transmit power.   */
+    NRF52_TX_POWER_0DBM     = RADIO_TXPOWER_TXPOWER_0dBm,     /**< 0 dBm radio transmit power.   */
+    NRF52_TX_POWER_NEG4DBM  = RADIO_TXPOWER_TXPOWER_Neg4dBm,  /**< -4 dBm radio transmit power.  */
+    NRF52_TX_POWER_NEG8DBM  = RADIO_TXPOWER_TXPOWER_Neg8dBm,  /**< -8 dBm radio transmit power.  */
+    NRF52_TX_POWER_NEG12DBM = RADIO_TXPOWER_TXPOWER_Neg12dBm, /**< -12 dBm radio transmit power. */
+    NRF52_TX_POWER_NEG16DBM = RADIO_TXPOWER_TXPOWER_Neg16dBm, /**< -16 dBm radio transmit power. */
+    NRF52_TX_POWER_NEG20DBM = RADIO_TXPOWER_TXPOWER_Neg20dBm, /**< -20 dBm radio transmit power. */
+    NRF52_TX_POWER_NEG30DBM = RADIO_TXPOWER_TXPOWER_Neg30dBm  /**< -30 dBm radio transmit power. */
+} nrf52_tx_power_t;
+
+/**@brief Enhanced ShockBurst transmission modes. */
+typedef enum {
+    NRF52_TXMODE_AUTO,        /*< Automatic TX mode - When the TX fifo is non-empty and the radio is idle packets will be sent automatically. */
+    NRF52_TXMODE_MANUAL,      /*< Manual TX mode - Packets will not be sent until radio_start_tx() is called. Can be used to ensure consistent packet timing. */
+    NRF52_TXMODE_MANUAL_START /*< Manual start TX mode - Packets will not be sent until radio_start_tx() is called, but transmission will continue automatically until the TX fifo is empty. */
+} nrf52_tx_mode_t;
+
+/**@brief Enhanced ShockBurst addresses.
+ *
+ * @details The module is able to transmit packets with the TX address stored in tx_address.
+            The module can also receive packets from peers with up to eight different tx_addresses
+            stored in esb_addr_p0 - esb_addr_p7. esb_addr_p0 can have 5 arbitrary bytes
+            independent of the other addresses. esb_addr_p1 - esb_addr_p7 will share the
+            same four byte base address found in the last four bytes of esb_addr_p1.
+            They have an independent prefix byte found in esb_addr_p1[0] and esb_addr_p2 -
+            esb_addr_p7.
+*/
+typedef struct {
+    uint8_t base_addr_p0[4];        /**< Base address for pipe 0 encoded in big endian. */
+    uint8_t base_addr_p1[4];        /**< Base address for pipe 1-7 encoded in big endian. */
+    uint8_t pipe_prefixes[8];       /**< Address prefix for pipe P0 to P7. */
+    uint8_t num_pipes;              /**< Number of pipes available. */
+    uint8_t addr_length;            /**< Length of address including prefix */
+    uint8_t rx_pipes;                   /**< Bitfield for enabled RX pipes. */
+    uint8_t rf_channel;             /**< Which channel is to be used. Must be in range 0 and 125 to be valid. */
+} nrf52_address_t;
+
+/**@brief Enhanced ShockBurst payload.
+ *
+ * @note The payload is used both for transmission and receive with ack and payload.
+*/
+typedef struct
+{
+    uint8_t length;                              /**< Length of the packet. Should be equal or less than NRF_ESB_MAX_PAYLOAD_LENGTH. */
+    uint8_t pipe;                                /**< Pipe used for this payload. */
+    int8_t  rssi;                                /**< RSSI for received packet. */
+    uint8_t noack;                               /**< Flag indicating that this packet will not be acknowledged. */
+    uint8_t pid;                                 /**< PID assigned during communication. */
+    uint8_t data[NRF52_MAX_PAYLOAD_LENGTH];      /**< The payload data. */
+} nrf52_payload_t;
+
+/**@brief Retransmit attempts delay and counter. */
+typedef struct {
+    uint16_t              delay;                  /**< The delay between each retransmission of unacked packets. */
+    uint16_t              count;                  /**< The number of retransmissions attempts before transmission fail. */
+} nrf52_retransmit_t;
+
+/**@brief Main nrf_esb configuration struct. */
+typedef struct {
+    nrf52_protocol_t      protocol;               /**< Enhanced ShockBurst protocol. */
+    nrf52_mode_t          mode;                   /**< Enhanced ShockBurst default RX or TX mode. */
+
+    // General RF parameters
+    nrf52_bitrate_t       bitrate;                /**< Enhanced ShockBurst bitrate mode. */
+    nrf52_crc_t           crc;                    /**< Enhanced ShockBurst CRC mode. */
+    nrf52_tx_power_t      tx_power;                   /**< Enhanced ShockBurst radio transmission power mode.*/
+
+    // Control settings
+    nrf52_tx_mode_t       tx_mode;                /**< Enhanced ShockBurst transmit mode. */
+
+    bool                  selective_auto_ack;     /**< Enable or disable selective auto acknowledgement. */
+
+    nrf52_retransmit_t    retransmit;             /**< Packet retransmit parameters */
+
+    uint8_t               payload_length;         /**< Enhanced ShockBurst static payload length */
+
+    nrf52_address_t       address;                /**< Address parameters structure */
+} nrf52_config_t;
+
+typedef struct {
+  /**
+   * @brief NRF52 radio peripheral.
+   */
+  NRF_RADIO_Type          *radio;
+  /**
+   * @brief NRF52 timer peripheral.
+   */
+  NRF_TIMER_Type          *timer;
+  /**
+   * @brief Driver state.
+   */
+  nrf52_state_t           state;
+  /**
+   * @brief RF parameters.
+   */
+  nrf52_config_t          config;
+  /**
+   * @brief Interrupts flag.
+   */
+  nrf52_int_flags_t           flags;
+  /**
+   * @brief TX attempt number.
+   */
+  uint16_t                tx_attempt;
+  /**
+   * @brief TX retransmits remaining.
+   */
+  uint16_t                tx_remaining;
+  /**
+   * @brief Radio events source.
+   */
+  event_source_t eventsrc;
+} RFDriver;
+
+extern RFDriver RFD1;
+
+nrf52_error_t radio_init(nrf52_config_t const *config);
+nrf52_error_t radio_disable(void);
+nrf52_error_t radio_write_payload(nrf52_payload_t const * p_payload);
+nrf52_error_t radio_read_rx_payload(nrf52_payload_t * p_payload);
+nrf52_error_t radio_start_tx(void);
+nrf52_error_t radio_start_rx(void);
+nrf52_error_t radio_stop_rx(void);
+nrf52_error_t radio_flush_tx(void);
+nrf52_error_t radio_flush_rx(void);
+nrf52_error_t radio_pop_tx(void);
+nrf52_error_t radio_set_base_address_0(uint8_t const * p_addr);
+nrf52_error_t radio_set_base_address_1(uint8_t const * p_addr);
+nrf52_error_t radio_set_prefixes(uint8_t const * p_prefixes, uint8_t num_pipes);
+nrf52_error_t radio_set_prefix(uint8_t pipe, uint8_t prefix);
+
+#endif /* NRF52_RADIO_H_ */
diff --git a/lib/chibios-contrib/os/various/devices_lib/sensors/hdc1000.c b/lib/chibios-contrib/os/various/devices_lib/sensors/hdc1000.c
new file mode 100644
index 000000000..39e47ecb9
--- /dev/null
+++ b/lib/chibios-contrib/os/various/devices_lib/sensors/hdc1000.c
@@ -0,0 +1,265 @@
+/*
+    HDC100x for ChibiOS/RT - Copyright (C) 2016 Stephane D'Alu
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+/**
+ * @file    HDC1000.c
+ * @brief   HDC1000 interface module code.
+ *
+ * @addtogroup hdc1000
+ * @{
+ */
+
+#define I2C_HELPERS_AUTOMATIC_DRV TRUE
+
+#include "hal.h"
+#include "i2c_helpers.h"
+#include "hdc1000.h"