Avoidance Experiments with the POC and Benewake TFMINI

slow down your code you dont need to send updates more than every 5hz, your sending them 500hz,!

what flight controller are you using ? im not sure proximity is enabled on the 1mb boards, you might need to make a custom firmware and select those features.

1 Like

what POC code are you using? It might be out of date. I had to update the mavlink libraries to get the original POC working again.

the mavlink messages has some more data in it now. it might not accept the mavlink packets without it

this is the old code.

  //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 = 1; /*< Minimum distance the sensor can measure in centimeters*/
  uint16_t max_distance = 170; /*< 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 = 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);
  Serial.write(buf, len);
}

New mavlink message


  //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 = 5; /*< Minimum distance the sensor can measure in centimeters*/
  uint16_t max_distance = 190; /*< 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);
}

1 Like

I’m using pixhawk 6c. Actually on refreshing the arduino the radar shows the object for 4 seconds. Can please you tell me how I can lower the rates?

just add a 200ms delay after sending the mavlink message.you are probably overloading the serial link at 500hz .

2 Likes

thank you very much it is working now.
I had to put some delay in the loop which I had forgotten :sweat_smile:

2 Likes

I got the POC running using the VL53l1x with up to 4 meters range, I only have 2 sensors to test with but im going to order more.

vl53l1x

1 Like

@ppoirier I built one for fun, going to test it on a little micro rover. I have a bunch of vl53l1x lidars too, im going to do a v2 with them once this is working.

Yeah NICE !!!
Happy that you continue on the POC experimentations, Have Fun :slight_smile:

2 Likes

I want to try and get setting parameters via mavlink working so this is going to be my test unit, after the v2 I will be building a 3d version using VL53L5x lidar arrays. the plan is to select the lines on the arrray based on the vehicle angle read via mavlink so its always looking ahead. so as it tilts forward it will read the higher lines at the front and the lower lines at the back, same for left and right. I want to be able to set parameters so it can be adjusted without reprogramming.

Ideally i want to get it working using the 3d input but I lack the maths abilities to convert distance and angle to a 3d vector in arduino.

1 Like

Which FC are you using right now??
I’m currently using MATEK H743 V-3 slim and the vl53l1x sensor is showing max range of 20cm.
I bought new sensor but the result was same.