3 point laser tracking

This is a long detailed post – grab a cuppa if you intend to plough through.

Here’s the plan for Phoebe.

There are 3 downward facing red lasers. Two are attached to the underside of her rear arms both pointing in parallel along Phoebe’s Z axis i.e. if Phoebe is horizontal then the laser beams are pointing vertically downwards.  The third laser is handheld by a human. All are probably 5mW / Class 2 – although the power rating may need to be reduced to conform with legislation which is unclear.  5mW is safe due to the human blink reaction; 1mW is safe as long as it’s not viewed through a focusing device such as a lens.

The RaspiCam with NoIR filter is fitted in the center of Phoebe’s lower plate, also facing along her Z axis.  A red camera-style gel filter is fitted over it in the expectation that this will increase the contrast between the laser dots and the rest of the background.  The camera is set to ISO 800 – its maximum sensitivity.  The photos are low resolution to reduce the processing required.  Each shot is taken in YUV mode, meaning the first half of the photo data is luminance / brightness / contrast information.  Photos are taken as fast as possible, which may actually be only a few per second due to the lighting conditions.  The camera code runs on a separate thread from Phoebe’s main flight control code.

A typical flight takes places as follows:

Immediately prior to each flight, the camera is turned on and feeds its output into an OS FIFO.

Quadcopter take-off code is unchanged using the standard flight plan configuration to attain an approximation of the desired hover height (e.g. 3s at 0.3m/s gives roughly 90cm hover height).

Once at hover each motion processing loop, the motion processing code checks whether the FIFO is not empty, and if not empty, it is emptied and the last batch of camera data (i.e. the last shot taken) is processed.

It is scanned for bright dots and their position in the frame stored.  By using the red filter and the Y channel (brightness / contract / luminance of YUV)  from the camera, and because the lasers are fixed in Phoebe’s frame with respect to the camera, the dots should stand out in the photo, and lie between the center and the bottom corners of the photo.  If bright spots are detected in this area, there is a very high level of confidence that these are the red dots from the frame lasers.  The distance between the dots in pixels is proportional to the actual height in meters based upon the camera lens focal length.

This pixel-separation height is saved at the first pass in the hover phase and used thereafter as the target height; deviation in the separation of the dots compared to the target dot separation means a height error which is fed as target change to the vertical velocity PID.

Once the height is processed as above, any third similarly bright dot is assumed to be from the human laser.  If such a dot is not found in 5 seconds, the code moved irreversibly to descent mode.

However if a 3rd dot is found in that 5s period then it’s position relative to the frame laser dots provides targets to

  • the yaw PID so that the 3 dots form an isosceles triangle with the quad laser dots at the base and the human dot is at the peak
  • the horizontal velocity PID so that the 3 dots form an equilateral triangle.

Loss of the human dot returns to frame laser dot mode for 5 seconds to reacquire the lost human dot which if not found, triggers the irreversible standard descent mode based upon the flight plan alone.

Similarly, loss of either of the frame laser dots triggers irreversible standard descent mode but without any wait for reacquisition of the missing frame dot.

This should provide stable hover flight for as long as the battery lasts, with the option of following the human dot to take the Quad for a “walk” on a “laser leash”.

Sounds like a plan, doesn’t is?  Some details and concerns, primarily so I don’t forget:

  • -l per-flight command line control of laser tracking mode
  • mosfets to switch on lasers based on the above?  Depends on whether GPIO pins can drive 2 10mA lasers
  • Is a new PCB needed to expose GPIO switch pin for lasers? If so, don’t forget the pull down resistor!
  • Prototype can be done in complete isolation from Phoebe, using one of my many spare RPi’s along with some LEGO and my as yet unused PaPiRus e-paper screen to show dot location.  This could could then be easily battery powered for testing.


  • Merging a successful prototype into Phoebe requires an ‘A3’ and a rework of the PSU – currently direct feeding 5V into the RPi backfeeds the regulator (which would normally be taking input from the LiPo) causing to heat up significantly
  • None of this can take place before I’ve finished and bagged up the GPIO tutorial for the next Cotswold Jam on 30th April.

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.