Hello,

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
  if(fastTimer.trigger()){
    read_radio();
    read_AHRS();
    read_GPS();
    update_yaw();
    update_roll();
    update_pitch();
    set_servos();
  }

  if(fiftyHerzTimer.trigger()){
  
  }

  if(tenHerzTimer.trigger()){
    read_compass();
  
    //print out attitude, location, etc.
    print_attitude();
    print_target_attitude();
    print_location();
    print_channels();
    print_servos();
    print_mode();
  }

  if(oneHerzTimer.trigger()){
    read_mode_switch();
  }

}

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

static void read_AHRS(void){
  ahrs.update();
  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;
}