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
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.
Regards,
Hunter,
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.
Perhaps use the DEM dataset NASA released? http://asterweb.jpl.nasa.gov/gdem.asp or http://www.cgiar-csi.org/data/srtm-90m-digital-elevation-database-v4-1
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!
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....
Joe
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?
-
1
-
2
-
3
-
4
of 4 Next