File content as of revision 0:e0163b3111e0:

/**
  ******************************************************************************
  * 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****/