Diff: main.cpp

Revision:

0:ed39e596d896

Child:

1:7e59bdb20309

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 28 16:22:40 2020 +0000
@@ -0,0 +1,509 @@
+/**
+  ******************************************************************************
+  * File Name          : main.c
+  * Date               : 21/10/2014 09:21:14
+  * Description        : Main program body
+  ******************************************************************************
+  *
+  * COPYRIGHT(c) 2014 STMicroelectronics
+  *
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *   1. Redistributions of source code must retain the above copyright notice,
+  *      this list of conditions and the following disclaimer.
+  *   2. Redistributions in binary form must reproduce the above copyright notice,
+  *      this list of conditions and the following disclaimer in the documentation
+  *      and/or other materials provided with the distribution.
+  *   3. Neither the name of STMicroelectronics nor the names of its contributors
+  *      may be used to endorse or promote products derived from this software
+  *      without specific prior written permission.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  ******************************************************************************
+  
+  This program controls VL6180 ToF sensors running on an STM32F401 card with the
+  VL6180 shield and up to 3 VL6180 satelite boards.  This program works in interrupt
+  mode with all 4 VL6180s operating at the same time. 
+  
+  Because this program is designed to run on MBed V2 the normal RTOS features 
+  such as threads and the signalling between them don't exist. Because of this 
+  the interupt routine signals the main program by setting flags, there is one to
+  signal that an interupt has been received from each VL6180. The mainloop then polls
+  these flags to know that an interrupt has occured.
+  
+  The display is very crude with no storing of the last digits, each digit is written
+  and then a wait occurs and then the next digit is written. Tjis means that a lot of time is 
+  taken writing tyhe display and any loong period when the display is not serviced 
+  will cause the display to flicker.
+  
+  In this program access to the VL6180 api is through wrapper functions in
+  vl6180_class.cpp. It is also possible to access the api directly. E.G both the lines below
+  do the same thing. 
+  
+status = VL6180_ClearInterrupt(dev,INTERRUPT_CLEAR_ERROR|INTERRUPT_CLEAR_RANGING);
+status = sensor->vl6180_ClearInterrupt(dev,INTERRUPT_CLEAR_ERROR|INTERRUPT_CLEAR_RANGING);
+  */
+
+/* Includes ------------------------------------------------------------------*/
+#include <stdio.h>
+
+#include "mbed.h"
+#include "XNucleo6810.h"
+#include <time.h>
+
+#include "spi_interface.h"
+
+
+/* Private variables ---------------------------------------------------------*/
+
+
+
+//VL6180_SINGLE_DEVICE_DRIVER
+
+// i2c tx and rx pins
+#define I2C_SDA   D14 
+#define I2C_SCL   D15 
+
+/* USER CODE BEGIN 0 */
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include "vl6180_api.h"
+#include "6180a1.h"
+
+#ifdef DEBUG
+    //TODO
+#include "diag/trace.h"
+    #define debug(msg, ...)   trace_printf(msg,__VA_ARGS__)
+    #define trace_warn(msg,...) trace_printf("W %s %d" msg "\n", __func__, __LINE__, __VA_ARGS__)
+#else
+    #define debug(msg, ...)  (void)0
+#endif
+
+#define DigitDisplay_ms     1 /* ms each digit is kept on */
+
+
+
+
+
+static int int_centre_result = 0;
+static int int_left_result = 0;
+static int int_right_result = 0;
+static int int_bottom_result = 0;
+static int int_result = 0;
+
+
+// timing for debug purposes
+uint32_t  centre_polling_time_ms;
+uint32_t  centre_start_time_ms;
+uint32_t  centre_end_time_ms;
+
+void start_sensor(VL6180Dev_t  dev ,VL6180 *sensor);
+int get_sensor_data(VL6180Dev_t  dev ,VL6180 *sensor); 
+
+
+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,MyVL6180Dev_t Dev) : _interrupt(pin)          // create the InterruptIn on the pin specified to Counter
+    {
+         int_result = 1;
+         Devlocal = Dev;
+        _interrupt.rise(callback(this, &WaitForMeasurement::got_interrupt)); // attach increment function of this counter instance
+        
+    }
+    
+    void process_right_interrupt()
+    {
+          printf("processing right interrupt\n");
+    }
+
+  // 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()
+    {
+    
+        _count++;
+
+        if (Devlocal.i2c_addr == NEW_SENSOR_CENTRE_ADDRESS)
+            int_centre_result = 1;  //flag to main that interrupt happened
+        if (Devlocal.i2c_addr == NEW_SENSOR_LEFT_ADDRESS)
+            int_left_result = 1;   //flag to main that interrupt happened
+        if (Devlocal.i2c_addr == NEW_SENSOR_RIGHT_ADDRESS)
+        {
+            int_right_result = 1;  //flag to main that interrupt happened
+        }
+        if (Devlocal.i2c_addr == NEW_SENSOR_BOTTOM_ADDRESS)
+            int_bottom_result = 1;   //flag to main that interrupt happened
+        }
+    
+    //destructor
+    ~WaitForMeasurement()
+    {
+        printf("destruction \n");
+    }
+
+private:
+    InterruptIn _interrupt;
+    volatile int _count;
+    MyVL6180Dev_t Devlocal;
+    int status;
+    
+};
+
+
+
+
+MyVL6180Dev_t                   devCentre;  //data  for each of the vl6180
+MyVL6180Dev_t                   devLeft;
+MyVL6180Dev_t                   devRight;
+MyVL6180Dev_t                   devBottom;
+VL6180Dev_t                     Dev = &devCentre; // the currently used vl6180
+
+volatile int IntrFired=0;
+
+VL6180 *Sensor;  
+
+static XNucleo53L1A1 *board=NULL;
+
+Serial pc(SERIAL_TX, SERIAL_RX); 
+
+vl6180_DevI2C *dev_I2C = new vl6180_DevI2C(I2C_SDA, I2C_SCL);
+
+//void WaitMilliSec(int ms);
+
+/**
+ * VL6180x CubeMX F401 i2c porting implementation
+ */
+
+#define theVL6180Dev   0x52    // what we use as "API device
+
+#define i2c_bus      (&hi2c1)
+#define def_i2c_time_out 100
+
+
+void XNUCLEO6180XA1_WaitMilliSec(int n){
+    wait_ms(n);
+}
+
+
+
+
+/**
+ * DISPLAY public
+ */
+/***************  DISPLAY PUBLIC *********************/
+/***************  DISPLAY PRIVATE *********************/
+static char DISP_CurString[10];
+
+/**
+ * call in the main loop
+ * when running under debugger it enable doing direct vl6180x reg access
+ * typcilay breaking at entrance
+ * change  the the local index/data and cmd variable to do what needed
+ * reg_cmd -1 wr byte -2wr word -3 wr dword
+ * reg_cmd 1 rd byte 2 rd word 3 rd dword
+ * step to last statement before return and read variable to get rd result exit
+ */
+ 
+void debug_stuff(void) {
+    int reg_cmd = 0;
+    static uint32_t reg_data;
+    static uint16_t reg_index;
+
+    if (reg_cmd) {
+        switch (reg_cmd) {
+        case -1:
+            VL6180_WrByte(Dev, reg_index, reg_data);
+            debug("Wr B 0x%X = %d", reg_index, (int)reg_data);
+            break;
+        case -2:
+            VL6180_WrWord(Dev, reg_index, reg_data);
+            debug("Wr W 0x%X = %d", reg_index,(int) reg_data);
+            break;
+
+        case -3:
+            VL6180_WrDWord(Dev, reg_index, reg_data);
+            debug("WrDW 0x%X = %d", reg_index, (int)reg_data);
+            break;
+
+        case 1:
+            reg_data=0;
+            VL6180_RdByte(Dev, reg_index, (uint8_t*)&reg_data);
+            debug("RD B 0x%X = %d", reg_index, (int)reg_data);
+            break;
+        case 2:
+            reg_data=0;
+            VL6180_RdWord(Dev, reg_index, (uint16_t*)&reg_data);
+            debug("RD W 0x%X = %d", reg_index, (int)reg_data);
+            break;
+
+        case 3:
+            VL6180_RdDWord(Dev, reg_index, &reg_data);
+            debug("RD DW 0x%X = %d", reg_index, (int)reg_data);
+            break;
+        default:
+            debug("Invalid command %d", reg_cmd);
+            /* nothing to do*/
+            ;
+        }
+    }
+}
+
+
+volatile int VL6180_IsrFired=0;
+
+
+char buffer[10];
+
+
+uint32_t TimeStarted;       /* various display and mode delay starting time */
+VL6180_RangeData_t Range;  /* Range measurmeent  */
+uint16_t range;             /* range average distance */
+
+
+/* USER CODE END 0 */
+
+/* Private function prototypes -----------------------------------------------*/
+
+int main(void)
+{
+    
+    WaitForMeasurement* int1; // the interrupt handler
+    WaitForMeasurement* int2; // the interrupt handler
+    WaitForMeasurement* int3; // the interrupt handler
+    WaitForMeasurement* int4; // the interrupt handler
+
+    pc.baud(115200);  // baud rate is important as printf statements take a lot of time
+
+
+      //create I2C channel
+    vl6180_DevI2C *dev_I2C = new vl6180_DevI2C(I2C_SDA, I2C_SCL);
+    
+    dev_I2C->frequency(400000); //also needs doing in spi_interface.c
+    devCentre.i2c_addr = theVL6180Dev;  // set to default adress for now
+
+    int status;
+    
+    // create instances for the sensors
+    board = XNucleo53L1A1::instance(dev_I2C, A3, D13, D2 ,A2);      
+        // find the sensors we have and initialise
+    status = board->init_board();
+    if (status) {
+        printf("Failed to init board!\r\n");
+        return 0;
+    }
+    
+    
+    //select centre sensor
+    if (board->sensor_centre != NULL ) {
+        devCentre.i2c_addr = NEW_SENSOR_CENTRE_ADDRESS;
+        Dev = &devCentre;
+        Sensor=board->sensor_centre;
+        int1 =  new WaitForMeasurement(A3,devCentre);  // create a interrupt class for this interrupt pin
+        start_sensor(Dev ,Sensor);
+    }
+    
+    if (board->sensor_left != NULL ) {
+        devLeft.i2c_addr = NEW_SENSOR_LEFT_ADDRESS;
+        Dev = &devLeft;
+        Sensor=board->sensor_left;
+        int2 =  new WaitForMeasurement(D13,devLeft);  // create a interrupt class for this interrupt pin
+        start_sensor(Dev ,Sensor);
+    }
+    
+    if (board->sensor_right != NULL ) {    
+        devRight.i2c_addr = NEW_SENSOR_RIGHT_ADDRESS;
+        Dev = &devRight;
+        Sensor=board->sensor_right;
+        int2 =  new WaitForMeasurement(D2,devRight);  // create a interrupt class for this interrupt pin
+        start_sensor(Dev ,Sensor);
+    }
+    
+    if (board->sensor_bottom != NULL ) {    
+        devBottom.i2c_addr = NEW_SENSOR_BOTTOM_ADDRESS;
+        Dev = &devBottom;
+        Sensor=board->sensor_bottom;
+        int2 =  new WaitForMeasurement(A2,devBottom);  // create a interrupt class for this interrupt pin
+        start_sensor(Dev ,Sensor);
+    }
+
+
+    
+    /* Infinite loop */
+    while (1) {
+               
+        if ( int_centre_result == 1 )
+        {
+            int_centre_result = 0;  //clear interrupt flag
+            Dev = &devCentre;
+            Sensor=board->sensor_centre;
+            
+            int result = get_sensor_data(Dev , Sensor);
+            if ( result != 0)
+                printf("C %d \n",result);
+       }
+       
+        if ( int_left_result == 1 )
+        {
+            int_left_result = 0;  //clear interrupt flag
+            Dev = &devLeft;
+            Sensor=board->sensor_left;
+            
+            int result = get_sensor_data(Dev , Sensor);
+            if ( result != 0)
+                printf("L %d \n",result);
+       }
+       
+        if ( int_right_result == 1 )
+        {
+            int_right_result = 0;  //clear interrupt flag
+            Dev = &devRight;
+            Sensor=board->sensor_right;
+            
+            int result = get_sensor_data(Dev , Sensor);
+            if ( result != 0)
+                printf("R %d \n",result);
+        }
+        
+        if ( int_bottom_result == 1 )
+        {
+            int_bottom_result = 0;  //clear interrupt flag
+            Dev = &devBottom;
+            Sensor=board->sensor_bottom;
+            
+            int result = get_sensor_data(Dev , Sensor);
+            if ( result != 0)
+                printf("B %d \n",result);
+        }
+
+
+       // the display is very simple and requires written to frequently so
+       // we are writing to the display when we would normally sleep.
+       // when we are not writing to the display it is blank
+       {
+           for (int i = 0 ;i < 10;i++)
+           {  // write to display
+                XNUCLEO6180XA1_DisplayString(DISP_CurString, DigitDisplay_ms* 5);
+           }
+       }
+
+//        debug_stuff();
+    }
+
+}
+
+
+void start_sensor(VL6180Dev_t  dev ,VL6180 *sensor)
+{
+    
+        /* Note that if we waited  1msec we could bypass VL6180_WaitDeviceBooted(&Dev); */
+    int status;
+    status = sensor->vl6180_WaitDeviceBooted(dev);
+    printf("vl6180_WaitDeviceBooted %d %d\n",status, dev->i2c_addr);
+    status = sensor->vl6180_InitData(dev);
+    printf("vl6180_InitData %d %d\n",status, dev->i2c_addr);
+   
+    status = sensor->vl6180_FilterSetState(dev,0); // disbale as not effective in continuous mose
+    printf("vl6180_FilterSetState %d \n",status);
+    status = sensor->vl6180_Prepare(dev);  // sensor init
+    printf("vl6180_Prepare %d \n",status);
+    
+    status = sensor->vl6180_UpscaleSetScaling(dev, 2); // set scaling  by 2  to get ranging in range 0 to 400mm
+    printf("vl6180_UpscaleSetScaling %d \n",status);
+
+    // if slow reaction is enough then set a high time like 100 ms (up to 2550 msec)
+    // if fastest reaction is required then set 0  that will set minimal possible
+    status = sensor->vl6180_RangeSetInterMeasPeriod(dev, 100);
+    printf("vl6180_RangeSetInterMeasPeriod %d \n",status);
+     // set vl6180x gpio1 pin to range interrupt output with high polarity (rising edge)
+    status = sensor->vl6180_SetupGPIO1(dev, GPIOx_SELECT_GPIO_INTERRUPT_OUTPUT, INTR_POL_HIGH);
+    printf("vl6180_SetupGPIO1 %d \n",status);
+    // set range interrupt reporting to low threshold   
+    status = sensor->vl6180_RangeConfigInterrupt(dev, CONFIG_GPIO_INTERRUPT_LEVEL_LOW);
+    printf("vl6180_RangeConfigInterrupt %d \n",status);
+    // we don't care of high threshold as we don't use it , group hold is managed externaly    
+    status = sensor->vl6180_RangeSetThresholds(dev, 100, 00, 0);
+    printf("vl6180_RangeSetThresholds %d %d\n",status, dev->i2c_addr);
+    
+    status = sensor->vl6180_ClearInterrupt(dev,INTERRUPT_CLEAR_ERROR|INTERRUPT_CLEAR_RANGING);
+    printf("vl6180_ClearInterrupt %d \n",status);
+    
+    status = sensor->vl6180_RangeStartContinuousMode(dev);
+    printf("vl6180_RangeStartContinuousMode %d %d\n",status, dev->i2c_addr);
+}
+
+
+int get_sensor_data(VL6180Dev_t  dev ,VL6180 *sensor)
+{
+    int status;
+    int result = 0;
+    status = sensor->vl6180_RangeGetMeasurement(dev, &Range);
+    if( status == 0 ){         
+        // Application must check Range.errorStatus before accessing the other data
+        //    If Range.errorStatus is DataNotReady, application knows that it has to wait a bit before getting a new data
+        //    If Range.errorStatus is 0, application knows it is a valid distance
+        //    If Range.errorStatus is not 0, application knows that reported distance is invalid so may take some decisions depending on the errorStatus
+        if (Range.errorStatus == DataNotReady){
+            printf("notready \n");
+        
+            return result;
+            }
+        
+        // only display the centre sensors values
+
+       if((Range.errorStatus == 0) && (status == 0))
+       {
+            // only display the centre sensors values
+             if ( dev->i2c_addr == NEW_SENSOR_CENTRE_ADDRESS )
+             { 
+                 sprintf(DISP_CurString, " %d", (int)Range.range_mm);
+             }
+             result = (int)Range.range_mm;
+       } 
+       else
+       {
+            // only display the centre sensors values
+             if ( dev->i2c_addr == NEW_SENSOR_CENTRE_ADDRESS )
+             { 
+                 sprintf(DISP_CurString, " %4d", 0);
+             }
+             result = 0;
+       }     
+                 
+            
+        /* re-arm next measurement */ 
+       sensor->vl6180_ClearInterrupt(dev,INTERRUPT_CLEAR_ERROR|INTERRUPT_CLEAR_RANGING);
+
+    }  // status != 0
+    else{
+        // it is an critical error
+//           HandleError("critical error on VL6180x_RangeCheckAndGetMeasurement");
+    }
+return result;
+
+}
+
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/