API tools faq
paste
Guest User

vl6180.c

a guest
Feb 11th, 2019
425
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 8.64 KB | None | 0 0
raw download clone embed print report
  1. /*
  2.  * vl6180.c
  3.  *
  4.  *  Created on: Feb 4, 2019
  5.  *      Author: DEVJEET MANDAL
  6.  */
  7.  
  8.  
  9. #include "vl6180.h"
  10.  
  11. extern I2C_HandleTypeDef hi2c1;
  12.  
  13. #define ADDRESS_DEFAULT 0b0101001
  14.  
  15. static uint16_t const ScalerValues[] = {0, 253, 127, 84};
  16.  
  17. VL6180X *lidar;
  18.  
  19. void setup_VL6180X(void)        {
  20.     lidar->address      = 0x29 << 1;//ADDRESS_DEFAULT;
  21.     lidar->scaling      = 0;
  22.     lidar->ptp_offset   = 0;
  23.     lidar->io_timeout   = 0;
  24.     lidar->did_timeout  = false;
  25. }
  26.  
  27. void setAddress(uint8_t new_addr)   {
  28.     writeReg(I2C_SLAVE__DEVICE_ADDRESS, new_addr & 0x7F);
  29.     lidar->address = new_addr;
  30. }
  31.  
  32. uint8_t getAddress(void)        {
  33.     return lidar->address;
  34. }
  35.  
  36. void getWhoAmI(VL6180X_Identity *Identity)      {
  37.     Identity->idModel           = readReg(IDENTIFICATION__MODEL_ID);
  38.     Identity->idModelRevMajor   = readReg(IDENTIFICATION__MODEL_REV_MAJOR);
  39.     Identity->idModelRevMinor   = readReg(IDENTIFICATION__MODEL_REV_MINOR);
  40.     Identity->idModuleRevMajor  = readReg(IDENTIFICATION__MODULE_REV_MAJOR);
  41.     Identity->idModuleRevMinor  = readReg(IDENTIFICATION__MODULE_REV_MINOR);
  42.     //TODO: Date
  43.     //TODO: Time
  44. }
  45.  
  46. bool init()                     {
  47.     bool xStatus = false;
  48.     lidar->ptp_offset = readReg(SYSRANGE__PART_TO_PART_RANGE_OFFSET);
  49.  
  50.     if (readReg(SYSTEM__FRESH_OUT_OF_RESET) == 1)
  51.     {
  52.         lidar->scaling = 1;
  53.  
  54.         writeReg(0x207, 0x01);
  55.         writeReg(0x208, 0x01);
  56.         writeReg(0x096, 0x00);
  57.         writeReg(0x097, 0xFD); // RANGE_SCALER = 253
  58.         writeReg(0x0E3, 0x00);
  59.         writeReg(0x0E4, 0x04);
  60.         writeReg(0x0E5, 0x02);
  61.         writeReg(0x0E6, 0x01);
  62.         writeReg(0x0E7, 0x03);
  63.         writeReg(0x0F5, 0x02);
  64.         writeReg(0x0D9, 0x05);
  65.         writeReg(0x0DB, 0xCE);
  66.         writeReg(0x0DC, 0x03);
  67.         writeReg(0x0DD, 0xF8);
  68.         writeReg(0x09F, 0x00);
  69.         writeReg(0x0A3, 0x3C);
  70.         writeReg(0x0B7, 0x00);
  71.         writeReg(0x0BB, 0x3C);
  72.         writeReg(0x0B2, 0x09);
  73.         writeReg(0x0CA, 0x09);
  74.         writeReg(0x198, 0x01);
  75.         writeReg(0x1B0, 0x17);
  76.         writeReg(0x1AD, 0x00);
  77.         writeReg(0x0FF, 0x05);
  78.         writeReg(0x100, 0x05);
  79.         writeReg(0x199, 0x05);
  80.         writeReg(0x1A6, 0x1B);
  81.         writeReg(0x1AC, 0x3E);
  82.         writeReg(0x1A7, 0x1F);
  83.         writeReg(0x030, 0x00);
  84.  
  85.         writeReg(SYSTEM__FRESH_OUT_OF_RESET, 0);
  86.         xStatus = true;
  87.     }
  88.     else
  89.     {
  90.         uint16_t s = readReg16Bit(RANGE_SCALER);
  91.  
  92.         if      (s == ScalerValues[3]) { lidar->scaling = 3; }
  93.         else if (s == ScalerValues[2]) { lidar->scaling = 2; }
  94.         else                           { lidar->scaling = 1; }
  95.  
  96.         lidar->ptp_offset *= lidar->scaling;
  97.         xStatus = false;
  98.     }
  99.     return xStatus;
  100. }
  101.  
  102. void configureDefault(void)         {
  103.     writeReg(READOUT__AVERAGING_SAMPLE_PERIOD, 0x30);
  104.     writeReg(SYSALS__ANALOGUE_GAIN, 0x46);
  105.     writeReg(SYSRANGE__VHV_REPEAT_RATE, 0xFF);
  106.     writeReg16Bit(SYSALS__INTEGRATION_PERIOD, 0x0063);
  107.     writeReg(SYSRANGE__VHV_RECALIBRATE, 0x01);
  108.     writeReg(SYSRANGE__INTERMEASUREMENT_PERIOD, 0x09);
  109.     writeReg(SYSALS__INTERMEASUREMENT_PERIOD, 0x31);
  110.     writeReg(SYSTEM__INTERRUPT_CONFIG_GPIO, 0x24);
  111.     writeReg(SYSRANGE__MAX_CONVERGENCE_TIME, 0x31);
  112.     writeReg(INTERLEAVED_MODE__ENABLE, 0);
  113.     setScaling(1);
  114. }
  115.  
  116. void setScaling(uint8_t new_scaling)        {
  117.     uint8_t const DefaultCrosstalkValidHeight = 20; // default value of SYSRANGE__CROSSTALK_VALID_HEIGHT
  118.     if (new_scaling < 1 || new_scaling > 3) { return; }
  119.     lidar->scaling = new_scaling;
  120.     writeReg16Bit(RANGE_SCALER, ScalerValues[lidar->scaling]);
  121.     writeReg(SYSRANGE__PART_TO_PART_RANGE_OFFSET, lidar->ptp_offset / lidar->scaling);
  122.     writeReg(SYSRANGE__CROSSTALK_VALID_HEIGHT, DefaultCrosstalkValidHeight / lidar->scaling);
  123.     uint8_t rce = readReg(SYSRANGE__RANGE_CHECK_ENABLES);
  124.     writeReg(SYSRANGE__RANGE_CHECK_ENABLES, (rce & 0xFE) | (lidar->scaling == 1));
  125. }
  126.  
  127. uint8_t getScaling(void) { return lidar->scaling; }
  128.  
  129. uint8_t readRangeSingle(void)       {
  130.     writeReg(SYSRANGE__START, 0x01);
  131.     return readRangeContinuous();
  132. }
  133.  
  134. uint16_t readRangeSingleMillimeters(void) { return (uint16_t)lidar->scaling * readRangeSingle(); }
  135.  
  136. uint16_t readAmbientSingle(void)        {
  137.     writeReg(SYSALS__START, 0x01);
  138.     return readAmbientContinuous();
  139.  
  140. }
  141.  
  142. uint16_t constrain(uint16_t x, uint16_t a, uint16_t b)          {
  143.     uint16_t retVal = x;
  144.     if (x >= a && x <= b)   retVal = x;
  145.     else if (x < a)  retVal = a;
  146.     else if (x > b)  retVal = b;
  147.  
  148.     return retVal;
  149. }
  150.  
  151. void startRangeContinuous(uint16_t period)      {
  152.     int16_t period_reg = (int16_t)(period / 10) - 1;
  153.     period_reg = constrain(period_reg, 0, 254);
  154.  
  155.     writeReg(SYSRANGE__INTERMEASUREMENT_PERIOD, period_reg);
  156.     writeReg(SYSRANGE__START, 0x03);
  157. }
  158.  
  159. void startAmbientContinuous(uint16_t period)    {
  160.     int16_t period_reg = (int16_t)(period / 10) - 1;
  161.     period_reg = constrain(period_reg, 0, 254);
  162.  
  163.     writeReg(SYSALS__INTERMEASUREMENT_PERIOD, period_reg);
  164.     writeReg(SYSALS__START, 0x03);
  165. }
  166.  
  167. void startInterleavedContinuous(uint16_t period)        {
  168.     int16_t period_reg = (int16_t)(period / 10) - 1;
  169.     period_reg = constrain(period_reg, 0, 254);
  170.  
  171.     writeReg(INTERLEAVED_MODE__ENABLE, 1);
  172.     writeReg(SYSALS__INTERMEASUREMENT_PERIOD, period_reg);
  173.     writeReg(SYSALS__START, 0x03);
  174. }
  175.  
  176. void stopContinuous()           {
  177.     writeReg(SYSRANGE__START, 0x01);
  178.     writeReg(SYSALS__START, 0x01);
  179.  
  180.     writeReg(INTERLEAVED_MODE__ENABLE, 0);
  181. }
  182.  
  183. uint8_t readRangeContinuous(void)           {
  184.     uint16_t millis_start = HAL_GetTick();
  185.     while ((readReg(RESULT__INTERRUPT_STATUS_GPIO) & 0x04) == 0)
  186.     {
  187.         if (lidar->io_timeout > 0 && ((uint16_t)HAL_GetTick() - millis_start) > lidar->io_timeout)
  188.         {
  189.           lidar->did_timeout = true;
  190.           return 255;
  191.         }
  192.     }
  193.  
  194.     uint8_t range = readReg(RESULT__RANGE_VAL);
  195.     writeReg(SYSTEM__INTERRUPT_CLEAR, 0x01);
  196.  
  197.     return range;
  198. }
  199.  
  200.  
  201. uint16_t readRangeContinuousMillimeters(void) { return (uint16_t)lidar->scaling * readRangeContinuous(); }
  202.  
  203. uint16_t readAmbientContinuous(void)        {
  204.     uint16_t millis_start = HAL_GetTick();
  205.     while ((readReg(RESULT__INTERRUPT_STATUS_GPIO) & 0x20) == 0)
  206.     {
  207.         if (lidar->io_timeout > 0 && ((uint16_t)HAL_GetTick() - millis_start) > lidar->io_timeout)
  208.         {
  209.           lidar->did_timeout = true;
  210.           return 0;
  211.         }
  212.     }
  213.  
  214.     uint16_t ambient = readReg16Bit(RESULT__ALS_VAL);
  215.     writeReg(SYSTEM__INTERRUPT_CLEAR, 0x02);
  216.  
  217.     return ambient;
  218. }
  219.  
  220. bool timeoutOccurred(void)          {
  221.     bool tmp = lidar->did_timeout;
  222.     lidar->did_timeout = false;
  223.     return tmp;
  224. }
  225.  
  226. void setTimeout(uint16_t timeout) { lidar->io_timeout = timeout; }
  227.  
  228. uint16_t getTimeout(void) { return lidar->io_timeout; }
  229.  
  230. void writeReg(uint16_t reg, uint8_t value)      {
  231.     char data_write[3];
  232.     data_write[0] = ((reg >> 8) & 0xff);
  233.     data_write[1] = (reg & 0xff);
  234.     data_write[2] = value;
  235.     HAL_I2C_Master_Transmit(&hi2c1, lidar->address, (uint8_t*)data_write, 3, 1000);
  236.     lidar->last_status = 0;
  237.  
  238. }
  239.  
  240. void writeReg16Bit(uint16_t reg, uint16_t value)        {
  241.     char data_write[4];
  242.     data_write[0] = ((reg >> 8) & 0xff);
  243.     data_write[1] = (reg & 0xff);
  244.     data_write[2] = ((value >> 8) & 0xff);
  245.     data_write[3] = (value & 0xff);
  246.     HAL_I2C_Master_Transmit(&hi2c1, lidar->address, (uint8_t*)data_write, 4, 1000);
  247.     lidar->last_status = 0;
  248. }
  249.  
  250. void writeReg32Bit(uint16_t reg, uint32_t value)        {
  251.     char data_write[6];
  252.     data_write[0] = ((reg >> 8) & 0xff);
  253.     data_write[1] = (reg & 0xff);
  254.     data_write[2] = ((value >> 24) & 0xff);
  255.     data_write[3] = ((value >> 16) & 0xff);
  256.     data_write[4] = ((value >> 8) & 0xff);
  257.     data_write[5] = (value & 0xff);
  258.     HAL_I2C_Master_Transmit(&hi2c1, lidar->address, (uint8_t*)data_write, 6, 1000);
  259.     lidar->last_status = 0;
  260. }
  261.  
  262. uint8_t readReg(uint16_t reg)           {
  263.     uint8_t value;
  264.     char data_write[2];
  265.     char data_read[1];
  266.  
  267.     data_write[0] = ((reg >> 8) & 0xff);
  268.     data_write[1] = (reg & 0xff);
  269.  
  270.     HAL_I2C_Master_Transmit(&hi2c1, lidar->address, (uint8_t*)data_write, 2, 1000);
  271.     HAL_I2C_Master_Receive(&hi2c1, lidar->address, (uint8_t*)data_read, 1, 1000);
  272.  
  273.     value = data_read[0];
  274.     lidar->last_status = 0;
  275.  
  276.     return value;
  277. }
  278.  
  279. uint16_t readReg16Bit(uint16_t reg)         {
  280.     uint16_t value;
  281.     char data_write[2];
  282.     char data_read[2];
  283.  
  284.     data_write[0] = ((reg >> 8) & 0xff);
  285.     data_write[1] = (reg & 0xff);
  286.  
  287.     HAL_I2C_Master_Transmit(&hi2c1, lidar->address, (uint8_t*)data_write, 2, 1000);
  288.     HAL_I2C_Master_Receive(&hi2c1, lidar->address, (uint8_t*)data_read, 2, 1000);
  289.  
  290.     value = (uint16_t)data_read[0] << 8;
  291.     value |= data_read[1];
  292.     lidar->last_status = 0;
  293.  
  294.     return value;
  295. }
  296.  
  297. uint32_t readReg32Bit(uint16_t reg)         {
  298.     uint32_t value;
  299.     char data_write[2];
  300.     char data_read[4];
  301.  
  302.     data_write[0] = ((reg >> 8) & 0xff);
  303.     data_write[1] = (reg & 0xff);
  304.  
  305.     HAL_I2C_Master_Transmit(&hi2c1, lidar->address, (uint8_t*)data_write, 2, 1000);
  306.     HAL_I2C_Master_Receive(&hi2c1, lidar->address, (uint8_t*)data_read, 4, 1000);
  307.  
  308.     value  = (uint32_t)data_read[0] << 24;
  309.     value |= (uint32_t)data_read[1] << 16;
  310.     value |= (uint16_t)data_read[2] << 8;
  311.     value |= data_read[3];
  312.     lidar->last_status = 0;
  313.  
  314.     return value;
  315. }
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!