Subversion Repositories ChibiGauge

Rev

Rev 4 | Go to most recent revision | 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 rangingTest(BaseSequentialStream *chp,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.     if(Status == VL53L0X_ERROR_NONE)
  45.     {
  46.         chprintf(chp,"Call of VL53L0X_StaticInit\r\n");
  47.         Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
  48.         print_pal_error(chp,Status);
  49.     }
  50.  
  51.     if(Status == VL53L0X_ERROR_NONE)
  52.     {
  53.         chprintf(chp,"Call of VL53L0X_PerformRefCalibration\r\n");
  54.         Status = VL53L0X_PerformRefCalibration(pMyDevice,
  55.                         &VhvSettings, &PhaseCal); // Device Initialization
  56.         print_pal_error(chp,Status);
  57.     }
  58.  
  59.     if(Status == VL53L0X_ERROR_NONE)
  60.     {
  61.         chprintf(chp,"Call of VL53L0X_PerformRefSpadManagement\r\n");
  62.         Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
  63.                         &refSpadCount, &isApertureSpads); // Device Initialization
  64.         chprintf(chp,"refSpadCount = %d, isApertureSpads = %d\r\n", refSpadCount, isApertureSpads);
  65.         print_pal_error(chp,Status);
  66.     }
  67.  
  68.     if(Status == VL53L0X_ERROR_NONE)
  69.     {
  70.  
  71.         // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement
  72.         chprintf(chp,"Call of VL53L0X_SetDeviceMode\r\n");
  73.         Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
  74.         print_pal_error(chp,Status);
  75.     }
  76.  
  77.     // Enable/Disable Sigma and Signal check
  78.  
  79.  /*   if (Status == VL53L0X_ERROR_NONE) {
  80.         Status = VL53L0X_SetSequenceStepEnable(pMyDevice,VL53L0X_SEQUENCESTEP_DSS, 1);
  81.     }*/
  82.  
  83.     if (Status == VL53L0X_ERROR_NONE) {
  84.         Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
  85.                         VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1);
  86.     }
  87.     if (Status == VL53L0X_ERROR_NONE) {
  88.         Status = VL53L0X_SetLimitCheckEnable(pMyDevice,
  89.                         VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1);
  90.     }
  91.  
  92.     if (Status == VL53L0X_ERROR_NONE) {
  93.         Status = VL53L0X_SetLimitCheckValue(pMyDevice,
  94.                         VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE,
  95.                         (FixPoint1616_t)(0.1*65536));
  96.         }
  97.     if (Status == VL53L0X_ERROR_NONE) {
  98.         Status = VL53L0X_SetLimitCheckValue(pMyDevice,
  99.                         VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE,
  100.                         (FixPoint1616_t)(60*65536));
  101.     }
  102.     if (Status == VL53L0X_ERROR_NONE) {
  103.         Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice,
  104.                         33000);
  105.         }
  106.  
  107.     if (Status == VL53L0X_ERROR_NONE) {
  108.         Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
  109.                         VL53L0X_VCSEL_PERIOD_PRE_RANGE, 18);
  110.     }
  111.     if (Status == VL53L0X_ERROR_NONE) {
  112.         Status = VL53L0X_SetVcselPulsePeriod(pMyDevice,
  113.                         VL53L0X_VCSEL_PERIOD_FINAL_RANGE, 14);
  114.     }
  115.  
  116.  
  117.     /*
  118.      *  Step  4 : Test ranging mode
  119.      */
  120.  
  121.     if(Status == VL53L0X_ERROR_NONE)
  122.     {
  123.         for(i=0;i<50;i++){
  124.             chprintf(chp,"Call of VL53L0X_PerformSingleRangingMeasurement\r\n");
  125.             Status = VL53L0X_PerformSingleRangingMeasurement(pMyDevice,
  126.                         &RangingMeasurementData);
  127.  
  128.             print_pal_error(chp,Status);
  129.             print_range_status(chp,&RangingMeasurementData);
  130.  
  131.  
  132.             if (Status != VL53L0X_ERROR_NONE) break;
  133.  
  134.             chprintf(chp,"Measured distance: %i\r\n\r\n", RangingMeasurementData.RangeMilliMeter);
  135.  
  136.  
  137.         }
  138.     }
  139.     return Status;
  140. }
  141. // make this into a shell command
  142.  
  143. void shellLidar(BaseSequentialStream *chp, int argc, char *argv[])
  144. {
  145.     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
  146.     VL53L0X_Dev_t MyDevice;
  147.     VL53L0X_Dev_t *pMyDevice = &MyDevice;
  148.     VL53L0X_Version_t                   Version;
  149.     VL53L0X_Version_t                  *pVersion   = &Version;
  150.     VL53L0X_DeviceInfo_t                DeviceInfo;
  151.  
  152.     int32_t status_int;
  153.     int32_t init_done = 0;
  154.     // TCHAR SerialCommStr[MAX_VALUE_NAME];
  155.  
  156.  
  157.  
  158.     chprintf(chp,"VL53L0X API Simple Ranging example\r\n\r\n");
  159. //    chprintf(chp,"Press a Key to continue!\r\n\r\n");
  160. //    getchar();
  161.  
  162.  
  163.     // Initialize Comms
  164.     pMyDevice->I2cDevAddr      = 0x29; // chibi-style
  165.     pMyDevice->comms_type      =  1;
  166.     pMyDevice->comms_speed_khz =  400;
  167.  
  168.  
  169.     /*
  170.      * Disable VL53L0X API logging if you want to run at full speed
  171.      */
  172. #ifdef VL53L0X_LOG_ENABLE
  173.     VL53L0X_trace_config("test.log", TRACE_MODULE_ALL, TRACE_LEVEL_ALL, TRACE_FUNCTION_ALL);
  174. #endif
  175.  
  176.     /*
  177.      *  Get the version of the VL53L0X API running in the firmware
  178.      */
  179.  
  180.     if(Status == VL53L0X_ERROR_NONE)
  181.     {
  182.         status_int = VL53L0X_GetVersion(pVersion);
  183.         if (status_int != 0)
  184.             Status = VL53L0X_ERROR_CONTROL_INTERFACE;
  185.     }
  186.  
  187.     /*
  188.      *  Verify the version of the VL53L0X API running in the firmware
  189.      */
  190.  
  191.     if(Status == VL53L0X_ERROR_NONE)
  192.     {
  193.         if( pVersion->major != VERSION_REQUIRED_MAJOR ||
  194.             pVersion->minor != VERSION_REQUIRED_MINOR ||
  195.             pVersion->build != VERSION_REQUIRED_BUILD )
  196.         {
  197.             chprintf(chp,"VL53L0X API Version Error: Your firmware has %d.%d.%d (revision %d). This example requires %d.%d.%d.\r\n",
  198.                 pVersion->major, pVersion->minor, pVersion->build, pVersion->revision,
  199.                 VERSION_REQUIRED_MAJOR, VERSION_REQUIRED_MINOR, VERSION_REQUIRED_BUILD);
  200.         }
  201.     }
  202.  
  203.  
  204.     if(Status == VL53L0X_ERROR_NONE)
  205.     {
  206.         chprintf(chp,"Call of VL53L0X_DataInit\r\n");
  207.         Status = VL53L0X_DataInit(&MyDevice); // Data initialization
  208.         print_pal_error(chp,Status);
  209.     }
  210.  
  211.     if(Status == VL53L0X_ERROR_NONE)
  212.     {
  213.         Status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
  214.         if(Status == VL53L0X_ERROR_NONE)
  215.         {
  216.             chprintf(chp,"VL53L0X_GetDeviceInfo:\r\n");
  217.             chprintf(chp,"Device Name : %s\r\n", DeviceInfo.Name);
  218.             chprintf(chp,"Device Type : %s\r\n", DeviceInfo.Type);
  219.             chprintf(chp,"Device ID : %s\r\n", DeviceInfo.ProductId);
  220.             chprintf(chp,"ProductRevisionMajor : %d\r\n", DeviceInfo.ProductRevisionMajor);
  221.         chprintf(chp,"ProductRevisionMinor : %d\r\n", DeviceInfo.ProductRevisionMinor);
  222.  
  223.         if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) {
  224.                 chprintf(chp,"Error expected cut 1.1 but found cut %d.%d\r\n",
  225.                        DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor);
  226.                 Status = VL53L0X_ERROR_NOT_SUPPORTED;
  227.             }
  228.         }
  229.         print_pal_error(chp,Status);
  230.     }
  231.  
  232.     if(Status == VL53L0X_ERROR_NONE)
  233.     {
  234.         Status = rangingTest(chp,pMyDevice);
  235.     }
  236.  
  237.     print_pal_error(chp,Status);
  238.  
  239.     // Implementation specific
  240.  
  241.     /*
  242.      *  Disconnect comms - part of VL53L0X_platform.c
  243.      */
  244.  
  245.     if(init_done == 0)
  246.     {
  247.         chprintf(chp,"Close Comms\r\n");
  248.         status_int = VL53L0X_comms_close();
  249.         if (status_int != 0)
  250.             Status = VL53L0X_ERROR_CONTROL_INTERFACE;
  251.     }
  252.  
  253.     print_pal_error(chp,Status);
  254.  
  255. //    chprintf(chp,"\r\nPress a Key to continue!");
  256. //    getchar();
  257.  
  258.     return (0);
  259. }
  260.  
  261.