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.

Method:

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!

-Hunter

## Comments

This is an interesting blogpost. I, too, am looking at something similar. I am working on C++ video processing and would like to be involved in your project if possible. I have some interesting ideas that would extend your system capabilities greatly.

Many Thanks.

Thanks

HI Hunter,

is there any update regarding your project, we have many inquiries for your solutions. When do you think it will be ready?

HI Hunter,

Where will you read the coordination, on the mission planner or on the OSD?

Regarding the target elevation, you need to rely on Google earth as you are saying.

As for the Plane, why you want to calculate the plane direction and doing some math?

Elios,

We are trying to make this as low cost as possible, just for this purpose...S&R. I have looked into crowd sourcing but finding the programmer was the hard part. So now I have a small team and we are moving forward with our small business plan. This software will be part of it. We will make it available for free to all who contribute to either source code, hardware or software, or any investments. To all others, we will try and make it as affordable as possible. Our organization is called Aerobotics Group and we are serious about civil RPAs doing great things for our communities.

Kevin, a rangefinder is very costly and heavy for smaller aircraft. I'm trying to make this software-based that can interface with most cameras and gimbals. All you will need, in theory, is a computer, camera, video stream, and gimbal.

Great Job, We are looking to have the solution when it is ready. It is a great solution to save missing people in snow, mountains....by knowing their exact location.

HI Hunter,

Is there any update regarding your project? It is a very interesting solution if it can be done.

Next