Overlaying GPS Coordinates for Camera Crosshairs


Hey Guys!

I am working on a project to allow us to implement GPS coordinates for the location of the crosshairs of a PTZ camera, other wise known as Geo-pointing.  This will essentially give us the LAT/LONG of what we are looking at.  

I have found several methods to do this, but I'm looking for the simplest way but accurate as possible.  I am fairly new to modifying APM and to MAVLink so instruction and suggestions are very welcome!  Here is what I have in mind:

In order to get this:

LAT/LON: Self explanatory

BRG: Bearing

SRG: Slant Range (Range from Aircraft to location)

GRG: Ground Range (Range from coordinate to coordinate)

I have a formula to triangulate the position of where the crosshairs are pointing relative to the aircraft by using trigonometry.  Here is the example:



The system will incorporate 1x APM 2.5 w/ GPS, 3DR Telemetry radios, 1x IMU w/ compass for camera gimbal, 2x MinimOSD boards, 2 cameras, 1x video TX with two-way camera switcher.


Application extracts altitude from APM and camera angle from Camera IMU and finds the tangent and cosine of the angle and finds the missing sides.  This will give us the ground distance and slant range respectively.

TAN(CAng)*Alt = GndDist     COS(CAng)*Alt = Slant

At this point the slant and ground range can be transmitted to the video feed.

Next, we use the aircraft's GPS coordinates from the APM and the ground distance to find what coordinate we are looking at on the ground:

aL-aircraft Latitude

aO-aircraft Longitude

cL-crosshair Latitude

cO-crosshair Longitude

A-aircraft Altitude

D-ground Distance

B-Bearing from Gimbal Compass


cL =ASIN(SIN(aL*PI()/180)*COS((D=TAN(A*PI()/180)/6378.137) + COS(aL*PI()/180)*SIN((D=TAN(A*PI()/180)/6378.137)*COS(B*PI()/180))*180/PI()

cO =((aO*PI()/180) + ATAN2(COS((D=TAN(A*PI()/180)/6378.137)-SIN(aL*PI()/180)*SIN(cL*PI()/180), SIN(B*PI()/180)*SIN((D=TAN(A*PI()/180)/6378.137)*COS(aL*PI()/180)))*180/PI()

These formulas are in excel format since it is what I used to test these.

This should give us the coordinate under the crosshair.

Next, I would like for this information to be sent to the second OSD for the gimbal camera.

Now my question is, how do I implement this?

I would assume that I can attach the IMU and compass via analog sensor input, however, I have no idea how to write code for it.

I also do not know how to write the code for the calculations to the APM in order for the APM to do all the formulas.  Can the APM handle it or do I need a separate processor?

How do I create a MAVLink message for the new coordinates and inject into the data stream?

How do I write the code to the MinimOSD to retrieve the new coordinates and display it?

These might sound like noob questions, but I come from a radio frequency engineer background and just now getting into programming.  I would love to learn how to do this stuff but there is very little out there on what I'm wanting to do exactly.  Sure, I can make an LED blink, and I know that's the basic skills, but I need to know how exactly to interface this with the APM and sensors.

Once again, all of your help is greatly appreciated and I very much welcome discussion and suggestions on a better way to do this!


E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • How did you solve the problem of determining the crosshair position's elevation?

    If your UAV has a laser rangefinder, you can triangulate the target's elevation and ground range from the UAV to the target. This data, along with the UAV's MSL altitude, lat/long, and sensor pan/tilt angles are all you need to compute the crosshair's elevation and lat/long. No need to derive target elevation from DTED or anywhere else.

  • If there is anything I can help with let me know. I would like this function to use with SAR. That without could relay locations to ground teams.

    Just a thought. In your development if there can be a way, ie hot key, to save the location as a POI. That way you could lock the camera to it, investigate it later, or generate a list for others to look at.
  • I have all of the formulas I need...now I just need to implement all of it into program format.  That's the hard part.  

    Jay, I know what you mean.  For such a large community, there seems to not be a large support group for stuff like this.  I really do despise how this community has evolved into multicopters and have almost completely forgotten about fixed wing.  I looked into the OSD doing the calcs but realize that an OSD should have one function, overlaying data.  With complex math and sub-minute calculations, a more robust processor is needed than any hobby-type OSD can provide.  I'm actually looking to do the OSD as a computer program running on the GCS computer.  That way, if the video or OSD board fails (which the MinimOSD isn't that robust), I can still fly IFR if needed as long as the data and autopilot are still cranking out measurements.  Pluf the Arduino board that the MinimOSD is based on has VERY little memory left after all the programming for the OSD is done.

  • I am glad to see you are getting more help with this than I did a year or so ago. Look at having the OSD do the calculations. 

  • Hunter-

    Have you made any progress on this?  I'm still in study mode, so there isn't much thought going into much anything else at the moment, but I thought I would drop a quick note to see how things are going.


  • I did notice the similarities...uncanny at first, but when I learned your background, is was obvious why.  As for LIDAR, that would be very cost prohibitive at this time, in my opinion.  I believe 3D photogrammetry would be a much better, cost effective option.  I read a white paper on how 3D Photogrammetry is just as good if not better than LIDAR at RC altitudes with the right equipment at a fraction of the cost .  You can generate some serious point cloud renderings that are pretty darn accurate at or below 400' AGL with something like the Leica Photogrammetry Suite.

  • I think DTED0 is accurate to 1km??

  • On my home computer, civilians only have access to DTED0 which is insufficient for navigation, especially on such a small scale.  DTED1 and DTED2 are reserved for military and government use only and can only be released at the discretion of NGA.  Laser range finders or LIDAR would come in handy here to map elevation while in flight since we can't get in on that guh'ment action!

  • AGM-114 Hellfires are laser guided! :)

  • I am quite happy to learn that the MQ-1s and MQ-9s use elevation data and not some crazy out-of-hobbiest-budget fancy-pants-super-duper-range-finder-laser-doohickey.

This reply was deleted.