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