Getting Reliable Attitude & Heading Data


For the sake of better understanding APM guts I've been working on what I've been calling ArduGimbal.  I've just assembled code together looking at Arducopter 2.5.5 for how to structure the code and Arduino AHRS example & Compass example code to get heading and attitude.  I'm using an APM2.

I've made a triple axis gimbal out of a Hobbyking gimbal, a prop balancer and the main rotor gear from a 450 sized heli.


I can't seem to get a reliable heading reading, I'm not sure what the cause is.

If anyone has any advice or example code for getting the correct heading I would be very appreciative.  I'm getting weird readings, like rotating the gimbal reads 360 - 270 degrees seemingly regardless of pitch & roll.

I can use gyro data and rate PIDs to resist yaw, or to yaw at a desired rotational speed, but I can't get it to hold a specific heading.

I'm reading attitude at 100hz and the compass at 10hz

Here's code:

void loop(void){

  // 100 herz


    //print out attitude, location, etc.



static void read_compass(void){
    compass.calculate(ToRad(roll), ToRad(pitch));
    yaw = 180.0 + ToDeg(compass.heading);

static void read_AHRS(void){
  Vector3f omega;

  omega = imu.get_gyro();
  pitch = ToDeg(ahrs.pitch);
  roll = ToDeg(ahrs.roll);

  //This is a filter of sorts, not sure if it's needed, but it seems to reduce wobbles
  pitch_rate = 0.10 * (ToDeg(omega.y) - pitch_rate) + pitch_rate;
  roll_rate = 0.10 * (ToDeg(omega.x) - roll_rate) + roll_rate;
  yaw_rate = 0.05 * (ToDeg(omega.z) - yaw_rate) + yaw_rate;

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

Join diydrones

Email me when people reply –


  • Moderator

    Have you considered looking at the code for the ArduPlane instead? The reason I ask is if all you want is a static heading direction, that is easy enough. However, as your gimbal moves through space, the heading to your point of interest is likely to change. See for an understanding of why I ask. 

    If you want to do this, also take a look at the reply to

    Now, to do this well, you will need to change it to a continuous servo, which means you would need to change the code, rather than setting the servo to a positional value, you'd want to do some quick math, decide which direction the gimbal needs to rotate, and decide how quickly you want it to move until you bring the gimbal to "zero" and then set the yaw servo to the "stop" value. I have been thinking about something similar, and plan to use PIDs to drive the continuous rotation gimbals so I can achieve better tracking. Just something to consider before you get in too deep. Since a lot of this code is in the libraries, it might be accessible to either platform, but I have not looked at if/how it is implemented in ArduCopter. You'll also find examples in the code written for antenna tracking, and that might be an even easier adaptation, since it is intended to point at a specific GPS location. FInd an example created for a mobile antenna and you've got everything you need without writing much code, just some tuning. Maybe. 

  • Pretty awesome! I'm interested to know how to read the heading as well. 

This reply was deleted.