3689406838?profile=originalHi guys this is another late night post but I'll try to lay it out right.

I have updated the:

  • Camera tracking code to use the new DCM maths
  • Camera control code to use the APM_RC stuff instead of the outdated servo_out (ashame to be honest as that was very easy to use but the new one is more accurate).
  • Waypoint activity sketch to work with the new location variables (thats the only change but it make the code look harder than it is).


Thanks to Mr Doug Weibel my fears of complexity have been allayed and everything needed to get this going is in one config file. . As I have received interest I am releasing this before I have flight tested under the new APM2.1 release. My previous blog posts show my old camera setup with retraction and targeting but I now have a GoPro so its back to the drawing board.


Camera Sketch Download v1.10 (whenever I edit the file I'll increment - thanks to those wanting more)

Extract the zip file inside your APM2.1 folder with the rest of the sketches, add #include "Camera_Config.h" at the end of the header includes section in the ArduPilotMega.pde, add the do_control_video case in the handles_must void of the commands_logic.pde, add the verify_do_control_video case in the verify_must void of the commands_logic.pde and overwrite test.pde (it has a test for the camera servos).

(Click the picture at the top for an animation for where - not sure why its not doing it always though guess ning isn't gif friendly).

The variables in the config are fully commented so you can change them as you need. This works with all servos of limited range (<360) and the tilt servo only move to vertical so the picture is always the right way up.

For setup of the variables use the comments next to the variables in the config as well as the picture below.



  • Fixed codebase problems, Johann's check created, Euler angle incorrect units now fixed.
  • Added stabilisation (two lines and no mathsm stupid to leave it our really).
  • Added relay triggering method (thanks Kirill).
  • Two methods for waypoint triggering now coded.
  • Added two new triggering methods that should limit vibration.
  • Separated the voids used by APM (mavlink ones and test code).
  • Added test code for the camera servos (pan/pitch, tilt/roll and trigger not retraction).
  • Added PAN_RATIO and TILT_RATIO to variables to change movement of servos easier.
  • Alter test code to move in 15° steps around axis so checking of servo movement ratio can be done.
  • Created new targeting code that works with all limited movement servo (<360) fitted the the bottom or top of the plane.
  • Diagram on the maths for calculating the virtual co-ordinates

Waypoint Triggering - Do something at a waypoint.

  1. Waypoint triggering using the waypoint_check code you need to add waypoint_check(); to case 1 of the medium loop in ArdupilotMega.pde then change the values in waypoint_check to the waypoints you wish to take picture at.
  2. Mission planner to use waypoint triggering (far easier in the field) you need to add picture_time_check(); to case 1 of the medium loop in ArdupilotMega.pde then in the mission planner add do_control_video waypoint to start and stop taking photos at waypoints.

Camera Triggering - use camera

  1. servo_pic - wiggles the servo to press the camera button. You need to adjust the wiggle to press the button correctly without stalling the servo.
  2. throttle_pic - turns off the throttle, waits (time), uses servo_pic to use camera then turns throttle back on. The wait is derived from the # of cycles the medium loop sees (default = 10).
  3. distance_pic - turns off the throttle, waits (distance), uses servo_pic to use camera then turns throttle back on. The wait is controlled by the distance to the waypoint (default <3).

GPS Tracking - its only pointing at home still but with waypoint planning hopefully working this may be updated soon so it will actually be useful :) until then I wouldn't worry about it.

Currently grabs waypoint 0 (home) and tracks that.

Add GPS_track(); to the fast loop of the ArdupilotMega.pde just before the } at the end.



Add cam_stabilisation(); to the fast loop of the ArdupilotMega.pde just before the } at the end. 


Testing your camera setup

With everything in your airframe and ready to test the connections to your servo setup and that you have configured it correctly use the test section of the CLI and use "camera". The test will:

  1. pan left to right in 15° increments
  2. pan right to left in 15° increments
  3. tilt up to down in 15° increments
  4. tilt down to up in 15° increments
  5. take a picture

To check angle increments use this picture around your servo. If the movement is less than 15° you need to increase the ratio value and if it is more you need to decrease the ratio value. If everything moves as it is supposed to well done you have configured the entire setup and are ready to use it on a flight.


I used the previous iteration (APM1.13) of this code happily but with the changes I may have borked it somehow. The config file can be changed by anyone and has details on what each variable means.

If you do not understand any of this do not use it. This is for those pushing the envelope.


Disclaimer: I have not flight tested this code yet, I nor DIYdrones take any responsibility for damage caused.



E-mail me when people leave their comments –

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

Join diydrones


  • This code worked way back in the day (over 18 months ago) and has been rewritten massively by Sandro, Amilcar, Joe and myself. This code may be modified to work but its pretty cycle hungry so I would not recommend it with the current release of code.

    The new version is out and in the mission planner for setup along with guidance in the wiki. This is only for reference to those who wish to know where I started it all and how.

  • Hello Richard I'm trying to make a trigger on each waypoint where pass my hexacoptero.
    To do this I made an external circuit that receives a logic signal, turn on, takes a photo and then turn off my gopro hero 2.

    I was reading your posts and your code and I got some questions:
    1) This code can be used in a hexacoptero? (There's delay on it)
    2) If I want to use just the part that take pictures on waypoints, I have to use the full code?
    3) I want to take pictures at all waypoints. At the moment, I have to compile the code every time that I want to fly? Because the waypoints where the pictures will be taken are determined in code. How can I solve this?

    Ps: I'm using APM 2.5 and I'm sorry for my bad english.

  • Distributor

    Hello Ritchie, what APM version should I download to try the camera tracking? I ask because I don't know if it will work on all new versions (we are on 2.28 today...) Great job and thanks for sharing with us. Guto (Brazil)

  • Hi Ritchie,


    I've been trying to implement your code.  So far, all I'd like to do is camera stabilization but I'm getting the following compilation errors:


    ArduPilotMega_ritchie_cam_stab.cpp: In function 'int8_t test_radio(uint8_t, const Menu::arg*)':
    test:165: error: 'set_servos_4' was not declared in this scope
    ArduPilotMega_ritchie_cam_stab.cpp: In function 'int8_t test_current(uint8_t, const Menu::arg*)':
    test:273: error: 'set_servos_4' was not declared in this scope
    ArduPilotMega_ritchie_cam_stab.cpp: In function 'int8_t test_dipswitches(uint8_t, const Menu::arg*)':
    test:398: error: 'mix_mode' was not declared in this scope
    test:399: error: 'reverse_roll' was not declared in this scope
    test:399: error: 'reverse_pitch' was not declared in this scope
    test:399: error: 'reverse_rudder' was not declared in this scope
    test:404: error: 'reverse_elevons' was not declared in this scope
    test:404: error: 'reverse_ch1_elevon' was not declared in this scope
    test:404: error: 'reverse_ch2_elevon' was not declared in this scope
    ArduPilotMega_ritchie_cam_stab.cpp: In function 'int8_t test_airspeed(uint8_t, const Menu::arg*)':
    test:585: error: 'airspeed_enabled' was not declared in this scope
    test:589: error: 'airspeed_enabled' was not declared in this scope
    test:591: error: 'airspeed_enabled' was not declared in this scope


    Any help would be appreciated.


  • The latest code now lives on the APM_Camera SVN branch.

    I have a patch the removes all the need to change the source code in order to configure the camera.

    All the configuration is done via the APM mission planner GUI.


    It supports pan-tilt and tilt-roll camera mounts.

    For those of you that want to play with it, here it is but I have improved it since then. Now it uses 1Kb less memory and automaticaly detects the gimbal type.

    Because I plan to work even more on it, I will not post the latest version here unless someone asks me to.



    Amilcar Lucas.

  • This is a code suggestion for the pending camera functionality addition. I will throw out the idea of leveling the aircraft right before and during the camera triggering. This might be very similar to the "throttle_pic" functionality. For vertical aerial photography, leveling the wings for a zero roll might be an excellent option right at the moment of photo trigger. This would be an alternative to creating a roll stabilized mount that needs to be driven by an extra servo to level the camera while taking photos. This functionality can dramatically simplify camera mounting and yield much better results for vertical mapping photos.
  • I think your .gif is nice, but not practical, a diff is easier.

    Index: ArduPilotMega.pde
    --- ArduPilotMega.pde   (revision 3351)
    +++ ArduPilotMega.pde   (working copy)
    @@ -52,6 +52,7 @@
     #include "Parameters.h"
     #include "GCS.h"
     #include "HIL.h"
    +#include "Camera_Config.h"
     // Serial ports
    @@ -555,6 +556,8 @@
            // XXX this should be absorbed into the above,
            // or be a "GCS fast loop" interface
    +       cam_stabilisation();
     static void medium_loop()
    Index: commands_logic.pde
    --- commands_logic.pde  (revision 3351)
    +++ commands_logic.pde  (working copy)
    @@ -40,6 +40,10 @@
    +               case MAV_CMD_DO_CONTROL_VIDEO:
    +                       do_control_video();
    +                       break;
    @@ -159,6 +163,10 @@
                            return verify_RTL();
    +               case MAV_CMD_DO_CONTROL_VIDEO:
    +                       return verify_do_control_video();
    +                       break;
                            gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd"));
                            return false;


  • I am new to DIY and amatuer UAVs.  Over two decades ago, as a part of Lockheed, I was intimately involved in developing and refining sensor control for the Army UAV, Aquila.  I am very impressed at how far the DIY community has taken this.  Now that we have pointing at a waypoint (target) pretty much in the works, I hope we get to the following progressively:

    1.  Automatically loiter the AC around the target;

    2.  "Walk" the target either on a map but most preferably on the live video (and the AC loiters along) from real-time operator input;

    3.  Have the APM generate a path for the "walking" target (using existing APM grid line routines);

    4.  Automatically adjust the "walk rate" based on the real-time distance to the "target" cow, horse or whatever.


    I believe this would prove to be a very user friendly interface to many who have tasks (finding lost stock, search, etc).

    I am now acquiring RC gear and APM hardware and intend to pursue this.

    Again, thank you Ritchie.

  • Will your code work with APM 2.2?


  • *swish and flick*
    I was not asked to accept you but you have appeared and my MyPage has not worked for a little while I'm not sure why but I get that error too in IE or nothing in Safari. The code is getting a big overhaul as it is but is not near human consumption yet.

    The only change was a weird compilation error that I fixed but as nothing was added I just reloaded it. I'll keep bashing.
This reply was deleted.