AMiRo-OS

/*

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