MAVLINK-POC-VL53L1x.ino

#include <Wire.h>
#include <VL53L1X.h>
#include "mavlink/common/mavlink.h"        // Mavlink interface
#include "mavlink/common/mavlink_msg_distance_sensor.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

  for (uint8_t i = 0; i < sensorCount; i++)
  {
    pinMode(xshutPins[i], OUTPUT);
    digitalWrite(xshutPins[i], LOW);
  }
  for (uint8_t i = 0; i < sensorCount; i++)
  {
    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);
    }
    sensors[i].setAddress(0x2A + i);
    sensors[i].startContinuous(50);
  }
//  Serial.println("setup");
}



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



void command_heartbeat() {

  //< ID 1 for this system
  int sysid = 1;                   
  //< The component sending the message.
  int compid = 196;    
  
  // 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);
  Serial.write(buf, len);
}

void command_distance_1() {

// READ THE DISTANCE SENSOR
  float Sensor1Smooth  = sensors[0].read();
  float dist1 = Sensor1Smooth / 10;

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

  uint32_t time_boot_ms = 0; /*< Time since system boot*/
  uint16_t min_distance = 5; /*< Minimum distance the sensor can measure in centimeters*/
  uint16_t max_distance = 400; /*< 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 = 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*/
  float horizontal_fov =0;
  float vertical_fov =0;
  uint8_t signal_quality =0;
  const float quaternion =0;

  // 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,horizontal_fov,vertical_fov,signal_quality,quaternion);

  // 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);
  Serial.write(buf, len);
}




void command_distance_2() {

// READ THE DISTANCE SENSOR
  float Sensor2Smooth  = sensors[1].read();
  float dist2 = Sensor2Smooth / 10;

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

  uint32_t time_boot_ms = 0; /*< Time since system boot*/
  uint16_t min_distance = 5; /*< Minimum distance the sensor can measure in centimeters*/
  uint16_t max_distance = 400; /*< 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 = 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*/
  float horizontal_fov =0;
  float vertical_fov =0;
  uint8_t signal_quality =0;
  const float quaternion =0;

  // 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,horizontal_fov,vertical_fov,signal_quality,quaternion);
  // 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);
  Serial.write(buf, len);
}