Diff: main.cpp

Revision:

0:020912dfa221

Child:

1:ff48a20de191

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Nov 08 15:35:55 2020 +0000
@@ -0,0 +1,371 @@
+/*
+ *  This VL53L1X Expansion board test application performs range measurements
+ *  using the onboard embedded centre sensor and two satelites, in autonomous, interrupt mode.
+ *  Measured ranges are ouput on the Serial Port, running at 115200 baud.
+ *
+*
+ * This is designed to work with MBed V2 , MBed V5 and MBed V6.
+ *
+ *
+ *  The Reset button can be used to restart the program.
+ */
+ 
+#include <stdio.h>
+
+#include "mbed.h"
+#include "XNucleo53L1A1.h"
+#include "ToF_I2C.h"
+#include <time.h>
+
+// i2c comms port pins
+#define I2C_SDA   D14 
+#define I2C_SCL   D15  
+
+
+#define NUM_SENSORS 3
+
+PinName CentreIntPin = A2;
+// the satellite pins depend on solder blobs on the back of the shield.
+// they may not exist or may be one of two sets.
+// the centre pin always exists
+PinName LeftIntPin = D9;
+PinName RightIntPin = D4;
+// alternate set
+//PinName LeftIntPin = D8;
+//PinName RightIntPin = D2;
+
+
+
+static XNucleo53L1A1 *board=NULL;
+
+// MBed V6.4 has renamed wait_ms and UnbufferedSerial replaces Serial
+#if (MBED_VERSION  > 60300) 
+UnbufferedSerial  pc(SERIAL_TX, SERIAL_RX); 
+extern "C" void wait_ms(int ms);
+#else
+Serial pc(SERIAL_TX, SERIAL_RX); 
+#endif
+
+// flags to indicate an interrupt has happened
+static int int_centre_result = 0;
+static int int_left_result = 0;
+static int int_right_result = 0;
+
+// flags to indicate an interrupt has cleared
+static int int_centre_dropped = 0;
+static int int_left_dropped = 0;
+static int int_right_dropped = 0;
+
+void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData );
+
+class WaitForMeasurement {
+public:
+
+
+// this class services the interrupts from the ToF sensors.
+// There is a limited amount you can do in an interrupt routine; printfs,mutexes break them among other things.
+// We keep things simple by only raising a flag so all the real work is done outside the interrupt.
+// This is designed around MBED V2 which doesn't have the RTOS features that would make this work nicely e.g. semaphores/queues.
+WaitForMeasurement(): _interrupt(A1)
+{
+}
+
+
+    // constructor - Sensor is not used and can be removed
+    WaitForMeasurement(PinName pin,VL53L1_DEV Dev) : _interrupt(pin)          // create the InterruptIn on the pin specified to Counter
+    {
+         Devlocal = Dev;
+         pinlocal = pin;
+         
+         #include "mbed.h"
+ 
+
+        _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // if interrupt happens read data
+        _interrupt.fall(callback(this, &WaitForMeasurement::linedropped)); // if interupt clears, clear interrupt
+        
+    }
+    
+      // function is called every time an interupt is cleared. Sets flags to clear the interrupt 
+    void linedropped()
+    {
+        
+        if (Devlocal->i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS)
+                int_centre_dropped = 1;  //flag to main that interrupt cleared. A flag is raised which allows the main routine to service interupt.
+        if (Devlocal->i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS)
+                int_left_dropped = 1;   //flag to main that interrupt cleared
+        if (Devlocal->i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS)
+                int_right_dropped = 1;  //flag to main that interrupt cleared
+        
+    }
+
+  // function is called every time an interupt is seen. A flag is raised which allows the main routine to service the interupt.
+    void got_interrupt()
+    {
+        DigitalIn intp(pinlocal);
+
+        if (Devlocal->i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS)
+                int_centre_result = 1;  //flag to main that interrupt happened
+        if (Devlocal->i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS)
+                int_left_result = 1;   //flag to main that interrupt happened7
+        if (Devlocal->i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS)
+                int_right_result = 1;  //flag to main that interrupt happened
+    }
+
+    
+    //destructor
+    ~WaitForMeasurement()
+    {
+        printf("WaitForMeasurement destruction \n");
+    }
+
+private:
+    InterruptIn _interrupt;
+    PinName pinlocal;
+    VL53L1_DEV Devlocal;
+    int status;
+    
+};
+
+
+
+VL53L1_Dev_t                   devCentre;
+VL53L1_Dev_t                   devLeft;
+VL53L1_Dev_t                   devRight;
+VL53L1_DEV                     Dev = &devCentre;
+
+ 
+/*=================================== Main ==================================
+=============================================================================*/
+int main()
+{   
+    int status;
+    VL53L1X * Sensor;
+    uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right
+  
+    //mbed compiler claims these are never used but they are.
+    WaitForMeasurement* int2;
+    WaitForMeasurement* int1;
+    WaitForMeasurement* int3;
+    
+    
+    pc.baud(115200);  // baud rate is important as printf statements take a lot of time
+    
+    printf("Autonomous Interruptmbed = %d \r\n",MBED_VERSION);
+
+// create i2c interface
+    ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL);
+    
+    dev_I2C->frequency(400000); //also needs doing in spi_interface.c
+    
+    /* creates the 53L1A1 expansion board singleton obj */
+   board = XNucleo53L1A1::instance(dev_I2C, CentreIntPin, LeftIntPin, RightIntPin);
+    
+    printf("board created!\r\n");
+
+    /* init the 53L1A1 expansion board with default values */
+    status = board->init_board();
+    if (status) {
+        printf("Failed to init board!\r\n");
+        return 0;
+    }
+       
+        
+    printf("board initiated! - %d\r\n", status);
+  // create sensor controller classes for each vl53l1 and configure each vl53l1                                            
+        for (ToFSensor=0;ToFSensor < NUM_SENSORS;ToFSensor++){
+            switch(ToFSensor){
+                case 0:
+                    if (board->sensor_centre== NULL ) continue;  // don't create if sensor not detected
+                    Dev=&devCentre;
+                    Sensor=board->sensor_centre;
+                    Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
+                    printf("configuring centre channel \n");
+                    break;
+                case 1:
+                    if (board->sensor_left== NULL ) continue;
+                    Dev=&devLeft; 
+                    Sensor=board->sensor_left;
+                    Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
+                    printf("configuring left channel \n");
+                    break;
+                case 2:
+                    if (board->sensor_right== NULL ) continue;
+                    Dev=&devRight;  
+                    Sensor=board->sensor_right;
+                    Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
+                    printf("configuring right channel \n");
+                    break;      
+                default:
+                   printf(" error in switch, invalid ToF sensor \n");
+            }
+        
+            // configure the sensors
+            Dev->comms_speed_khz = 400;
+    
+            Dev->comms_type = 1;
+    
+           /* Device Initialization and setting */  
+            status = Sensor->vl53L1_DataInit();
+            status = Sensor->vl53L1_StaticInit();
+            status = Sensor->vl53L1_SetPresetMode(VL53L1_PRESETMODE_AUTONOMOUS);
+            status = Sensor->vl53L1_SetDistanceMode(VL53L1_DISTANCEMODE_LONG);
+            status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 200 * 1000); 
+            
+   
+           // set the ranging and signal rate filter
+            VL53L1_DetectionConfig_t thresholdconfig;
+            thresholdconfig.DetectionMode = VL53L1_DETECTION_DISTANCE_ONLY; /// type VL53L1_DetectionMode in vl53l1_def.h
+            thresholdconfig.Distance.CrossMode = VL53L1_THRESHOLD_IN_WINDOW; // type VL53L1_ThresholdMode. ignore if distance outside high and low 
+            thresholdconfig.Distance.High = 300;  // high distance in mm
+            thresholdconfig.Distance.Low = 200;   // low distance in mm
+            thresholdconfig.Rate.CrossMode=0;  // type VL53L1_ThresholdMode VL53L1_THRESHOLD_CROSSED_LOW VL53L1_THRESHOLD_CROSSED_HIGH VL53L1_THRESHOLD_OUT_OF_WINDOW VL53L1_THRESHOLD_IN_WINDOW 
+            thresholdconfig.Rate.High = 0;
+            thresholdconfig.Rate.Low = 0;
+            thresholdconfig.IntrNoTarget = 0 ;// if 1 produce an interrupt even if there is no target found e.g out of range 
+            status = Sensor->vl53L1_SetThresholdConfig(&thresholdconfig);
+        }
+        
+        // create interrupt handlers for the three sensors and start measurements
+        if (board->sensor_centre!= NULL )
+        {
+            printf("starting interrupt centre\n");
+            devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS;
+            int1 =  new WaitForMeasurement(CentreIntPin,&devCentre);    // create interrupt handler
+            status = board->sensor_centre->vl53L1_StartMeasurement();
+        }
+        
+        if (board->sensor_left!= NULL )
+        {
+            printf("starting interrupt left\n");
+            devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS;
+            int2 = new WaitForMeasurement(LeftIntPin,&devLeft);    // create interrupt handler
+            status = board->sensor_left->vl53L1_StartMeasurement();
+            printf("started interrupt left\n");
+        }
+
+        if (board->sensor_right!= NULL )
+        {
+            printf("starting interrupt right\n");
+            devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS;
+            int3 = new WaitForMeasurement(RightIntPin,&devRight);    // create interrupt handler
+            status = board->sensor_right->vl53L1_StartMeasurement();
+        }
+        
+        
+        printf("loop forever\n"); 
+
+       // loop waiting for interrupts to happen. This is signaled by int_centre_result,int_left_result or int_right_result
+       // being non zero. When the interrupts clear this is signaled by int_centre_dropped,int_left_dropped and int_right_dropped. 
+       // These are set back to zero when processing is completed
+        while (1)
+        {
+            VL53L1_MultiRangingData_t MultiRangingData;
+            VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData;   
+
+            if ( int_left_dropped  || int_centre_dropped  || int_right_dropped )
+                wait_ms(30);  
+            
+            // when the interrupt pin goes loww start new measurement
+            if ( int_centre_dropped != 0)
+            {
+               int_centre_dropped = 0;
+               status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement();
+            }
+            
+            if ( int_left_dropped != 0)               
+            {
+               int_left_dropped = 0;
+               status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement();
+            }
+            
+            if ( int_right_dropped != 0)            
+            {
+               int_right_dropped = 0;
+               status = board->sensor_right->vl53L1_ClearInterruptAndStartMeasurement();
+            }
+
+            if (int_right_result != 0) // interrupt seen on right sensor
+            {
+                status = board->sensor_right->vl53L1_GetMultiRangingData( pMultiRangingData);
+                if ( status == 0)
+                {
+                    print_results( devRight.i2c_slave_address, pMultiRangingData );
+                }
+
+                // clear interrupt flag
+                int_right_result = 0;
+            }
+
+            if (int_left_result != 0)  // interrupt seen on left sensor
+            {
+                status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData);
+                if ( status == 0)
+                {
+                    print_results( devLeft.i2c_slave_address, pMultiRangingData );
+                }
+
+                // clear interrupt flag
+                int_left_result = 0;
+            }
+              
+            if (int_centre_result != 0)
+            {
+                status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData);
+                if ( status == 0)
+                {
+                    print_results( devCentre.i2c_slave_address, pMultiRangingData );
+                }
+                
+                 // clear interrupt flag
+                int_centre_result = 0;
+
+            }
+            wait_ms( 1 * 10);
+                                  
+        }
+    }
+    
+    
+ // print what ever results are required   
+void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData )
+{
+            int no_of_object_found=pMultiRangingData->NumberOfObjectsFound;
+            
+            int RoiNumber=pMultiRangingData->RoiNumber;
+ //           int RoiStatus=pMultiRangingData->RoiStatus;
+
+            if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) 
+            {
+                for(int j=0;j<no_of_object_found;j++){
+                    if ((pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID) || 
+                        (pMultiRangingData->RangeData[j].RangeStatus == VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL))
+                    {
+                        printf("\t spiAddr=%d \t RoiNumber=%d  \t D=%5dmm \n",
+                                devSpiNumber,
+                                RoiNumber,
+                                pMultiRangingData->RangeData[j].RangeMilliMeter);
+                        /*
+                                                printf("\t spiAddr=%d \t RoiNumber=%d   \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n",
+                        devSpiNumber,
+                        RoiNumber,
+                        pMultiRangingData->RangeData[j].RangeStatus,
+                        pMultiRangingData->RangeData[j].RangeMilliMeter,
+                        pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0,
+                        pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0);
+                        */
+                    }
+
+                }
+            } // if (( no_of_object_found < 10 ) &&  ( no_of_object_found != 0)) 
+
+}    
+
+        
+#if (MBED_VERSION  > 60300)
+extern "C" void wait_ms(int ms)
+ {
+    thread_sleep_for(ms);
+ }
+ #endif 
+     
+