VL53l1X_TESTER.ino

// Arduino MAVLink  http://forum.arduino.cc/index.php?topic=382592.0
// https://github.com/ArduPilot/ardupilot_wiki/blob/master/dev/source/docs/code-overview-object-avoidance.rst

/*
    The system id of the message should match the system id of the vehicle
    (default is "1" but can be changed using the SYSID_THISMAV parameter).
    The component id can be anything but MAV_COMP_ID_PATHPLANNER (195)
    or MAV_COMP_ID_PERIPHERAL (158) are probably good choices.

   # Define function to send distance_message mavlink message for mavlink based rangefinder, must be >10hz
  # http://mavlink.org/messages/common#DISTANCE_SENSOR
  def send_distance_message(dist):
    msg = vehicle.message_factory.distance_sensor_encode(
        0,          # time since system boot, not used
        1,          # min distance cm
        10000,      # max distance cm
        dist,       # current distance, must be int
        0,          # type = 0 MAV_DISTANCE_SENSOR_LASER Laser rangefinder, e.g. LightWare SF02/F or PulsedLight units
        0,          # onboard id, not used
        mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270, # must be set to MAV_SENSOR_ROTATION_PITCH_270 for mavlink rangefinder, represents downward facing
        0           # covariance, not used
    )
    vehicle.send_mavlink(msg)
    vehicle.flush()
    if args.verbose:
        log.debug("Sending mavlink distance_message:" +str(dist))
*/


/*
  This example shows how to set up and read multiple VL53L1X sensors connected to
  the same I2C bus. Each sensor needs to have its XSHUT pin connected to a
  different Arduino pin, and you should change sensorCount and the xshutPins array
  below to match your setup.

  For more information, see ST's application note AN4846 ("Using multiple VL53L0X
  in a single design"). The principles described there apply to the VL53L1X as
  well.
*/

#include <Wire.h>
#include <VL53L1X.h>

// The number of sensors in your system.
const uint8_t sensorCount = 2;

// The Arduino pin connected to the XSHUT pin of each sensor.
const uint8_t xshutPins[sensorCount] = { 2, 3,};

VL53L1X sensors[sensorCount];

void setup()
{
  while (!Serial) {}
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000); // use 400 kHz I2C

  // Disable/reset all sensors by driving their XSHUT pins low.
  for (uint8_t i = 0; i < sensorCount; i++)
  {
    pinMode(xshutPins[i], OUTPUT);
    digitalWrite(xshutPins[i], LOW);
  }

  // Enable, initialize, and start each sensor, one by one.
  for (uint8_t i = 0; i < sensorCount; i++)
  {
    // Stop driving this sensor's XSHUT low. This should allow the carrier
    // board to pull it high. (We do NOT want to drive XSHUT high since it is
    // not level shifted.) Then wait a bit for the sensor to start up.
    pinMode(xshutPins[i], INPUT);
    delay(10);

    sensors[i].setTimeout(500);
    if (!sensors[i].init())
    {
      Serial.print("Failed to detect and initialize sensor ");
      Serial.println(i);
      while (1);
    }

    // Each sensor must have its address changed to a unique value other than
    // the default of 0x29 (except for the last one, which could be left at
    // the default). To make it simple, we'll just count up from 0x2A.
    sensors[i].setAddress(0x2A + i);

    sensors[i].startContinuous(50);
  }
//  Serial.println("setup");
}



void loop() {

  command_distance_1();
  command_distance_2();
  //command_distance_3();
  //command_distance_4();
  //Serial.println("loop");
  //delay(200);
}


void command_distance_1() {


if (sensors[0].dataReady()) {
  Serial.print(sensors[0].read());
   Serial.print('\t');
}
  if (sensors[0].timeoutOccurred()) {
    Serial.print(" sensor1 timeout");
  }

}


void command_distance_2() {



if (sensors[1].dataReady()){
   Serial.print(sensors[1].read());
   Serial.println('\t');
}
  if (sensors[1].timeoutOccurred()) {
    Serial.print(" sensor2 timeout");
  }
 
}

 // void command_distance_3() {

 //   Serial.print(sensors[3].read());
 //   if (sensors[3].timeoutOccurred()) {
 //     Serial.print(" sensor3 timeout");
 //   }
 //   Serial.print('\t');
 // }



 // void command_distance_4() {

 //   Serial.print(sensors[4].read());
 //   if (sensors[4].timeoutOccurred()) {
   //   Serial.print(" sensor4 timeout");
 //   }
  //}