Subversion Repositories ChibiGauge

Rev

Rev 4 | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1. #include "vl53l0x_api.h"
  2. #include "vl53l0x_platform.h"
  3. #include "required_version.h"
  4. #include <malloc.h>
  5.  
  6. #include "useLidar.h"
  7. #include "ch.h"
  8. #include "hal.h"
  9. #include "shell.h"
  10. #include "chprintf.h"
  11.  
  12. void print_pal_error(BaseSequentialStream *chp, VL53L0X_Error Status){
  13.     char buf[VL53L0X_MAX_STRING_LENGTH];
  14.     VL53L0X_GetPalErrorString(Status, buf);
  15.     chprintf(chp,"API Status: %i : %s\r\n", Status, buf);
  16. }
  17.  
  18. void print_range_status(BaseSequentialStream *chp,VL53L0X_RangingMeasurementData_t* pRangingMeasurementData){
  19.     char buf[VL53L0X_MAX_STRING_LENGTH];
  20.     uint8_t RangeStatus;
  21.  
  22.     /*
  23.      * New Range Status: data is valid when pRangingMeasurementData->RangeStatus = 0
  24.      */
  25.  
  26.     RangeStatus = pRangingMeasurementData->RangeStatus;
  27.  
  28.     VL53L0X_GetRangeStatusString(RangeStatus, buf);
  29.     chprintf(chp,"Range Status: %i : %s\r\n", RangeStatus, buf);
  30.  
  31. }
  32.  
  33.  
  34. VL53L0X_Error VL53L0XdeviceSetup(VL53L0X_Dev_t *pMyDevice)
  35. {
  36. VL53L0X_Error Status = VL53L0X_ERROR_NONE;
  37. VL53L0X_RangingMeasurementData_t    RangingMeasurementData;
  38. int i;
  39. uint32_t refSpadCount;
  40. uint8_t isApertureSpads;
  41. uint8_t VhvSettings;
  42. uint8_t PhaseCal;
  43.  
  44. // Initialize Comms
  45. pMyDevice->I2cDevAddr      = 0x29; // chibi-style
  46. pMyDevice->comms_type      =  1;
  47. pMyDevice->comms_speed_khz = 100;
  48.  
  49. // call init anyway
  50. VL53L0X_comms_initialise(0,0);
  51.  
  52.  
  53. if(Status == VL53L0X_ERROR_NONE)
  54. {
  55.     Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
  56. }
  57.  
  58. if(Status == VL53L0X_ERROR_NONE)
  59. {
  60.     Status = VL53L0X_PerformRefCalibration(pMyDevice,
  61.                 &VhvSettings, &PhaseCal); // Device Initialization
  62. }
  63.  
  64. if(Status == VL53L0X_ERROR_NONE)
  65. {
  66.     Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
  67.                 &refSpadCount, &isApertureSpads); // Device Initialization
  68. }
  69.  
  70. if(Status == VL53L0X_ERROR_NONE)
  71. {
  72.     // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
  73.     Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
  74. }
  75.  
  76. // Enable/Disable Sigma and Signal check
  77.  
  78. /*   if (Status == VL53L0X_ERROR_NONE) {
  79.     Status = VL53L0X_SetSequenceStepEnable(pMyDevice,VL53L0X_SEQUENCESTEP_DSS, 1);
  80. }*/
  81.  
  82. if (Status == VL53L0X_ERROR_NONE) {
  83.     Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
  84.                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
  85. }
  86. if (Status == VL53L0X_ERROR_NONE) {
  87.     Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
  88.                 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
  89. }
  90.  
  91. if (Status == VL53L0X_ERROR_NONE) {
  92.     Status = VL53L0X_SetLimitCheckValue(pMyDevice,
  93.                 VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
  94.                 (FixPoint1616_t)(0.1*65536));
  95. }
  96. if (Status == VL53L0X_ERROR_NONE) {
  97.     Status = VL53L0X_SetLimitCheckValue(pMyDevice,
  98.                 VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
  99.                 (FixPoint1616_t)(60*65536));
  100. }
  101. if (Status == VL53L0X_ERROR_NONE) {
  102.     Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice,
  103.                 33000);
  104. }
  105.  
  106. if (Status == VL53L0X_ERROR_NONE) {
  107.     Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
  108.                 VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18);
  109. }
  110. if (Status == VL53L0X_ERROR_NONE) {
  111.     Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
  112.                 VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14);
  113. }
  114. return Status;
  115.  
  116. }
  117.  
  118.  
  119.  
  120.  
  121. VL53L0X_Error rangingTest(BaseSequentialStream *chp,VL53L0X_Dev_t *pMyDevice)
  122. {
  123.     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
  124.     VL53L0X_RangingMeasurementData_t    RangingMeasurementData;
  125.     int i;
  126.     uint32_t refSpadCount;
  127.     uint8_t isApertureSpads;
  128.     uint8_t VhvSettings;
  129.     uint8_t PhaseCal;
  130.  
  131.     if(Status == VL53L0X_ERROR_NONE)
  132.     {
  133.         chprintf(chp,"Call of VL53L0X_StaticInit\r\n");
  134.         Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
  135.         print_pal_error(chp,Status);
  136.     }
  137.  
  138.     if(Status == VL53L0X_ERROR_NONE)
  139.     {
  140.         chprintf(chp,"Call of VL53L0X_PerformRefCalibration\r\n");
  141.         Status = VL53L0X_PerformRefCalibration(pMyDevice,
  142.                         &VhvSettings, &PhaseCal); // Device Initialization
  143.         print_pal_error(chp,Status);
  144.     }
  145.  
  146.     if(Status == VL53L0X_ERROR_NONE)
  147.     {
  148.         chprintf(chp,"Call of VL53L0X_PerformRefSpadManagement\r\n");
  149.         Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
  150.                         &refSpadCount, &isApertureSpads); // Device Initialization
  151.         chprintf(chp,"refSpadCount = %d, isApertureSpads = %d\r\n", refSpadCount, isApertureSpads);
  152.         print_pal_error(chp,Status);
  153.     }
  154.  
  155.     if(Status == VL53L0X_ERROR_NONE)
  156.     {
  157.  
  158.         // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
  159.         chprintf(chp,"Call of VL53L0X_SetDeviceMode\r\n");
  160.         Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
  161.         print_pal_error(chp,Status);
  162.     }
  163.  
  164.     // Enable/Disable Sigma and Signal check
  165.  
  166.  /*   if (Status == VL53L0X_ERROR_NONE) {
  167.         Status = VL53L0X_SetSequenceStepEnable(pMyDevice,VL53L0X_SEQUENCESTEP_DSS, 1);
  168.     }*/
  169.  
  170.     if (Status == VL53L0X_ERROR_NONE) {
  171.         Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
  172.                         VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
  173.     }
  174.     if (Status == VL53L0X_ERROR_NONE) {
  175.         Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
  176.                         VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
  177.     }
  178.  
  179.     if (Status == VL53L0X_ERROR_NONE) {
  180.         Status = VL53L0X_SetLimitCheckValue(pMyDevice,
  181.                         VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
  182.                         (FixPoint1616_t)(0.1*65536));
  183.         }
  184.     if (Status == VL53L0X_ERROR_NONE) {
  185.         Status = VL53L0X_SetLimitCheckValue(pMyDevice,
  186.                         VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
  187.                         (FixPoint1616_t)(60*65536));
  188.     }
  189.     if (Status == VL53L0X_ERROR_NONE) {
  190.         Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice,
  191.                         33000);
  192.         }
  193.  
  194.     if (Status == VL53L0X_ERROR_NONE) {
  195.         Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
  196.                         VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18);
  197.     }
  198.     if (Status == VL53L0X_ERROR_NONE) {
  199.         Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
  200.                         VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14);
  201.     }
  202.  
  203.  
  204.     /*
  205.      *  Step  4 : Test ranging mode
  206.      */
  207.  
  208.     if(Status == VL53L0X_ERROR_NONE)
  209.     {
  210.         for(i=0;i<50;i++){
  211.             chprintf(chp,"Call of VL53L0X_PerformSingleRangingMeasurement\r\n");
  212.             Status = VL53L0X_PerformSingleRangingMeasurement(pMyDevice,
  213.                         &RangingMeasurementData);
  214.  
  215.             print_pal_error(chp,Status);
  216.             print_range_status(chp,&RangingMeasurementData);
  217.  
  218.  
  219.             if (Status != VL53L0X_ERROR_NONE) break;
  220.  
  221.             chprintf(chp,"Measured distance: %i\r\n\r\n", RangingMeasurementData.RangeMilliMeter);
  222.  
  223.  
  224.         }
  225.     }
  226.     return Status;
  227. }
  228. // make this into a shell command
  229. extern void setPause(bool pause);
  230.  
  231. void shellLidar(BaseSequentialStream *chp, int argc, char *argv[])
  232. {
  233.     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
  234.     VL53L0X_Dev_t MyDevice;
  235.     VL53L0X_Dev_t *pMyDevice = &MyDevice;
  236.     VL53L0X_Version_t                   Version;
  237.     VL53L0X_Version_t                  *pVersion   = &Version;
  238.     VL53L0X_DeviceInfo_t                DeviceInfo;
  239.  
  240.     int32_t status_int;
  241.     int32_t init_done = 0;
  242.     // TCHAR SerialCommStr[MAX_VALUE_NAME];
  243.  
  244.     setPause(true);
  245.  
  246.     chprintf(chp,"VL53L0X API Simple Ranging example\r\n\r\n");
  247. //    chprintf(chp,"Press a Key to continue!\r\n\r\n");
  248. //    getchar();
  249.  
  250.  
  251.     // Initialize Comms
  252.     pMyDevice->I2cDevAddr      = 0x29; // chibi-style
  253.     pMyDevice->comms_type      =  1;
  254.     pMyDevice->comms_speed_khz = 100;
  255.  
  256.     // call init anyway
  257.     VL53L0X_comms_initialise(0,0);
  258.     /*
  259.      * Disable VL53L0X API logging if you want to run at full speed
  260.      */
  261. #ifdef VL53L0X_LOG_ENABLE
  262.     VL53L0X_trace_config("test.log", TRACE_MODULE_ALL, TRACE_LEVEL_ALL, TRACE_FUNCTION_ALL);
  263. #endif
  264.  
  265.     /*
  266.      *  Get the version of the VL53L0X API running in the firmware
  267.      */
  268.  
  269.     if(Status == VL53L0X_ERROR_NONE)
  270.     {
  271.         status_int = VL53L0X_GetVersion(pVersion);
  272.         if (status_int != 0)
  273.             Status = VL53L0X_ERROR_CONTROL_INTERFACE;
  274.     }
  275.  
  276.     /*
  277.      *  Verify the version of the VL53L0X API running in the firmware
  278.      */
  279.  
  280.     if(Status == VL53L0X_ERROR_NONE)
  281.     {
  282.         if( pVersion->major != VERSION_REQUIRED_MAJOR ||
  283.             pVersion->minor != VERSION_REQUIRED_MINOR ||
  284.             pVersion->build != VERSION_REQUIRED_BUILD )
  285.         {
  286.             chprintf(chp,"VL53L0X API Version Error: Your firmware has %d.%d.%d (revision %d). This example requires %d.%d.%d.\r\n",
  287.                 pVersion->major, pVersion->minor, pVersion->build, pVersion->revision,
  288.                 VERSION_REQUIRED_MAJOR, VERSION_REQUIRED_MINOR, VERSION_REQUIRED_BUILD);
  289.         }
  290.     }
  291.  
  292.  
  293.     if(Status == VL53L0X_ERROR_NONE)
  294.     {
  295.         chprintf(chp,"Call of VL53L0X_DataInit\r\n");
  296.         Status = VL53L0X_DataInit(&MyDevice); // Data initialization
  297.         print_pal_error(chp,Status);
  298.     }
  299.  
  300.     if(Status == VL53L0X_ERROR_NONE)
  301.     {
  302.         Status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
  303.         if(Status == VL53L0X_ERROR_NONE)
  304.         {
  305.             chprintf(chp,"VL53L0X_GetDeviceInfo:\r\n");
  306.             chprintf(chp,"Device Name : %s\r\n", DeviceInfo.Name);
  307.             chprintf(chp,"Device Type : %s\r\n", DeviceInfo.Type);
  308.             chprintf(chp,"Device ID : %s\r\n", DeviceInfo.ProductId);
  309.             chprintf(chp,"ProductRevisionMajor : %d\r\n", DeviceInfo.ProductRevisionMajor);
  310.         chprintf(chp,"ProductRevisionMinor : %d\r\n", DeviceInfo.ProductRevisionMinor);
  311.  
  312.         if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) {
  313.                 chprintf(chp,"Error expected cut 1.1 but found cut %d.%d\r\n",
  314.                        DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor);
  315.                 Status = VL53L0X_ERROR_NOT_SUPPORTED;
  316.             }
  317.         }
  318.         print_pal_error(chp,Status);
  319.     }
  320.  
  321.     if(Status == VL53L0X_ERROR_NONE)
  322.     {
  323.         Status = rangingTest(chp,pMyDevice);
  324.     }
  325.  
  326.     print_pal_error(chp,Status);
  327.  
  328.     // Implementation specific
  329.  
  330.     /*
  331.      *  Disconnect comms - part of VL53L0X_platform.c
  332.      */
  333.  
  334.     if(init_done == 0)
  335.     {
  336.         chprintf(chp,"Close Comms\r\n");
  337.         status_int = VL53L0X_comms_close();
  338.         if (status_int != 0)
  339.             Status = VL53L0X_ERROR_CONTROL_INTERFACE;
  340.     }
  341.  
  342.     print_pal_error(chp,Status);
  343.  
  344. //    chprintf(chp,"\r\nPress a Key to continue!");
  345. //    getchar();
  346.     setPause(false);
  347.  
  348.     return (0);
  349. }
  350.  
  351.