VL53L1CB ranging example, using expansion shield and satellites, with polling.
Dependencies: X_NUCLEO_53L1A2
Revision 11:96465baec780, committed 2021-05-18
- Comitter:
- johnAlexander
- Date:
- Tue May 18 08:36:02 2021 +0000
- Parent:
- 10:cf7d563200fc
- Commit message:
- forking, to share internally
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu May 13 09:17:02 2021 +0000 +++ b/main.cpp Tue May 18 08:36:02 2021 +0000 @@ -29,6 +29,18 @@ #define I2C_SDA D14 #define I2C_SCL D15 +#define NUM_SENSORS 3 + +// define interrupt pins +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 = D8; +PinName RightIntPin = D2; +// alternate set +PinName LeftIntPin = D9; +//PinName RightIntPin = D4; static XNucleo53L1A2 *board=NULL; @@ -39,6 +51,13 @@ Serial pc(SERIAL_TX, SERIAL_RX); #endif +#if TARGET_STM // we are cross compiling for an STM32-Nucleo + InterruptIn stop_button(BUTTON1); +#endif +#if TARGET_Freescale // we are cross-compiling for NXP FRDM boards. + InterruptIn stop_button(SW2); +#endif + void process_interrupt( VL53L1 * sensor,VL53L1_DEV dev ); void print_results( int devSpiNumber, VL53L1_MultiRangingData_t *pMultiRangingData ); @@ -47,24 +66,257 @@ VL53L1_Dev_t devRight; VL53L1_DEV Dev = &devCentre; +/* Installed sensors count */ +int sensorCnt = 0; + +/* installed sensors prefixes */ +char installedSensors[3]; + +/* interrupt requests */ +volatile bool centerSensor = false; +volatile bool leftSensor = false; +volatile bool rightSensor = false; +volatile bool int_measuring_stop = false; + +/* Current sensor number*/ +volatile int currentSensor = 0; + +/* current displayed sensor change IRQ */ +volatile bool switchChanged = false; + +/* ISR callback function of the centre sensor */ +void sensor_centre_irq(void) +{ + centerSensor = true; + board->sensor_centre->disable_interrupt_measure_detection_irq(); +} + +/* ISR callback function of the left sensor */ +void sensor_left_irq(void) +{ + leftSensor = true; + board->sensor_left->disable_interrupt_measure_detection_irq(); +} + +/* ISR callback function of the right sensor */ +void sensor_right_irq(void) +{ + rightSensor = true; + board->sensor_right->disable_interrupt_measure_detection_irq(); +} + +/* ISR callback function of the user blue button to switch measuring sensor. */ +void switch_measuring_sensor_irq(void) +{ + stop_button.disable_irq(); + switchChanged = true; +} + +/* + * This function calls the interrupt handler for each sensor + * and outputs the range + */ +inline void measure_sensors() +{ + int status = 0; + bool current = false; + + uint16_t distance = 0; + + VL53L1_MultiRangingData_t MultiRangingData; + VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData; + + /* Handle the interrupt and output the range from the centre sensor */ + if (centerSensor) { + centerSensor = false; + + status = board->sensor_centre->VL53L1_GetDistance(&distance); + + current = (currentSensor == 0); + if (current) { + printf("Centre: %d\r\n", distance); + } + + status = board->sensor_centre->VL53L1_ClearInterrupt(); + board->sensor_centre->enable_interrupt_measure_detection_irq(); + + } + + /* Handle the interrupt and output the range from the left sensor */ + if (leftSensor) { + leftSensor = false; + + status = board->sensor_left->VL53L1_GetDistance(&distance); + + current = (installedSensors[currentSensor] == 'L'); + if (current) { + printf("Left: %d\r\n", distance); + } + status = board->sensor_left->VL53L1_ClearInterrupt(); + board->sensor_left->enable_interrupt_measure_detection_irq(); + + } + + /* Handle the interrupt and output the range from the right sensor */ + if (rightSensor) { + rightSensor = false; + + status = board->sensor_right->VL53L1_GetDistance(&distance); + + current = (installedSensors[currentSensor] == 'R'); + if (current) { + printf("Right: %d\r\n", distance); + } + status = board->sensor_right->VL53L1_ClearInterrupt(); + board->sensor_right->enable_interrupt_measure_detection_irq(); + + } +} + +int configure_sensor(VL53L1 *Sensor) +{ + int status = 0; +/* + //configure the regions of interest for each sensor + VL53L1_RoiConfig_t roiConfig; + + roiConfig.NumberOfRoi = 3; + roiConfig.UserRois[0].TopLeftX = 0; + roiConfig.UserRois[0].TopLeftY = 9; + roiConfig.UserRois[0].BotRightX = 4; + roiConfig.UserRois[0].BotRightY = 5; + + roiConfig.UserRois[1].TopLeftX = 5; + roiConfig.UserRois[1].TopLeftY = 9; + roiConfig.UserRois[1].BotRightX = 9; + roiConfig.UserRois[1].BotRightY = 4; + + roiConfig.UserRois[2].TopLeftX = 11; + roiConfig.UserRois[2].TopLeftY = 9; + roiConfig.UserRois[2].BotRightX = 15; + roiConfig.UserRois[2].BotRightY = 5; + + status = Sensor->vl53L1_SetROI( &roiConfig); +*/ +// Device Initialization and setting + status = Sensor->vl53L1_DataInit(); + status = Sensor->vl53L1_StaticInit(); + + status = Sensor->vl53L1_SetPresetMode(VL53L1_PRESETMODE_MULTIZONES_SCANNING); + status = Sensor->vl53L1_SetDistanceMode(VL53L1_DISTANCEMODE_LONG); + + status = Sensor->vl53L1_StartMeasurement(); + + return status; +} + +/* + * Add to an array a character that represents the sensor and start ranging + */ +int init_sensors_array() +{ + int status = 0; + VL53L1 *Sensor; + uint8_t ToFSensor = 1; // 0=Left, 1=Center(default), 2=Right + + sensorCnt = 0; + + Dev->comms_speed_khz = 400; + Dev->comms_type = 1; + + if (board->sensor_centre != NULL) { + Dev = &devCentre; + Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS; + devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS; + + status = configure_sensor(board->sensor_centre); + printf("configuring centre channel \n"); + } + + if (board->sensor_left != NULL) { + Dev = &devLeft; + Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS; + devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS; + + status = configure_sensor(board->sensor_left); + printf("configuring left channel \n"); + } + + if (board->sensor_right != NULL) { + Dev = &devRight; + Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS; + devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS; + + status = configure_sensor(board->sensor_right); + printf("configuring right channel \n"); + } + + /* start the measure on the center sensor */ + if (NULL != board->sensor_centre) { + installedSensors[sensorCnt] = 'C'; + status = board->sensor_centre->stop_measurement(); + if (status != 0) { + return status; + } + status = board->sensor_centre->start_measurement(&sensor_centre_irq); + if (status != 0) { + return status; + } + ++sensorCnt; + } + /* start the measure on the left sensor */ + if (NULL != board->sensor_left) { + installedSensors[sensorCnt] = 'L'; + status = board->sensor_left->stop_measurement(); + if (status != 0) { + return status; + } + status = board->sensor_left->start_measurement(&sensor_left_irq); + if (status != 0) { + return status; + } + ++sensorCnt; + } + /* start the measure on the right sensor */ + if (NULL != board->sensor_right) { + installedSensors[sensorCnt] = 'R'; + status = board->sensor_right->stop_measurement(); + if (status != 0) { + return status; + } + status = board->sensor_right->start_measurement(&sensor_right_irq); + if (status != 0) { + return status; + } + ++sensorCnt; + } + currentSensor = 0; + return status; +} + + /*=================================== Main ================================== =============================================================================*/ int main() { int status; - VL53L1 * Sensor; - uint16_t wordData; - + VL53L1 *Sensor; +// uint16_t wordData; + + stop_button.rise(&switch_measuring_sensor_irq); + stop_button.enable_irq(); + pc.baud(115200); // baud rate is important as printf statements take a lot of time - printf("Polling single multizone mbed = %d \r\n",MBED_VERSION); + printf("mbed version = %d \r\n",MBED_VERSION); // create i2c interface ToF_DevI2C *dev_I2C = new ToF_DevI2C(I2C_SDA, I2C_SCL); /* creates the 53L1A1 expansion board singleton obj */ - board = XNucleo53L1A2::instance(dev_I2C, A2, D8, D2); +// board = XNucleo53L1A2::instance(dev_I2C, A2, D8, D2); + board = XNucleo53L1A2::instance(dev_I2C, A2, D9, D2); printf("board created!\r\n"); @@ -73,12 +325,17 @@ if (status) { printf("Failed to init board!\r\n"); return 0; - } - + } printf("board initiated! - %d\r\n", status); - // create the sensor controller classes + /* init an array with chars to id the sensors */ +// status = init_sensors_array(); + if (status != 0) { + printf("Failed to init sensors!\r\n"); + return status; + } + VL53L1_RoiConfig_t roiConfig; roiConfig.NumberOfRoi =3; @@ -97,103 +354,155 @@ roiConfig.UserRois[2].TopLeftY = 9; roiConfig.UserRois[2].BotRightX = 15; roiConfig.UserRois[2].BotRightY = 5; - -// start of setup of central sensor - Dev=&devCentre; + // configure the sensors Dev->comms_speed_khz = 400; Dev->comms_type = 1; - Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS; - devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS; - Sensor=board->sensor_centre; - - printf("configuring centre channel \n"); +// start of setup of central sensor + if (board->sensor_centre != NULL) { + Dev=&devCentre; + Dev->i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS; + devCentre.i2c_slave_address = NEW_SENSOR_CENTRE_ADDRESS; + + Sensor=board->sensor_centre; -// Sensor->VL53L1_RdWord(Dev, 0x01, &wordData); -// printf("VL53L1X: %02X %d\n\r", wordData,Dev->i2c_slave_address); -/* Device Initialization and setting */ - status = Sensor->vl53L1_DataInit(); - status = Sensor->vl53L1_StaticInit(); - status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING); - status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG); + printf("configuring centre channel \n"); + + // Device Initialization and setting + status = Sensor->vl53L1_DataInit(); + status = Sensor->vl53L1_StaticInit(); + + status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING); + status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG); + + status = Sensor->vl53L1_SetROI( &roiConfig); + + printf("VL53L1_SetROI %d \n",status); - - status = Sensor->vl53L1_SetROI( &roiConfig); - - printf("VL53L1_SetROI %d \n",status); - - status = board->sensor_centre->vl53L1_StartMeasurement(); - + status = board->sensor_centre->vl53L1_StartMeasurement(); + } // start of setup of left satellite - Dev=&devLeft; + if (board->sensor_left != NULL) { + Dev=&devLeft; + Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS; + devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS; + + Sensor=board->sensor_left; -// configure the sensors - Dev->comms_speed_khz = 400; - Dev->comms_type = 1; - Dev->i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS; - devLeft.i2c_slave_address = NEW_SENSOR_LEFT_ADDRESS; - - Sensor=board->sensor_left; - - printf("configuring centre channel \n"); + printf("configuring left channel \n"); + + // Device Initialization and setting + status = Sensor->vl53L1_DataInit(); + status = Sensor->vl53L1_StaticInit(); + + status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING); + status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG); + + status = Sensor->vl53L1_SetROI( &roiConfig); + + printf("VL53L1_SetROI %d \n",status); -// Sensor->VL53L1_RdWord(Dev, 0x01, &wordData); -// printf("VL53L1X: %02X %d\n\r", wordData,Dev->i2c_slave_address); -/* Device Initialization and setting */ - status = Sensor->vl53L1_DataInit(); - status = Sensor->vl53L1_StaticInit(); - status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING); - status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG); + status = board->sensor_left->vl53L1_StartMeasurement(); + } + + // start of setup of right satellite + if (board->sensor_right != NULL) { + Dev=&devRight; + Dev->i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS; + devRight.i2c_slave_address = NEW_SENSOR_RIGHT_ADDRESS; + + Sensor=board->sensor_right; - - status = Sensor->vl53L1_SetROI( &roiConfig); + printf("configuring right channel \n"); + + // Device Initialization and setting + status = Sensor->vl53L1_DataInit(); + status = Sensor->vl53L1_StaticInit(); + + status = Sensor->vl53L1_SetPresetMode( VL53L1_PRESETMODE_MULTIZONES_SCANNING); + status = Sensor->vl53L1_SetDistanceMode( VL53L1_DISTANCEMODE_LONG); + + status = Sensor->vl53L1_SetROI( &roiConfig); + + printf("VL53L1_SetROI %d \n",status); - printf("VL53L1_SetROI %d \n",status); + status = board->sensor_right->vl53L1_StartMeasurement(); + } - status = board->sensor_left->vl53L1_StartMeasurement(); - VL53L1_MultiRangingData_t MultiRangingData; VL53L1_MultiRangingData_t *pMultiRangingData = &MultiRangingData; // looping polling for results - while (1) - { + while (true) { pMultiRangingData = &MultiRangingData; - - // wait for result - status = board->sensor_centre->vl53L1_WaitMeasurementDataReady(); - // get the result - status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData); - // if valid, print it - if(status==0) { - print_results(devCentre.i2c_slave_address, pMultiRangingData ); - status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement(); - - } //if(status==0) - else - { - printf("VL53L1_GetMultiRangingData centre %d \n",status); +/* + if (board->sensor_centre != NULL) { + // wait for result + status = board->sensor_centre->vl53L1_WaitMeasurementDataReady(); + // get the result + status = board->sensor_centre->vl53L1_GetMultiRangingData( pMultiRangingData); + // if valid, print it + if(status==0) { + print_results(devCentre.i2c_slave_address, pMultiRangingData ); + status = board->sensor_centre->vl53L1_ClearInterruptAndStartMeasurement(); + + } //if(status==0) + else + { + printf("VL53L1_GetMultiRangingData centre %d \n",status); + } } - // wait for result - status = board->sensor_left->vl53L1_WaitMeasurementDataReady(); - // get the result - status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData); - // if valid, print it - if(status==0) { - print_results(devLeft.i2c_slave_address, pMultiRangingData ); - status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement(); - - } //if(status==0) - else - { - printf("VL53L1_GetMultiRangingData centre %d \n",status); + if (board->sensor_left != NULL) { + // wait for result + status = board->sensor_left->vl53L1_WaitMeasurementDataReady(); + // get the result + status = board->sensor_left->vl53L1_GetMultiRangingData( pMultiRangingData); + // if valid, print it + if(status==0) { + print_results(devLeft.i2c_slave_address, pMultiRangingData ); + status = board->sensor_left->vl53L1_ClearInterruptAndStartMeasurement(); + + } //if(status==0) + else + { + printf("VL53L1_GetMultiRangingData left %d \n",status); + } } + if (board->sensor_right != NULL) { + // wait for result + status = board->sensor_right->vl53L1_WaitMeasurementDataReady(); + // get the result + status = board->sensor_right->vl53L1_GetMultiRangingData( pMultiRangingData); + // if valid, print it + if(status==0) { + print_results(devRight.i2c_slave_address, pMultiRangingData ); + status = board->sensor_right->vl53L1_ClearInterruptAndStartMeasurement(); + + } //if(status==0) + else + { + printf("VL53L1_GetMultiRangingData right %d \n",status); + } + } +*/ + + measure_sensors(); + + if (switchChanged) { + ++currentSensor; + if (currentSensor == sensorCnt) + currentSensor = 0; + printf("Sensor changed to %c\r\n", installedSensors[currentSensor]); + switchChanged = false; + stop_button.enable_irq(); + } + } }