A sample program getting measurements from a VL53L1CB ToF sensors which are directly connected to the STM32F401. Copes with three sensors. Interrupt mode. VL53L1 is operated in ranging mode. MBed V2.
Dependencies: mbed X_NUCLEO_53L1CB
main.cpp
- Committer:
- charlesmn
- Date:
- 2021-01-19
- Revision:
- 1:02f2b9e04b46
- Parent:
- 0:2b5a44a0d2ec
File content as of revision 1:02f2b9e04b46:
/* * This VL53L1 test application performs range measurements * using the onboard embedded centre sensor and two satelites, in ranging, interrupt mode. * This program does not use the expansion board and the sensors a directly connected to the F401. * 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 pins #define I2C_SDA D14 #define I2C_SCL D15 // i2c addresses the sensors are configured to be #define NEW_SENSOR_CENTRE_ADDRESS 0x56 #define NEW_SENSOR_LEFT_ADDRESS 0x58 #define NEW_SENSOR_RIGHT_ADDRESS 0x5a #define NUM_SENSORS 3 // define the sensor interrupt pins. This must match the wiring! PinName CentreIntPin = A2; PinName LeftIntPin = A3; PinName RightIntPin = A4; // 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 static int int_centre_result = 0; static int int_left_result = 0; static int int_right_result = 0; 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 WaitForMeasurement(PinName pin, uint8_t slave_address) : _interrupt(pin) // create the InterruptIn on the pin specified { i2c_slave_address = slave_address; _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // attach routine to deal with interrupt } // 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() { // identify which sensor this instance relates to and flag an interrupt has occured if (i2c_slave_address == NEW_SENSOR_CENTRE_ADDRESS) int_centre_result = 1; //flag to main that interrupt happened if (i2c_slave_address == NEW_SENSOR_LEFT_ADDRESS) int_left_result = 1; //flag to main that interrupt happened if (i2c_slave_address == NEW_SENSOR_RIGHT_ADDRESS) int_right_result = 1; //flag to main that interrupt happened } //destructor ~WaitForMeasurement() { printf("destruction \n"); } private: InterruptIn _interrupt; int status; uint8_t i2c_slave_address; }; /*=================================== Main ================================== =============================================================================*/ int main() { int status; VL53L1X * Sensor; VL53L1X * SensorCentre; VL53L1X * SensorLeft; VL53L1X * SensorRight; DigitalOut xshutdownCentre(D9); DigitalOut xshutdownLeft(D4); DigitalOut xshutdownRight(D3); uint8_t ToFSensor = 1; WaitForMeasurement* int2; WaitForMeasurement* int1; WaitForMeasurement* int3; pc.baud(115200); // baud rate is important as printf statements take a lot of time printf("VL53L1CB_NoShield_3sensors_interrupt_ranging mbed = %d \r\n",MBED_VERSION); // create the i2c instance ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL); // create the sensor instances SensorCentre = new VL53L1X(dev_I2C, &xshutdownCentre, CentreIntPin); SensorLeft = new VL53L1X(dev_I2C, &xshutdownLeft, LeftIntPin); SensorRight = new VL53L1X(dev_I2C, &xshutdownRight, RightIntPin); printf("board created!\r\n"); // initialise sensors for (ToFSensor=0;ToFSensor< NUM_SENSORS ;ToFSensor++){ switch(ToFSensor){ case 0: if (SensorCentre == NULL ) continue; // try to initialise the sensor. returns -1 if sensor is not there. if (SensorCentre->InitSensor(NEW_SENSOR_CENTRE_ADDRESS) ==-1) { printf("centre not found \n"); SensorCentre = NULL; continue; } Sensor=SensorCentre; printf("configuring centre channel \n"); break; case 1: if (SensorLeft== NULL ) continue; if (SensorLeft->InitSensor(NEW_SENSOR_LEFT_ADDRESS)==-1) { printf("left not found \n"); SensorLeft = NULL; continue; } Sensor=SensorLeft; printf("configuring left channel \n"); break; case 2: if (SensorRight == NULL ) continue; if(SensorRight->InitSensor(NEW_SENSOR_RIGHT_ADDRESS)==-1) { printf("right not found \n"); SensorRight = NULL; continue; } Sensor=SensorRight; printf("configuring right channel \n"); break; default: printf(" error in switch, invalid ToF sensor \n"); } wait_ms(250); // wait for the sensor to come alive // configure the sensors /* Device Initialization and setting */ status = Sensor->vl53L1_DataInit(); status = Sensor->vl53L1_StaticInit(); status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_RANGING); status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG); status = Sensor->vl53L1_SetMeasurementTimingBudgetMicroSeconds( 50000); status = Sensor->vl53L1_SetInterMeasurementPeriodMilliSeconds( 100); } if (SensorCentre != NULL ) { printf("starting interrupt centre\n"); // create interrupt process for the centre sensor int1 = new WaitForMeasurement(CentreIntPin,NEW_SENSOR_CENTRE_ADDRESS); // start the snesor measuring status = SensorCentre->vl53L1_StartMeasurement(); printf("end interrupt centre\n"); } if (SensorLeft!= NULL ) { printf("starting interrupt left\n"); int2 = new WaitForMeasurement(LeftIntPin,NEW_SENSOR_LEFT_ADDRESS); status = SensorLeft->vl53L1_StartMeasurement(); } if (SensorRight!= NULL ) { printf("starting interrupt right\n"); int3 = new WaitForMeasurement(RightIntPin,NEW_SENSOR_RIGHT_ADDRESS); status = SensorRight->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. The are set back to zero when processing is completed while (1) { static VL53L1_RangingMeasurementData_t RangingData; VL53L1_MultiRangingData_t MultiRangingData; VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData; wait_ms( 1 * 50); if (int_centre_result != 0) { // as there has been an interrupt there is data waiting so get it status = SensorCentre->vl53L1_GetMultiRangingData( pMultiRangingData); if ( status == VL53L1_ERROR_NONE) { int no_of_object_found=pMultiRangingData->NumberOfObjectsFound; if ( no_of_object_found < 10 ) { 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("centre\t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n", pMultiRangingData->RangeData[j].RangeStatus, pMultiRangingData->RangeData[j].RangeMilliMeter, pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0, pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0); } } } } int_centre_result = 0; status = SensorCentre->vl53L1_ClearInterruptAndStartMeasurement(); } if (int_left_result != 0) { status = SensorLeft->vl53L1_GetMultiRangingData( pMultiRangingData); if ( status == VL53L1_ERROR_NONE) { int no_of_object_found=pMultiRangingData->NumberOfObjectsFound; if ( no_of_object_found < 10 ) { 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("left \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n", pMultiRangingData->RangeData[j].RangeStatus, pMultiRangingData->RangeData[j].RangeMilliMeter, pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0, pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0); } } } } int_left_result = 0; status = SensorLeft->vl53L1_ClearInterruptAndStartMeasurement(); } if (int_right_result != 0) { status = SensorRight->vl53L1_GetMultiRangingData( pMultiRangingData); if ( status == VL53L1_ERROR_NONE) { // if valid result print it int no_of_object_found=pMultiRangingData->NumberOfObjectsFound; if ( no_of_object_found < 10 ) { 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("right \t status=%d, \t D=%5dmm, \t Signal=%2.2f Mcps, \t Ambient=%2.2f Mcps \n", pMultiRangingData->RangeData[j].RangeStatus, pMultiRangingData->RangeData[j].RangeMilliMeter, pMultiRangingData->RangeData[j].SignalRateRtnMegaCps/65536.0, pMultiRangingData->RangeData[j].AmbientRateRtnMegaCps/65536.0); } } } } // clear interrupt flag int_right_result = 0; // clear theinterrupt and wait for another result status = SensorRight->vl53L1_ClearInterruptAndStartMeasurement(); } } printf("terminated"); } // this function doesn't exist in MBed6.4 onwards. It is required for the F401 drivers. // addded here as it allows the file to be compatible with all mbed versions #if (MBED_VERSION > 60300) void wait_ms(int ms) { thread_sleep_for(ms); } #endif