TFmini 360°(almost) lidar

count74 · September 13, 2021, 10:17am

The new version of my rotating lidar uses a Benewake TF02-pro lidar.
It does not rotate anymore, but rather oscillate using a stepper motor and a poti.
I could not use a 360° rotating lidar on my rover anyway and the TF02-pro is too big to fit over the mirror of the previous version.
I also figured out how to correctly use the “obstacle_distance” mavlink message and why it did not work before. 115200 bauds were just to slow to handle the 72 value array. With the baudrate set to 1500000 (I just went for max, lower values may also work) everything works as it should. The mechanical setup allows for maximum 120° field of view and can be configured for lower values and therefor faster scanning.
Pictures, a short video of just the lidar moving and the arduino sketch for the blackpill:

https://drive.google.com/drive/folders/1dcY7kj2rSGAXR2O6DakMMg_pGuxE1VBh?usp=sharing

geofrancis · August 16, 2022, 2:24pm

@count74 I have a version of your code up and running but using a servo and sonar rather than a stepper and lidar, the plan is to eventually use it underwater.

after a lot of messing around i have something working but i can only get the 8 segment display on mission planner, how do i get the 72 segment display?

how do I get this?

github.com

geofrancis/Underwater-360-sonar-for-Ardupilot/blob/main/sweeping_sonar.ino

// Includes
#include "mavlink/common/mavlink.h"        // Mavlink interface
#include "mavlink/common/mavlink_msg_obstacle_distance.h"
#include <Servo.h>


#define POTI_PIN A0
#define FCbaud 1500000
#define sonarbaud 9600

int pothigh = 700;
int potlow = 300;

int res = 2;
int FOV = 120;


int potiValue = 0;
 //multiple of res and even(res is 3 degree for the TF02-pro)
int lidarAngle = 0;

This file has been truncated. show original

@ppoirier any ideas?

I think this might be it.

count74 · August 16, 2022, 3:46pm

That is a good question. I never saw the display with the dots myself.

geofrancis · August 16, 2022, 3:46pm

Im not seeing it in mission planner as a separate mavlink component.

I think the problem is its not sending a heartbeat.

geofrancis · August 16, 2022, 4:45pm

Got it working, It needs to broadcast a heatbeat then you can select that mavlink component in mission planner and you will get the high resolution view.

 int sysid = 1;                   
  //< The component sending the message.
  int compid = 196;    
  uint64_t time_usec = 0; /*< Time since system boot*/
  uint8_t sensor_type = 0;
  distances[messageAngle/res] = Dist-2.0f; //UINT16_MAX gets updated with actual distance values
  uint8_t increment = 3;
  uint16_t min_distance = 30; /*< Minimum distance the sensor can measure in centimeters*/
  uint16_t max_distance = 500; /*< Maximum distance the sensor can measure in centimeters*/
  float increment_f = 0;
  float angle_offset = -FOV/2;
  uint8_t frame = 12;
  uint8_t system_type = MAV_TYPE_GENERIC;
  uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;
  uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
  uint32_t custom_mode = 0;                 ///< Custom mode, can be defined by user/adopter
  uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight

  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 int type = MAV_TYPE_GROUND_ROVER;
  // Pack the message
  
 mavlink_msg_obstacle_distance_pack(sysid,compid,&msg,time_usec,sensor_type,distances,increment,min_distance,max_distance,increment_f,angle_offset,frame);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
  Serial1.write(buf, len);
  
  // Pack the message
  //mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
  mavlink_msg_heartbeat_pack(1,196, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
 
  // Copy the message to the send buffer  
  len = mavlink_msg_to_send_buffer(buf, &msg);
  Serial1.write(buf, len);
  
}
```.

count74 · August 16, 2022, 5:01pm

Good find, I will try this with my lidar when I am back home.

geofrancis · August 16, 2022, 6:32pm

the sonar im using is really slow, il have a go later at using a vl53l1x.

geofrancis · August 17, 2022, 8:48am

I have got some non blocking servo code working so I can slow down the scan rate,

github.com

geofrancis/Mavlink-servo-Sonar/blob/main/sweeping_sonar.ino

// Includes
#include "mavlink/common/mavlink.h"        // Mavlink interface
#include "mavlink/common/mavlink_msg_obstacle_distance.h"
#include <Servo.h>


#define POTI_PIN A0
#define FCbaud 1500000
#define sonarbaud 9600

int pothigh = 700;
int potlow = 300;

int res = 2;
int FOV = 120;


int potiValue = 0;
 //multiple of res and even(res is 3 degree for the TF02-pro)
int lidarAngle = 0;

This file has been truncated. show original

count74 · August 17, 2022, 10:45am

Why do you keep all the poti stuff in your servo version? I guess analog read is slowing down the code quite a bit.

geofrancis · August 17, 2022, 10:47am

I keep it there so i can drive the servo using alternative inputs, i can tie it to something like the camera gimbal so it ranges where the camera points, or just manually control the scan using the rc. my sonar tops out around 10hz so 72 slices means im limited to around 1 scan every 7.2 seconds. accuracy is more important to me than raw speed. I have 5x fixed underwater sonars for close range detection.

I also have a radar im planning on adapting to this system for use above water.

What would be nice is to request data from the flight controller to alter the scan profile dynamically, so when the boat is under 1mph do a full 360 scan to cover all sides but reduce the scan field as speed increases so it would drop from 360, to 180, to 120, or even 60 degrees at full speed.

another idea would be to limit the left and right scan field as the boat rolls so its not scanning the surface of the water for false returns.

count74 · August 17, 2022, 11:49am

Does your setup really fill all 72 segments? With my lidar/stepper setup, the scan range is limited to 120° and the FOV of the lidar is ca. 3°. So I can fill 120 / 3 = 40 segments with distance data. I could overlap the scans a bit and the FOV of the lidar is actually 2.7°, if I remember correctly. But the small gaps do not really matter for a moving vehicle where the perspective for the sensor changes all the time.
Your sonar probably has a greater FOV and the servo does 180° max. So take the 180 / SensorFOV = number of segments. And if you use microseconds instead of degrees in the code, you can move the servo in smaller steps and even beyond 0/180 degrees, if your servos allows that.

myservo.writeMicroseconds(1500);
min 700
max 2300

count74 · August 17, 2022, 11:58am

Yes, false positives from the ground are quite annoying. If you drive on a sports field and place a few objects, it is all fine. But as soon as there are slopes, vegetation, curbs it goes all crazy.

geofrancis · August 17, 2022, 6:33pm

I did see that it wasnt always filling every segment, took me a while to figure that out as i was still working out timing issues, I have increased the scan field to 180 degrees and set the resolution to 2 degrees to make sure it could fill them all for testing. The sonar does have a much wider fov when dealing with flat surfaces but when dealing with smaller objects at long range the detection range is much narrower so having fine slices is still useful.

I want to use it to look for fish as well as path planning.

Another idea was to mount it on a gimbal for tilt and roll to prevent ground detection, it doesnt have to be fast or high resolution so servos would work fine.

geofrancis · August 17, 2022, 7:16pm

count74 · August 17, 2022, 7:22pm

With 2 degree steps you get 90 values. If you use writeMicroseconds, you can get the 2.5 degrees you need.

geofrancis · August 17, 2022, 10:12pm

the stops on the servo dont really matter its all about how many sections the pot is split into.

I changed the FOV to 360 and resolution to 5 degrees, then split the pot into 144 sections to get the math to line up. giving me 72 slices now all filled. The key was getting the end points bang on or it wont use all the slots.

I have 3d printed a 2:1 servo gearbox to turn my 180 servo into a 360 servo, if it works il either get a proper robot feedback servo or a 1.5 turn sail winch servo.

geofrancis · September 9, 2022, 9:43pm

@count74 I was looking at the RP-lidar a1, its only 100 and i dont think it would take much to adapt your code to it using the provided example

github.com

robopeak/rplidar_arduino/blob/master/RPLidarDriver/examples/simple_connect/simple_connect.ino

/*
 * RoboPeak RPLIDAR Arduino Example
 * This example shows the easy and common way to fetch data from an RPLIDAR
 * 
 * You may freely add your application code based on this template
 *
 * USAGE:
 * ---------------------------------
 * 1. Download this sketch code to your Arduino board
 * 2. Connect the RPLIDAR's serial port (RX/TX/GND) to your Arduino board (Pin 0 and Pin1)
 * 3. Connect the RPLIDAR's motor ctrl pin to the Arduino board pin 3 
 */
 
/* 
 * Copyright (c) 2014, RoboPeak 
 * All rights reserved.
 * RoboPeak.com
 *
 * Redistribution and use in source and binary forms, with or without modification, 
 * are permitted provided that the following conditions are met:

This file has been truncated. show original

geofrancis · August 29, 2023, 2:45pm

I am working on another version of this that is as small as possible. Its almost there just some tidying up to do.

Using a RP2040 controller, a 9g servo and a Vl53l1x lidar. Im just using the servo pwm to set the position rather than the analog feedback.

count74 · August 29, 2023, 3:43pm

I did that too before using a stepper motor. The servos I used were not happy at all about being operated constantly.

geofrancis · August 29, 2023, 3:52pm

I have burnt one out before testing a setup like this, running them too fast and not giving them time to stop at the end before the change direction really causes them to heat up fast.

there are some tiny stepper motors out there, I will look into adapting one.

this looks nice
https://www.printables.com/en/model/410798-lidar-scanner-head