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!


Views: 7840

Comment by Russkel on November 27, 2013 at 8:12am

Am I wrong in thinking that this will not work accurately if the target is on a different elevation than the elevation of the ground under the plane?

Comment by UAS_Pilot on November 27, 2013 at 8:23am

This would be a great feature. You should get a kick-starter for this project. Design and produce a complete Gimbal with 360deg rotation with all the parts inside so that it can be plug and play.

Great Idea....


Comment by Hunter Parris on November 27, 2013 at 8:40am
You are correct. I believe there are libraries for elevation calculation based on coordinates. Similar to the way Google Earth can tell elevation under mouse pointer. If that's the case, I'm definitely going to have to have something more robust than the APM for computing. Any ideas?

Great idea for kickstarter! I might look into to that being the only geo-pointing module I've found is made by FLIR, and we can imagine how expensive that is!
Comment by Russkel on November 27, 2013 at 9:13am
Comment by Hunter Parris on November 27, 2013 at 9:20am
Awesome stuff. I'm pretty familiar with GIS, now, how to add that pesky ASCII or Arcdata to arduino...
Comment by Greg Fletcher on November 27, 2013 at 9:20am


It's actually much easier than that. You don't need a seperate IMU gimbal controler code all ready does almost all of the steps needed. I am very familair with the Mount code and could have it working prety quick asssuming we are flying over flat terain. If you target is at a different altitude than you takeoff point then it gets more complicated. The initial soloution would need to be feed to the GCS and google earth would need to come up with an elevation based on the initial flat terain assumtion and this would then change the lat/lon soloution of the target. It would have to go through a few itterations of this process for the true soloution to converge where lat/lon and altitude are all on the same point. It's been something I have been meaning to work on. I have a plane with a pan/tilt gimbal and can maybe test out a flat terain methode this weakend. The elevation based soloution will ned work done in MP and I don't know how to do that.

You should look at the mount.cpp code particularly the calc_gps_target_angle code at line 573.

Comment by Amilcar Lucas on November 27, 2013 at 1:36pm

Basically the existing code does it the other way around:

- You tell the target coordinates (including altitude) and it points the camera there.

What you plan to do is the other way around:

- You point the camera manually at some point, and it tells you the target coordinates (including altitude)

My guess is that you need to invert the transformation matrix, and you are done. But this is just a guess, I have not though about it fully.


Comment by Jake Warren on November 27, 2013 at 2:24pm

I'm no expert at code, but I would love to get a piece of what you're working on. In terms of application, it would be nice to be able to aim your camera gimbal at something, then tell your quad to head to those coordinates and get a closer look. Or same with a fixed-wing, pick a point in the distance and instruct plane to circle around that ground coordinate.

As for finding unknown altitude at a point, could that be pulled from Mission Planner? Can it be pulled out of the maps from Google used in the software?

Great idea you've got

Comment by Evan Coasts on November 28, 2013 at 4:00pm

These guys are doing it already for quite some years now as far as I know: http://magnetsystems.net/

They provide the coordinates not only of the crosshair location but from every pixel of the video window. An impressing technology called video draping that georeferences in real time the video from the UAV to its actual location on the ground.

Click on the video draping link and watch the videos. I have seen this technology in action and it allows you to hover the mouse over the draped video window to get real time geographic coordinates out of it……

Comment by Hunter Parris on November 28, 2013 at 10:48pm

I checked out magnetsystems.net.  They're stuff is pretty impressive.  But it still runs the question of cost.  It seems this is tailored to military and law enforcement so the cost would seem to be out of the reach of the hobbyist.  I want to make a low cost system that will be readily available to everyone, even the hobbyist.  The formulas and implementation doesn't seem that hard, it's just getting it done.  I appreciate everyone's input on this.  You all have been fantastic over the years.


You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service