VL53L1X using XIAO SAMD21

i used 5 VL53L1X with Arduino and send the data on mavlink which was done by @geofrancis
but whenever i am flying the avoidance is sometimes happening and sometimes not. Its not very consistent due to come reason. Don’t have flight logs as its a custom board using matekek target with few modifications. its doing everything fine but there is sort of delay. in the code provided by geofransis i see that it calls command distance and that sends a packet 5 times in one loop. At the mavlink inspector
i see i am getting the distance packets at 20hz i.e.
at first loop
i get sensor1 sensor 2 … sensor5 which takes 250ms
and soo on for 1 sec
the disadvantage is that after sensor1 reading is sent i have to wait for 200ms for the sensor to appear again

so i have chosen XIAO SAMD21 for now because its smaller and faster.


using this code

#include “mavlink.h”
#include “mavlink_msg_distance_sensor.h”
#include <Wire.h>
#include <VL53L1X.h>
const int MIN = 20;
const int MAX = 2200;
const int idle = 500;
const int Scale = 10;
const int RMIN = 10;
const int RMAX = 350;

#define bRate 115200

#define XSHUT_pin5 0
#define XSHUT_pin4 1
#define XSHUT_pin3 10
#define XSHUT_pin2 9
#define XSHUT_pin1 8

VL53L1X Sensor1;
VL53L1X Sensor2;
VL53L1X Sensor3;
VL53L1X Sensor4;
VL53L1X Sensor5;

void setup()

{
Serial1.begin(115200);
Wire.begin();
Wire.setClock(400000);
/WARNING/
//Shutdown pins of VL53L1X ACTIVE-LOW-ONLY NO TOLERANT TO 5V will fry them
pinMode(XSHUT_pin1, OUTPUT);
digitalWrite(XSHUT_pin1, LOW);
pinMode(XSHUT_pin2, OUTPUT);
digitalWrite(XSHUT_pin2, LOW);
pinMode(XSHUT_pin3, OUTPUT);
digitalWrite(XSHUT_pin3, LOW);
pinMode(XSHUT_pin4, OUTPUT);
digitalWrite(XSHUT_pin4, LOW);
pinMode(XSHUT_pin5, OUTPUT);
digitalWrite(XSHUT_pin5, LOW);

pinMode(XSHUT_pin5, INPUT);
delay(10);

Sensor5.setTimeout(500);
if (!Sensor5.init()) {
Serial.print("Failed to detect and initialize sensor ");
Serial.println(5);
while (1)
;
}

Sensor5.setAddress(0x2A + 4);
Sensor5.setDistanceMode(VL53L1X::Medium);
Sensor5.setMeasurementTimingBudget(15000);
// Sensor5.startContinuous(15);

pinMode(XSHUT_pin4, INPUT);
delay(10);

Sensor4.setTimeout(500);
if (!Sensor4.init()) {
Serial.print("Failed to detect and initialize sensor ");
Serial.println(4);
while (1)
;
}

Sensor4.setAddress(0x2A + 3);
Sensor4.setDistanceMode(VL53L1X::Medium);
Sensor4.setMeasurementTimingBudget(15000);
// Sensor4.startContinuous(15);

pinMode(XSHUT_pin3, INPUT);
delay(10);

Sensor3.setTimeout(500);
if (!Sensor3.init()) {
Serial.print("Failed to detect and initialize sensor ");
Serial.println(3);
while (1)
;
}

Sensor3.setAddress(0x2A + 2);
Sensor3.setDistanceMode(VL53L1X::Medium);
Sensor3.setMeasurementTimingBudget(15000);
// Sensor3.startContinuous(15);

pinMode(XSHUT_pin2, INPUT);
delay(10);

Sensor2.setTimeout(500);
if (!Sensor2.init()) {
Serial.print("Failed to detect and initialize sensor ");
Serial.println(2);
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.
Sensor2.setAddress(0x2A + 1);
Sensor2.setDistanceMode(VL53L1X::Medium);
Sensor2.setMeasurementTimingBudget(15000);
// Sensor2.startContinuous(15);

pinMode(XSHUT_pin1, INPUT);
delay(10);

Sensor1.setTimeout(500);
if (!Sensor1.init()) {
Serial.print("Failed to detect and initialize sensor ");
Serial.println(1);
while (1)
;
}
Sensor1.setDistanceMode(VL53L1X::Medium);
Sensor1.setMeasurementTimingBudget(15000);
//ADDRESS_DEFAULT 0b0101001 or 41
// Sensor1.startContinuous(15);
}

void loop()
{

command_heartbeat();
command_distance_1();
command_distance_2();
command_distance_3();
command_distance_4();
command_distance_5();
}

void command_heartbeat() {

//< ID 1 for this system
int sysid = 100;
//< The component sending the message.
int compid = MAV_COMP_ID_PATHPLANNER;

// Define the system type, in this case ground control station
uint8_t system_type =MAV_TYPE_GCS;
uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;

uint8_t system_mode = 0;
uint32_t custom_mode = 0;
uint8_t system_state = 0;

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
mavlink_msg_heartbeat_pack(sysid,compid, &msg, system_type, autopilot_type, system_mode, custom_mode, system_state);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

// Send the message
//delay(1);
Serial1.write(buf, len);
}

void command_distance_1() {

// READ THE DISTANCE SENSOR
float Sensor1Smooth = Sensor1.readRangeSingleMillimeters();
Sensor1Smooth = constrain(Sensor1Smooth, MIN , MAX);
float dist1 = Sensor1Smooth / Scale;

//MAVLINK DISTANCE MESSAGE
int sysid = 1;
//< The component sending the message.
int compid = 158;

uint32_t time_boot_ms = 0; /< Time since system boot/
uint16_t min_distance = RMIN; /< Minimum distance the sensor can measure in centimeters/
uint16_t max_distance = RMAX; /< Maximum distance the sensor can measure in centimeters/
uint16_t current_distance = dist1; /< Current distance reading/
uint8_t type = 0; /< Type from MAV_DISTANCE_SENSOR enum./
uint8_t id = 1; /< Onboard ID of the sensor/
uint8_t orientation = 24; /(0=forward, each increment is 45degrees more in clockwise direction), 24 (upwards) or 25 (downwards)/
// Consumed within ArduPilot by the proximity class

uint8_t covariance = 0; /< Measurement covariance in centimeters, 0 for unknown / invalid readings/

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
mavlink_msg_distance_sensor_pack(sysid,compid,&msg,time_boot_ms,min_distance,max_distance,current_distance,type,id,orientation,covariance);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
//delay(1);
Serial1.write(buf, len);
}

void command_distance_2() {

// READ THE DISTANCE SENSOR
float Sensor2Smooth = Sensor2.readRangeSingleMillimeters();
Sensor2Smooth = constrain(Sensor2Smooth, MIN , MAX);
float dist2 = Sensor2Smooth / Scale;

//MAVLINK DISTANCE MESSAGE
int sysid = 1;
//< The component sending the message.
int compid = 158;

uint32_t time_boot_ms = 0; /< Time since system boot/
uint16_t min_distance = RMIN; /< Minimum distance the sensor can measure in centimeters/
uint16_t max_distance = RMAX; /< Maximum distance the sensor can measure in centimeters/
uint16_t current_distance = dist2; /< Current distance reading/
uint8_t type = 0; /< Type from MAV_DISTANCE_SENSOR enum./
uint8_t id = 2; /< Onboard ID of the sensor/
uint8_t orientation = 0; /(0=forward, each increment is 45degrees more in clockwise direction), 24 (upwards) or 25 (downwards)/
// Consumed within ArduPilot by the proximity class

uint8_t covariance = 0; /< Measurement covariance in centimeters, 0 for unknown / invalid readings/

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
mavlink_msg_distance_sensor_pack(sysid,compid,&msg,time_boot_ms,min_distance,max_distance,current_distance,type,id,orientation,covariance);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
//delay(1);
Serial1.write(buf, len);
}

void command_distance_3() {

// READ THE DISTANCE SENSOR
float Sensor3Smooth = Sensor3.readRangeSingleMillimeters();
Sensor3Smooth = constrain(Sensor3Smooth, MIN , MAX);
float dist3 = Sensor3Smooth / Scale;

//MAVLINK DISTANCE MESSAGE
int sysid = 1;
//< The component sending the message.
int compid = 158;

uint32_t time_boot_ms = 0; /< Time since system boot/
uint16_t min_distance = RMIN; /< Minimum distance the sensor can measure in centimeters/
uint16_t max_distance = RMAX; /< Maximum distance the sensor can measure in centimeters/
uint16_t current_distance = dist3; /< Current distance reading/
uint8_t type = 0; /< Type from MAV_DISTANCE_SENSOR enum./
uint8_t id = 3; /< Onboard ID of the sensor/
uint8_t orientation = 2; /(0=forward, each increment is 45degrees more in clockwise direction), 24 (upwards) or 25 (downwards)/
// Consumed within ArduPilot by the proximity class

uint8_t covariance = 0; /< Measurement covariance in centimeters, 0 for unknown / invalid readings/

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
mavlink_msg_distance_sensor_pack(sysid,compid,&msg,time_boot_ms,min_distance,max_distance,current_distance,type,id,orientation,covariance);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
//delay(1);
Serial1.write(buf, len);
}

void command_distance_4() {

// READ THE DISTANCE SENSOR
float Sensor4Smooth = Sensor4.readRangeSingleMillimeters();
Sensor4Smooth = constrain(Sensor4Smooth, MIN , MAX);
float dist4 = Sensor4Smooth / Scale;

//MAVLINK DISTANCE MESSAGE
int sysid = 1;
//< The component sending the message.
int compid = 158;

uint32_t time_boot_ms = 0; /< Time since system boot/
uint16_t min_distance = RMIN; /< Minimum distance the sensor can measure in centimeters/
uint16_t max_distance = RMAX; /< Maximum distance the sensor can measure in centimeters/
uint16_t current_distance = dist4; /< Current distance reading/
uint8_t type = 0; /< Type from MAV_DISTANCE_SENSOR enum./
uint8_t id = 4; /< Onboard ID of the sensor/
uint8_t orientation = 4; /(0=forward, each increment is 45degrees more in clockwise direction), 24 (upwards) or 25 (downwards)/
// Consumed within ArduPilot by the proximity class

uint8_t covariance = 0; /< Measurement covariance in centimeters, 0 for unknown / invalid readings/

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
mavlink_msg_distance_sensor_pack(sysid,compid,&msg,time_boot_ms,min_distance,max_distance,current_distance,type,id,orientation,covariance);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
//delay(1);
Serial1.write(buf, len);
}

void command_distance_5() {

// READ THE DISTANCE SENSOR
float Sensor5Smooth = Sensor5.readRangeSingleMillimeters();
Sensor5Smooth = constrain(Sensor5Smooth, MIN , MAX);
float dist5 = Sensor5Smooth / Scale;

//MAVLINK DISTANCE MESSAGE
int sysid = 1;
//< The component sending the message.
int compid = 158;

uint32_t time_boot_ms = 0; /< Time since system boot/
uint16_t min_distance = RMIN; /< Minimum distance the sensor can measure in centimeters/
uint16_t max_distance = RMAX; /< Maximum distance the sensor can measure in centimeters/
uint16_t current_distance = dist5; /< Current distance reading/
uint8_t type = 0; /< Type from MAV_DISTANCE_SENSOR enum./
uint8_t id = 5; /< Onboard ID of the sensor/
uint8_t orientation = 6; /(0=forward, each increment is 45degrees more in clockwise direction), 24 (upwards) or 25 (downwards)/
// Consumed within ArduPilot by the proximity class

uint8_t covariance = 0; /< Measurement covariance in centimeters, 0 for unknown / invalid readings/

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

// Pack the message
mavlink_msg_distance_sensor_pack(sysid,compid,&msg,time_boot_ms,min_distance,max_distance,current_distance,type,id,orientation,covariance);

// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
//delay(1);
Serial1.write(buf, len);
}

but its not showing up on prx radar
the only time it shows up is when i give the sensor timing budget of 50000(50ms)
but funny thing is that i get the same readings for 15ms also

Please help me figure this out

@ppoirier done the code initially, i just made some modifications.

I guess that changing processor type must impact on timing and some tweaking is necessary

Out of curiousity, I checked for better implementation for this distance sensor with SAM type processor and I found the SparkFun driver to be up-to date and support their Red Board Turbo (SAM 21) and the new VL53L4 sensor

https://learn.sparkfun.com/tutorials/qwiic-distance-sensor-vl53l1x-vl53l4cd-hookup-guide/all#introduction

The example is using the sensor interrupt that is a better implementation than approximative timing loop like on the example above. In conclusion, if I had to rewrite this code I would certainly use the interrupt method as it is optimal to get the fastest timing possible.

Looking at the specs of the sensor, we see that the Int pin is Open-Drain, so it is possible to hook all Int pins to one GPIO using pull down resistor to VCC ( 3.5V)
You have to be certain that the loop serves and release the interrupt befoer switching to next device otherwise you can have 2 interrupt lines pulled to ground and may damage the MosFet within the sensor.

SparkFun products are nice, I see that the hardware implementation can now be quite simplified if you use the I2C Multiplexor that is compatible with their connector/cable ecosystem
https://www.sparkfun.com/products/16784