commit 849b383a300fdd7113a54b1d8142348677804196
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Mon Jun 15 11:45:49 2020 +0200

    Add tests for vl53l1x

diff --git a/modules/NUCLEO-L476RG/Makefile b/modules/NUCLEO-L476RG/Makefile
index 52ed971..6acfb7c 100644
--- a/modules/NUCLEO-L476RG/Makefile
+++ b/modules/NUCLEO-L476RG/Makefile
@@ -144,6 +144,7 @@ include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk
 include $(AMIROLLD)/drivers/LED/v1/alld_LED.mk
 include $(AMIROLLD)/drivers/button/v1/alld_button.mk
 include $(AMIROLLD)/drivers/MPU6050/v1/alld_MPU6050.mk
+include $(AMIROLLD)/drivers/VL53L1X/v1/alld_VL53L1X.mk
 
 # Module specific inclusion directories
 MODULE_INC = $(MODULE_DIR)
@@ -156,6 +157,8 @@ MODULE_CSRC = $(MODULE_DIR)/board.c \
 include $(MODULE_DIR)/test/button/module_test_button.mk
 include $(MODULE_DIR)/test/LED/module_test_LED.mk
 include $(MODULE_DIR)/test/MPU6050/module_test_MPU6050.mk
+include $(MODULE_DIR)/test/VL53L1X/module_test_VL53L1X.mk
+
 
 # C warning options.
 CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes
diff --git a/modules/NUCLEO-L476RG/board.h b/modules/NUCLEO-L476RG/board.h
index b3f88bd..8ea74b7 100644
--- a/modules/NUCLEO-L476RG/board.h
+++ b/modules/NUCLEO-L476RG/board.h
@@ -64,6 +64,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #define BOARD_MPU6050_CONNECTED     false
 
+#define BOARD_VL53L1X_CONNECTED     true
+
 
 /*
  * IO pins assignments.
diff --git a/modules/NUCLEO-L476RG/module.c b/modules/NUCLEO-L476RG/module.c
index 8b9dff8..d433d60 100644
--- a/modules/NUCLEO-L476RG/module.c
+++ b/modules/NUCLEO-L476RG/module.c
@@ -59,6 +59,16 @@ I2CConfig moduleHalI2c3Config = {
 
 #endif /* (BOARD_MPU6050_CONNECTED == true) */
 
+#if (BOARD_VL53L1X_CONNECTED == true)
+
+I2CConfig moduleHalI2c3Config = {
+  /* timing reg */ 0, // configured later in MODULE_INIT_PERIPHERY_IF_MPU6050() hook
+  /* CR1        */ 0,
+  /* CR2        */ 0,
+};
+
+#endif /* (BOARD_VL53L1X_CONNECTED == true) */
+
 /** @} */
 
 /*===========================================================================*/
@@ -98,6 +108,31 @@ ROMCONST apalControlGpio_t moduleGpioUserButton = {
   },
 };
 
+
+static apalGpio_t _gpioVl53l1xINT = {
+  /* line */ PAL_LINE(GPIOC, GPIOC_PIN2),
+};
+ROMCONST apalControlGpio_t moduleGpioVl53l1xINT = {
+  /* GPIO */ &_gpioVl53l1xINT,
+  /* meta */ {
+    /* direction      */ APAL_GPIO_DIRECTION_INPUT,
+    /* active state   */ APAL_GPIO_ACTIVE_LOW,
+    /* interrupt edge */ APAL_GPIO_EDGE_FALLING,
+  },
+};
+
+static apalGpio_t _gpioVl53l1xXSHUT = {
+  /* line */ PAL_LINE(GPIOC, GPIOC_PIN3),
+};
+ROMCONST apalControlGpio_t moduleGpioVl53l1xXSHUT = {
+  /* GPIO */ &_gpioVl53l1xXSHUT,
+  /* meta */ {
+    /* direction      */ APAL_GPIO_DIRECTION_OUTPUT,
+    /* active state   */ APAL_GPIO_ACTIVE_HIGH,
+    /* interrupt edge */ APAL_GPIO_EDGE_NONE,
+  },
+};
+
 /** @} */
 
 /*===========================================================================*/
@@ -137,6 +172,17 @@ ButtonDriver moduleLldUserButton = {
   /* Button Gpio  */ &moduleGpioUserButton,
 };
 
+
+#if (BOARD_VL53L1X_CONNECTED == true)
+VL53L1XDriver moduleLldVl53l1x = {
+   /* I2C Driver       */ &MODULE_HAL_I2C3,
+  /* I²C address      */ VL53L1X_LLD_I2C_ADDR_DEFAULT,
+  /* GPIO 1:INT pin */ &moduleGpioVl53l1xINT,
+   /* Xshut  */        &moduleGpioVl53l1xXSHUT,
+};
+
+#endif /* (BOARD_VL53L1X_CONNECTED == true) */
+
 #if (BOARD_MPU6050_CONNECTED == true)
 
 MPU6050Driver moduleLldMpu6050 = {
@@ -144,6 +190,8 @@ MPU6050Driver moduleLldMpu6050 = {
   /* I²C address      */ MPU6050_LLD_I2C_ADDR_FIXED,
 };
 
+
+
 #endif /* (BOARD_MPU6050_CONNECTED == true) */
 
 /** @} */
@@ -167,6 +215,22 @@ static int _testLedShellCmdCb(BaseSequentialStream* stream, int argc, char* argv
 AOS_SHELL_COMMAND(moduleTestLedShellCmd, "test:LED", _testLedShellCmdCb);
 
 /*
+ * LED
+ */
+#include <module_test_VL53L1X.h>
+static int _testShellVL53L1XCmdCb(BaseSequentialStream* stream, int argc, char* argv[])
+{
+  return moduleTestVL53L1XShellCb(stream, argc, argv, NULL);
+}
+AOS_SHELL_COMMAND(moduleTestVL53L1XShellCmd, "test:VL53L1X", _testShellVL53L1XCmdCb);
+
+static int _testShellVL53L1XInitCmdCb(BaseSequentialStream* stream, int argc, char* argv[])
+{
+  return moduleTestVL53L1XInitShellCb(stream, argc, argv, NULL);
+}
+AOS_SHELL_COMMAND(moduleTestVL53L1XInitShellCmd, "test:VL53L1X:INIT", _testShellVL53L1XInitCmdCb);
+
+/*
  * User button
  */
 #include <module_test_button.h>
diff --git a/modules/NUCLEO-L476RG/module.h b/modules/NUCLEO-L476RG/module.h
index 86dbcfa..df2ca53 100644
--- a/modules/NUCLEO-L476RG/module.h
+++ b/modules/NUCLEO-L476RG/module.h
@@ -60,7 +60,7 @@ extern SerialConfig moduleHalProgIfConfig;
  */
 #define MODULE_HAL_RTC                          RTCD1
 
-#if (BOARD_MPU6050_CONNECTED == true) || defined(__DOXYGEN__)
+#if (BOARD_MPU6050_CONNECTED == true || BOARD_VL53L1X_CONNECTED == true) || defined(__DOXYGEN__)
 
 /**
  * @brief   I2C driver to access multiplexer, proximity sensors 5 to 8, power monitors for VSYS4.2, VIO 5.0 and VDD, EEPROM, touch sensor, and fuel gauge (front battery).
@@ -72,7 +72,7 @@ extern SerialConfig moduleHalProgIfConfig;
  */
 extern I2CConfig moduleHalI2c3Config;
 
-#endif /* (BOARD_MPU6050_CONNECTED == true) */
+#endif /* (BOARD_MPU6050_CONNECTED == true || BOARD_VL53L1X_CONNECTED == true) */
 
 /** @} */
 
@@ -92,6 +92,8 @@ extern ROMCONST apalControlGpio_t moduleGpioLed;
  * @brief   User button input signal.
  */
 extern ROMCONST apalControlGpio_t moduleGpioUserButton;
+extern ROMCONST apalControlGpio_t moduleGpioVl53l1xINT;
+extern ROMCONST apalControlGpio_t moduleGpioVl53l1xXSHUT;
 
 /** @} */
 
@@ -102,6 +104,8 @@ extern ROMCONST apalControlGpio_t moduleGpioUserButton;
  */
 /*===========================================================================*/
 
+#include <math.h>
+
 /**
  * @brief   Event flag to be set on a USER_BUTTON interrupt.
  */
@@ -121,8 +125,19 @@ extern ROMCONST char* moduleShellPrompt;
   /* user button */                                                           \
   palSetLineCallback(moduleGpioUserButton.gpio->line, aosSysGetStdGpioCallback(), &moduleGpioUserButton.gpio->line);  \
   palEnableLineEvent(moduleGpioUserButton.gpio->line, APAL2CH_EDGE(moduleGpioUserButton.meta.edge));                  \
+  MODULE_INIT_INTERRUPTS_VL53L1X();                                           \
 }
 
+
+#if (BOARD_VL53L1X_CONNECTED == true)
+  #define MODULE_INIT_INTERRUPTS_VL53L1X() {                                  \
+    palSetLineCallback(moduleGpioVl53l1xINT.gpio->line, aosSysGetStdGpioCallback(), &moduleGpioVl53l1xINT.gpio->line);  \
+    palEnableLineEvent(moduleGpioVl53l1xINT.gpio->line, APAL2CH_EDGE(moduleGpioVl53l1xINT.meta.edge));                  \
+  }
+#else /* (BOARD_VL53L1X_CONNECTED == true) */
+  #define MODULE_INIT_INTERRUPTS_VL53L1X() {}
+#endif /* (BOARD_VL53L1X_CONNECTED == true) */
+
 /**
  * @brief   Test initialization hook.
  */
@@ -131,6 +146,7 @@ extern ROMCONST char* moduleShellPrompt;
   aosShellAddCommand(&aos.shell, &moduleTestLedShellCmd);                     \
   aosShellAddCommand(&aos.shell, &moduleTestButtonShellCmd);                  \
   MODULE_INIT_TEST_MPU6050();                                                 \
+  MODULE_INIT_TEST_VL53L1X();                                                 \
   aosShellAddCommand(&aos.shell, &moduleTestAllShellCmd);                     \
 }
 #if (BOARD_MPU6050_CONNECTED == true)
@@ -141,6 +157,15 @@ extern ROMCONST char* moduleShellPrompt;
   #define MODULE_INIT_TEST_MPU6050() {}
 #endif /* (BOARD_MPU6050_CONNECTED == true) */
 
+#if (BOARD_VL53L1X_CONNECTED == true)
+  #define MODULE_INIT_TEST_VL53L1X() {                                        \
+    aosShellAddCommand(&aos.shell, &moduleTestVL53L1XShellCmd);               \
+    aosShellAddCommand(&aos.shell, &moduleTestVL53L1XInitShellCmd);               \
+  }
+#else /* (BOARD_VL53L1X_CONNECTED == true) */
+  #define MODULE_INIT_TEST_VL53L1X() {}
+#endif /* (BOARD_VL53L1X_CONNECTED == true) */
+
 /**
  * @brief   Periphery communication interfaces initialization hook.
  */
@@ -149,6 +174,7 @@ extern ROMCONST char* moduleShellPrompt;
   sdStart(&MODULE_HAL_PROGIF, &moduleHalProgIfConfig);                        \
   /* MPU6050 demo */                                                          \
   MODULE_INIT_PERIPHERY_IF_MPU6050();                                         \
+  MODULE_INIT_PERIPHERY_IF_VL53L1X();                                         \
 }
 #if (BOARD_MPU6050_CONNECTED == true)
   #define MODULE_INIT_PERIPHERY_IF_MPU6050() {                                \
@@ -179,6 +205,43 @@ extern ROMCONST char* moduleShellPrompt;
   #define MODULE_INIT_PERIPHERY_IF_MPU6050() {}
 #endif /* (BOARD_MPU6050_CONNECTED == true) */
 
+#if (BOARD_VL53L1X_CONNECTED == true)
+  #define MODULE_INIT_PERIPHERY_IF_VL53L1X() {                                \
+    /* maximum I2C frequency is 1MHz for this MCU */                          \
+    uint32_t i2c3_freq = 1000000;                                             \
+    /* find minimum amon all devices connected to this bus */                 \
+    i2c3_freq = (VL53L1X_LLD_I2C_MAXFREQUENCY < i2c3_freq) ? VL53L1X_LLD_I2C_MAXFREQUENCY : i2c3_freq;  \
+    /* calculate PRESC (prescaler):                                           \
+     *   target is 1/(I2CXCLK * (PRESC + 1)) = 125ns                          \
+     */                                                                       \
+    moduleHalI2c3Config.timingr = ((uint8_t)((0.000000125f * STM32_I2C3CLK) - 1)) << I2C_TIMINGR_PRESC_Pos; \
+    /* SCL shall be low half of the time. */                                  \
+    moduleHalI2c3Config.timingr |= ((uint8_t)((1.f / i2c3_freq / 2 / 0.000000125f) - 1)) << I2C_TIMINGR_SCLL_Pos; \
+    /* SCL shall be high half the time of low or slightly longer. */          \
+    moduleHalI2c3Config.timingr |= (uint8_t)(ceilf(((moduleHalI2c3Config.timingr & I2C_TIMINGR_SCLL_Msk) >> I2C_TIMINGR_SCLL_Pos) / 2.f)) << I2C_TIMINGR_SCLH_Pos;  \
+    /* SDA shall be delayed 1/10 of SCL low, or shorter */                    \
+    moduleHalI2c3Config.timingr |= (uint8_t)(((moduleHalI2c3Config.timingr & I2C_TIMINGR_SCLL_Msk) >> I2C_TIMINGR_SCLL_Pos) * 0.1f) << I2C_TIMINGR_SDADEL_Pos;  \
+    /* SCL shall be delyed twice as long as SDA, but longer than 0. */        \
+    moduleHalI2c3Config.timingr |= ((((moduleHalI2c3Config.timingr & I2C_TIMINGR_SDADEL_Msk) >> I2C_TIMINGR_SDADEL_Pos) * 2) + 1) << I2C_TIMINGR_SCLDEL_Pos;  \
+    /* setup the GPIO signals */                                              \
+    moduleHalI2c3Config.timingr = 0x00702991; \
+    chSysLock();                                \
+    PAL_PORT(moduleLldVl53l1x.xshut->gpio->line)->PUPDR &= ~(3U << (PAL_PAD(moduleLldVl53l1x.xshut->gpio->line) * 2U)); \
+    PAL_PORT(moduleLldVl53l1x.xshut->gpio->line)->PUPDR |= PIN_PUPDR_PULLUP(PAL_PAD(moduleLldVl53l1x.xshut->gpio->line)); \
+    /* palSetLineMode(moduleLldVl53l1x.xshut->gpio->line, PAL_MODE_OUTPUT_OPENDRAIN); \*/ \
+      palSetLineMode(moduleLldVl53l1x.xshut->gpio->line, PAL_MODE_OUTPUT_PUSHPULL); \
+palSetLineMode(moduleLldVl53l1x.gpio1->gpio->line, PAL_MODE_INPUT_PULLUP); \
+    palSetLineMode(LINE_ARD_A4, PAL_MODE_ALTERNATE(4));                       \
+    palSetLineMode(LINE_ARD_A5, PAL_MODE_ALTERNATE(4));                       \
+    chSysUnlock();                                                            \
+    apalControlGpioSet(moduleLldVl53l1x.xshut, APAL_GPIO_OFF);                \
+    /* now we can start the I2C driver */                                     \
+    i2cStart(&MODULE_HAL_I2C3, &moduleHalI2c3Config);                         \
+  }
+#else /* (BOARD_VL53L1X_CONNECTED == true) */
+  #define MODULE_INIT_PERIPHERY_IF_VL53L1X() {}
+#endif /* (BOARD_VL53L1X_CONNECTED == true) */
+
 /**
  * @brief   Periphery communication interface deinitialization hook.
  */
@@ -226,6 +289,14 @@ extern ROMCONST char* moduleShellPrompt;
  */
 extern LEDDriver moduleLldLed;
 
+#if (BOARD_VL53L1X_CONNECTED == true) || defined(__DOXYGEN__)
+
+  #include <alld_VL53L1X.h>
+
+  extern VL53L1XDriver moduleLldVl53l1x;
+
+#endif /* (BOARD_VL53L1X_CONNECTED == true) */
+
 /**
  * @brief   Button driver.
  */
@@ -258,6 +329,12 @@ extern MPU6050Driver moduleLldMpu6050;
 extern aos_shellcommand_t moduleTestLedShellCmd;
 
 /**
+ * @brief   VL53L1X test command.
+ */
+extern aos_shellcommand_t moduleTestVL53L1XShellCmd;
+extern aos_shellcommand_t moduleTestVL53L1XInitShellCmd;
+
+/**
  * @brief   User button test command.
  */
 extern aos_shellcommand_t moduleTestButtonShellCmd;
diff --git a/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.c b/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.c
new file mode 100644
index 0000000..b9c39cc
--- /dev/null
+++ b/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.c
@@ -0,0 +1,88 @@
+/*
+AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
+Copyright (C) 2016..2020  Thomas Schöpping et al.
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <amiroos.h>
+
+#if (AMIROOS_CFG_TESTS_ENABLE == true) || defined(__DOXYGEN__)
+
+#include <module_test_VL53L1X.h>
+#include <aos_test_VL53L1X.h>
+
+/******************************************************************************/
+/* LOCAL DEFINITIONS                                                          */
+/******************************************************************************/
+
+/******************************************************************************/
+/* EXPORTED VARIABLES                                                         */
+/******************************************************************************/
+
+/******************************************************************************/
+/* LOCAL TYPES                                                                */
+/******************************************************************************/
+
+/******************************************************************************/
+/* LOCAL VARIABLES                                                            */
+/******************************************************************************/
+
+static aos_test_vl53l1x_bus _data = {
+  /* pointer to VL53L1X driver  */ &moduleLldVl53l1x,
+};
+
+static AOS_TEST(_test, "VL53L1X", NULL, moduleTestVL53L1XShellCb, aosTestVL53L1XFunc, &_data);
+static AOS_TEST(_test2, "VL53L1X:INIT", NULL, moduleTestVL53L1XInitShellCb, aosTestVL53L1Xi2cInit, &_data);
+
+/******************************************************************************/
+/* LOCAL FUNCTIONS                                                            */
+/******************************************************************************/
+
+/******************************************************************************/
+/* EXPORTED FUNCTIONS                                                         */
+/******************************************************************************/
+
+int moduleTestVL53L1XShellCb(BaseSequentialStream* stream, int argc, char* argv[], aos_testresult_t* result)
+{
+  (void)argc;
+  (void)argv;
+
+
+  aosprintf("STM32_I2C3CLK: %u\n", STM32_I2C3CLK);
+  aosprintf("Timing_: %08x\n", moduleHalI2c3Config.timingr);
+  if (result != NULL) {
+    *result = aosTestRun(stream, &_test, NULL);
+  } else {
+    aosTestRun(stream, &_test, NULL);
+  }
+
+  return AOS_OK;
+}
+
+int moduleTestVL53L1XInitShellCb(BaseSequentialStream* stream, int argc, char* argv[], aos_testresult_t* result)
+{
+  (void)argc;
+  (void)argv;
+
+  if (result != NULL) {
+    *result = aosTestRun(stream, &_test2, NULL);
+  } else {
+    aosTestRun(stream, &_test2, NULL);
+  }
+
+  return AOS_OK;
+}
+
+#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) */
diff --git a/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.h b/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.h
new file mode 100644
index 0000000..1685945
--- /dev/null
+++ b/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.h
@@ -0,0 +1,67 @@
+/*
+AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
+Copyright (C) 2016..2020  Thomas Schöpping et al.
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef MODULE_TEST_VL53L1X_H
+#define MODULE_TEST_VL53L1X_H
+
+#include <amiroos.h>
+
+#if (AMIROOS_CFG_TESTS_ENABLE == true) || defined(__DOXYGEN__)
+
+/******************************************************************************/
+/* CONSTANTS                                                                  */
+/******************************************************************************/
+
+/******************************************************************************/
+/* SETTINGS                                                                   */
+/******************************************************************************/
+
+/******************************************************************************/
+/* CHECKS                                                                     */
+/******************************************************************************/
+
+/******************************************************************************/
+/* DATA STRUCTURES AND TYPES                                                  */
+/******************************************************************************/
+
+/******************************************************************************/
+/* MACROS                                                                     */
+/******************************************************************************/
+
+/******************************************************************************/
+/* EXTERN DECLARATIONS                                                        */
+/******************************************************************************/
+
+#if defined(__cplusplus)
+extern "C" {
+#endif /* defined(__cplusplus) */
+  int moduleTestVL53L1XShellCb(BaseSequentialStream* stream, int argc, char* argv[], aos_testresult_t* result);
+  int moduleTestVL53L1XInitShellCb(BaseSequentialStream* stream, int argc, char* argv[], aos_testresult_t* result);
+#if defined(__cplusplus)
+}
+#endif /* defined(__cplusplus) */
+
+/******************************************************************************/
+/* INLINE FUNCTIONS                                                           */
+/******************************************************************************/
+
+#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) */
+
+#endif /* MODULE_TEST_VL53L1X_H */
+
diff --git a/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.mk b/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.mk
new file mode 100644
index 0000000..5c80906
--- /dev/null
+++ b/modules/NUCLEO-L476RG/test/VL53L1X/module_test_VL53L1X.mk
@@ -0,0 +1,35 @@
+################################################################################
+# AMiRo-OS is an operating system designed for the Autonomous Mini Robot       #
+# (AMiRo) platform.                                                            #
+# Copyright (C) 2016..2020  Thomas Schöpping et al.                            #
+#                                                                              #
+# This program is free software: you can redistribute it and/or modify         #
+# it under the terms of the GNU General Public License as published by         #
+# the Free Software Foundation, either version 3 of the License, or            #
+# (at your option) any later version.                                          #
+#                                                                              #
+# This program is distributed in the hope that it will be useful,              #
+# but WITHOUT ANY WARRANTY; without even the implied warranty of               #
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                #
+# GNU General Public License for more details.                                 #
+#                                                                              #
+# You should have received a copy of the GNU General Public License            #
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.        #
+#                                                                              #
+# This research/work was supported by the Cluster of Excellence Cognitive      #
+# Interaction Technology 'CITEC' (EXC 277) at Bielefeld University, which is   #
+# funded by the German Research Foundation (DFG).                              #
+################################################################################
+
+
+
+# path to this directory
+MODULE_TEST_VL53L1X_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
+
+include $(AMIROOS_TEST_DIR)periphery-lld/VL53L1X_v1/aos_test_VL53L1X.mk
+# include paths
+MODULE_INC += $(MODULE_TEST_VL53L1X_DIR:/=)
+
+# C source files
+MODULE_CSRC += $(MODULE_TEST_VL53L1X_DIR)module_test_VL53L1X.c
+
diff --git a/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.c b/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.c
new file mode 100644
index 0000000..3bac08b
--- /dev/null
+++ b/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.c
@@ -0,0 +1,268 @@
+/*
+AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
+Copyright (C) 2016..2020  Thomas Schöpping et al.
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <amiroos.h>
+#include <aos_test_VL53L1X.h>
+
+#if (AMIROOS_CFG_TESTS_ENABLE == true) || defined(__DOXYGEN__)
+
+/******************************************************************************/
+/* LOCAL DEFINITIONS                                                          */
+/******************************************************************************/
+
+/******************************************************************************/
+/* EXPORTED VARIABLES                                                         */
+/******************************************************************************/
+
+/******************************************************************************/
+/* LOCAL TYPES                                                                */
+/******************************************************************************/
+
+/******************************************************************************/
+/* LOCAL VARIABLES                                                            */
+/******************************************************************************/
+
+/******************************************************************************/
+/* LOCAL FUNCTIONS                                                            */
+/******************************************************************************/
+
+/******************************************************************************/
+/* EXPORTED FUNCTIONS                                                         */
+/*****************************************************************************
+*/
+
+/**
+ * @brief   LED test function.
+ *
+ * @param[in] stream  Stream for input/output.
+ * @param[in] test    Test object.
+ *
+ * @return            Test result value.
+ */
+aos_testresult_t aosTestVL53L1XFunc(BaseSequentialStream* stream, const aos_test_t* test)
+{
+//   aosDbgCheck(test->data != NULL && ((aos_test_leddata_t*)test->data)->led != NULL);
+
+chprintf(stream, "Peter Wurst\n");
+
+  // local variables
+  aos_testresult_t result = {0,0};
+//   int32_t status;
+//     chprintf(stream, "Hello World!\n");
+// //   aosTestResultInit(&result);
+
+//   VL53L1_Dev_t dev;
+//   dev.vl53l1x = *(((aos_test_vl53l1x_bus*)test->data)->vl53l1x);
+
+//   chprintf(stream, "Toggle PC2...\n");
+//   chprintf(stream, "On...\n");
+  /* vl53l1x_GPIO1_set(((aos_test_vl53l1x_bus*)test->data)->vl53l1x, GPIO_LLD_STATE_ON); */
+//   aosThdSSleep(5);
+//   chprintf(stream, "off...\n");
+//   status |= vl53l1x_GPIO1_set(((aos_test_vl53l1x_bus*)test->data)->vl53l1x, GPIO_LLD_STATE_OFF);
+//   aosThdSSleep(5);
+//   chprintf(stream, "On...\n");
+//   status |= vl53l1x_GPIO1_set(((aos_test_vl53l1x_bus*)test->data)->vl53l1x, GPIO_LLD_STATE_ON);
+//   aosThdSSleep(5);
+//   chprintf(stream, "Off...\n");
+//   status |= vl53l1x_GPIO1_set(((aos_test_vl53l1x_bus*)test->data)->vl53l1x, GPIO_LLD_STATE_OFF);
+//   // status |= led_lld_get(((aos_test_leddata_t*)test->data)->led, &state);
+//   if (status == APAL_STATUS_OK) {
+//       aosTestPassed(stream, &result);
+//   } else {
+//     aosTestFailed(stream, &result);
+//   }p
+
+//   chprintf(stream, "turning off for two seconds...\n");
+//   state = LED_LLD_STATE_OFF;
+//   status = led_lld_set(((aos_test_leddata_t*)test->data)->led, state);
+//   aosThdSSleep(2);
+//   status |= led_lld_get(((aos_test_leddata_t*)test->data)->led, &state);
+//   if (status == APAL_STATUS_OK && state == LED_LLD_STATE_OFF) {
+//     aosTestPassed(stream, &result);
+//   } else {
+//     aosTestFailed(stream, &result);
+//   }
+
+//   chprintf(stream, "toggling for two seconds...\n");
+//   status = led_lld_set(((aos_test_leddata_t*)test->data)->led, LED_LLD_STATE_ON);
+//   for (uint32_t i = 0; i < 2000/100; ++i) {
+//     status |= led_lld_toggle(((aos_test_leddata_t*)test->data)->led);
+//     status |= led_lld_get(((aos_test_leddata_t*)test->data)->led, &state);
+//     status |= ((uint8_t)state != (i & 0x01u)) ? APAL_STATUS_ERROR : APAL_STATUS_OK;
+//     aosThdMSleep(100);
+//   }
+//   status = led_lld_set(((aos_test_leddata_t*)test->data)->led, LED_LLD_STATE_OFF);
+//   if (status == APAL_STATUS_OK) {
+//     aosTestPassed(stream, &result);
+//   } else {
+//     aosTestFailed(stream, &result);
+//   }
+
+//   aosTestInfoMsg(stream,"driver object memory footprint: %u bytes\n", sizeof(LEDDriver));
+
+/*
+  uint8_t byteData, sensorState=0;
+  uint16_t wordData;
+  uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
+  uint16_t Distance;
+  uint16_t SignalRate;
+  uint16_t AmbientRate;
+  uint8_t RangeStatus;
+  uint8_t dataReady;
+  int32_t status;
+  */
+
+    
+//   aosTestResultInit(&result);
+
+  VL53L1_Dev_t dev;
+  dev.vl53l1x = *(((aos_test_vl53l1x_bus*)test->data)->vl53l1x);
+  /* vl53l1x_GPIO1_set(&dev.vl53l1x, GPIO_LLD_STATE_ON); */
+
+  // status = VL53L1_RdByte(&dev, 0x010F, &byteData);
+  // printf("VL53L1X Model_ID: %X\n", byteData);
+  // status = VL53L1_RdByte(&dev, 0x0110, &byteData);
+  // printf("VL53L1X Module_Type: %X\n", byteData);
+  // status = VL53L1_RdWord(&dev, 0x010F, &wordData);
+  // printf("VL53L1X: %X\n", wordData);
+  
+  // activate device
+  apalControlGpioState_t xshut_state;
+  apalControlGpioState_t gpio_state;
+  apalControlGpioGet(dev.vl53l1x.xshut, &xshut_state);
+  apalControlGpioGet(dev.vl53l1x.gpio1, &gpio_state);
+  if (xshut_state == APAL_GPIO_OFF) {
+    chprintf(stream, "XSHUT OFF -> activating\n");
+    apalControlGpioSet(dev.vl53l1x.xshut, APAL_GPIO_ON);
+    aosThdSSleep(1);
+    aosThdUSleep(2 * VL53L1X_LLD_BOOTDURATION);
+  } else {
+    chprintf(stream, "XSHUT ON\n");
+  }
+  chprintf(stream, "0x%08X | 0x%08X\n", (uint32_t)dev.vl53l1x.i2cd, (uint32_t)(&I2CD3));
+  chprintf(stream, "0x%02X\n", dev.vl53l1x.xshut);
+  if (gpio_state == APAL_GPIO_OFF) {
+    chprintf(stream, "Gpio OFF \n");
+  }
+
+  uint8_t testBuf[49] = {0};
+  // Reste device
+  
+  // printf("Resetting device! \n");
+  /* chprintf(stream, "Testif broken!\n"); */
+  
+  uint8_t data[4] = {0};
+  /* Status = VL53L1_RdByte(Dev, VL53L1_PAD_I2C_HV__EXTSUP_CONFIG, &i); */
+  /* apalI2CMasterReceive(dev.vl53l1x.i2cd, dev.vl53l1x.addr, testBuf, 3, 10); */
+  /* aosThdMSleep(1000); */
+  /* chprintf(stream, "Start reset!\n"); */
+  /* apalI2CMasterTransmit(dev.vl53l1x.i2cd, dev.vl53l1x.addr, data, 3, testBuf, 0, 10); */
+  chprintf(stream, "Before sleep!\n");
+  /* apalI2CMasterTransmit() */
+  /* aosThdMSleep(1000); */
+  aosThdUSleep(2 * VL53L1X_LLD_BOOTDURATION);
+  /* 0x002E */
+
+  data[0] =  0x01;
+  data[1] =  0x0f;
+  uint8_t stuff[4] = { 0 };
+  /* data[3] = 0x01; */
+  chprintf(stream, "End reset!\n");
+  
+  apalExitStatus_t res = apalI2CMasterTransmit(dev.vl53l1x.i2cd,  dev.vl53l1x.addr, data, 2, stuff , 0, 10);
+  
+
+  chprintf(stream, "Res: %d\n", res);
+  aosThdMSleep(100);
+  i2cflags_t err =  i2cGetErrors(dev.vl53l1x.i2cd);
+  chprintf(stream, "Error: %08x\n", err);
+  aosThdMSleep(100);
+  /* apalI2CMasterReceive(dev.vl53l1x.i2cd,  dev.vl53l1x.addr, testBuf, 4, 10); */
+  /* apalI2CMasterTransmit(dev.vl53l1x.i2cd, dev.vl53l1x.addr, data, 3, testBuf, 0, 10); */
+  /* /\* VL53L1_RdByte(&dev, VL53L1_IDENTIFICATION__MODEL_ID, &data); *\/ */
+  chprintf(stream, "0x%02\n", stuff[0]);
+  chprintf(stream, "0x%02\n", stuff[1]);
+  chprintf(stream, "0x%02\n", data[0]);
+  chprintf(stream, "0x%02\n", data[1]);
+  for(int i = 0; i < 49 ; i++){
+    chprintf(stream, "0x%02 ", testBuf[i]);
+  }
+  chprintf(stream, "\n");
+
+  /* VL53L1_WrByte(&dev, 0x0000, 0x00); */
+  /* aosThdSSleep(2); */
+  /* vl53l1_WrByte(&dev, 0x0000, 0x01); */
+
+
+  aosTestPassed(stream, &result);
+
+
+  return result;
+}
+
+
+
+/**
+ * @brief   Test if I2C was initialized correctly.
+ *
+ * @param[in] stream  Stream for input/output.
+ * @param[in] test    Test object.
+ *
+ * @return            Test result value.
+ */
+aos_testresult_t aosTestVL53L1Xi2cInit(BaseSequentialStream* stream, const aos_test_t* test)
+{
+//   aosDbgCheck(test->data != NULL && ((aos_test_leddata_t*)test->data)->led != NULL);
+
+  // local variables
+  aos_testresult_t result = {0,0};
+  uint8_t byteData, sensorState=0;
+  uint16_t wordData;
+  uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
+  uint16_t Distance;
+  uint16_t SignalRate;
+  uint16_t AmbientRate;
+  uint8_t RangeStatus;
+  uint8_t dataReady;
+  int32_t status;
+    chprintf(stream, "Hello World!\n");
+//   aosTestResultInit(&result);
+
+  VL53L1_Dev_t dev;
+  dev.vl53l1x = *(((aos_test_vl53l1x_bus*)test->data)->vl53l1x);
+  // status = VL53L1_RdByte(&dev, 0x010F, &byteData);
+  // printf("VL53L1X Model_ID: %X\n", byteData);
+  // status = VL53L1_RdByte(&dev, 0x0110, &byteData);
+  // printf("VL53L1X Module_Type: %X\n", byteData);
+  // status = VL53L1_RdWord(&dev, 0x010F, &wordData);
+  // printf("VL53L1X: %X\n", wordData);
+  uint8_t testBuf[2] = {0};
+  // Reste device
+  printf("Resetting device! \n");
+  VL53L1_WrByte(&dev, 0x0000, 0x00);
+  aosThdSSleep(2);
+  VL53L1_WrByte(&dev, 0x0000, 0x01);
+
+
+  aosTestPassed(stream, &result);
+
+  return result;
+}
+
+#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) */
diff --git a/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.h b/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.h
new file mode 100644
index 0000000..a02af88
--- /dev/null
+++ b/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.h
@@ -0,0 +1,80 @@
+/*
+AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
+Copyright (C) 2016..2020  Thomas Schöpping et al.
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef AMIROOS_TEST_VL53L1X
+#define AMIROOS_TEST_VL53L1X
+
+#include <amiroos.h>
+
+
+#if (AMIROOS_CFG_TESTS_ENABLE == true) || defined(__DOXYGEN__)
+
+#include <alld_VL53L1X.h>
+
+/******************************************************************************/
+/* CONSTANTS                                                                  */
+/******************************************************************************/
+
+/******************************************************************************/
+/* SETTINGS                                                                   */
+/******************************************************************************/
+
+/******************************************************************************/
+/* CHECKS                                                                     */
+/******************************************************************************/
+
+/******************************************************************************/
+/* DATA STRUCTURES AND TYPES                                                  */
+/******************************************************************************/
+
+/**
+ * @brief   Custom data structure for the test.
+ */
+typedef struct {
+  /**
+   * @brief   Pointer to the driver to use.
+   */
+  VL53L1XDriver* vl53l1x;
+
+} aos_test_vl53l1x_bus;
+
+
+/******************************************************************************/
+/* MACROS                                                                     */
+/******************************************************************************/
+
+/******************************************************************************/
+/* EXTERN DECLARATIONS                                                        */
+/******************************************************************************/
+
+#if defined(__cplusplus)
+extern "C" {
+#endif /* defined(__cplusplus) */
+  aos_testresult_t aosTestVL53L1XFunc(BaseSequentialStream* stream, const aos_test_t* test);
+  aos_testresult_t aosTestVL53L1Xi2cInit(BaseSequentialStream* stream, const aos_test_t* test);
+#if defined(__cplusplus)
+}
+#endif /* defined(__cplusplus) */
+
+/******************************************************************************/
+/* INLINE FUNCTIONS                                                           */
+/******************************************************************************/
+
+#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) */
+
+#endif /* AMIROOS_TEST_VL53L1X */
diff --git a/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.mk b/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.mk
new file mode 100644
index 0000000..f06d645
--- /dev/null
+++ b/test/periphery-lld/VL53L1X_v1/aos_test_VL53L1X.mk
@@ -0,0 +1,34 @@
+################################################################################
+# AMiRo-OS is an operating system designed for the Autonomous Mini Robot       #
+# (AMiRo) platform.                                                            #
+# Copyright (C) 2016..2020  Thomas Schöpping et al.                            #
+#                                                                              #
+# This program is free software: you can redistribute it and/or modify         #
+# it under the terms of the GNU General Public License as published by         #
+# the Free Software Foundation, either version 3 of the License, or            #
+# (at your option) any later version.                                          #
+#                                                                              #
+# This program is distributed in the hope that it will be useful,              #
+# but WITHOUT ANY WARRANTY; without even the implied warranty of               #
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                #
+# GNU General Public License for more details.                                 #
+#                                                                              #
+# You should have received a copy of the GNU General Public License            #
+# along with this program.  If not, see <http://www.gnu.org/licenses/>.        #
+#                                                                              #
+# This research/work was supported by the Cluster of Excellence Cognitive      #
+# Interaction Technology 'CITEC' (EXC 277) at Bielefeld University, which is   #
+# funded by the German Research Foundation (DFG).                              #
+################################################################################
+
+
+
+# path to this directory
+AMIROOS_TEST_VL53L1X_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
+
+# include path
+AMIROOS_TEST_INC += $(AMIROOS_TEST_VL53L1X_DIR)
+
+# C sources
+AMIROOS_TEST_CSRC += $(AMIROOS_TEST_VL53L1X_DIR)aos_test_VL53L1X.c
+
