I am trying to build a sweeping lidar with my TFmini, to use it with my rover. I placed it on a servo and let the servo rotate to 45, 90 and 135. I then send the distances to my pixhawk via mavlink distance_sensor messages.
The problem I have is, even in single meassurement mode, the TFmini seems to send “old” data, from previous servo positions. I now uploaded the example sketch from the tfmini library I found here: https://github.com/opensensinglab/tfmini
I use the single messurement example and add a delay of 1sec (delay(1000)). What I was expecting is to get a new reading with each meassurement/every second, but if I place an object in front of the sensor, I get up to 5 readings with the old unobstructed distance in arduinos serial monitor, before I see the new value. The same is true for removing the object. The number of “old” readings varies, but I never got the new reading immediatly.
I guess, this is happening with my servo mavlink sketch too. I need to have a delay between the servo command and the sensor reading to give the servo time to reach its position. If anyone could give me a hint, what to change or what might be wrong here, I would be very thankful.
Since anyone also includes me :-), I will answer my own question.
The problem seems to be old data in the serial (softserial) buffer or somewhere else. By killing and rebuilding the softserial port directly before and after the sensor is read, I now get the new values immediatly, even missing objects that are in front of the sensor for less than a second. Now I try to incorporate this into my full sketch.
It works now. I set the delay to 200ms for each step to save the servo from dying an early death. I use a cheapo micro servo with plastic gears for now. I will use a fast standard size servo later.
The only problem that remains is the “Bad Lidar Health” message together with the very annoying speaker beeps, although the sensor is working.
The “Bad Lidar health” message seems to be related to a timer, if a sensor does not update for a given time, the message is triggered. I reduced the wait time to 150ms for each step and the message is gone. 200ms meant 600ms between updates for the 45° and 135° positions, I guess this was too long.
I builded a prototype for scanning:
Its a a micro servo and controlers inserted in a TicTac Box
You can see the Digistump (Attiny85) clone that is used to control the servo. The Arduino Pro Mini sends 3 bits vector codes (3 digital pins) corresponding to the pwm code for azimuth. The output is MavLink avoidance message with the beam direction and distance.
As you note, the problem is mainly dealing with the ‘‘timing’’ of the servo.
I now tried an EZ-Robot HD servo. I thought a standard size servo would have more thermal “capacity”, but it gets really hot after a few minutes of sweeping. It seems to have a lot of inertia, the stop at 90 degrees is almost not noticable and it freewheels a lot if I cut power while it is moving. That means stopping and accelerating at 45/135 degrees takes a lot of force. I will look for a fast standard size servo with nylon gears.
You might be right, but it will take some time to reduce my stockpile of leftover servos.
How did you solve the problem with the old data being transmitted? Or did this not occure, because you use your own function to read the TFMini? If that is the case, it might be a problem of the library I am using.
It looks like your are now flushing the buffer before reading so you get old values. Working with softserial can get someone insane. !!
Sending over mavlink is somewhat easier because the FC is dealing with continuous streams instead of a sensor.