AMiRo-OS

include $(AMIROLLD)/drivers/VL53L1X/v1/alld_VL53L1X.mk

include $(MODULE_DIR)/test/VL53L1X/module_test_VL53L1X.mk

#define BOARD_VL53L1X_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) */

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,

  },

};

#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) */

 * 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);

/*

#if (BOARD_MPU6050_CONNECTED == true || BOARD_VL53L1X_CONNECTED == true) || defined(__DOXYGEN__)

#endif /* (BOARD_MPU6050_CONNECTED == true || BOARD_VL53L1X_CONNECTED == true) */

extern ROMCONST apalControlGpio_t moduleGpioVl53l1xINT;

extern ROMCONST apalControlGpio_t moduleGpioVl53l1xXSHUT;

#include <math.h>

  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) */

  MODULE_INIT_TEST_VL53L1X();                                                 \

#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) */

  MODULE_INIT_PERIPHERY_IF_VL53L1X();                                         \

#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) */

#if (BOARD_VL53L1X_CONNECTED == true) || defined(__DOXYGEN__)

  #include <alld_VL53L1X.h>

  extern VL53L1XDriver moduleLldVl53l1x;

#endif /* (BOARD_VL53L1X_CONNECTED == true) */

 * @brief   VL53L1X test command.

 */

extern aos_shellcommand_t moduleTestVL53L1XShellCmd;

extern aos_shellcommand_t moduleTestVL53L1XInitShellCmd;

/**

/*

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) */

/*

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 */

################################################################################

# 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

/*

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) */

/*

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 */

################################################################################

# 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