imu_test/src/main.cpp
void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
  float roll = atan2(ay, az);
  float pitch = atan2(-ax, sqrt(ay * ay + az * az));

  float heading;
  if (my == 0)
    heading = (mx < 0) ? PI : 0;
  else
    heading = atan2(mx, my);

  heading -= DECLINATION * PI / 180;

  if (heading > PI) heading -= (2 * PI);
  else if (heading < -PI) heading += (2 * PI);

  // Convert everything from radians to degrees:
  heading *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll  *= 180.0 / PI;
  //Serial.print("janky estimated angle is ");
  //Serial.println(gyro_angle);
  //Serial.print("Pitch, Roll: ");
  Serial.print(pitch, 2);
  Serial.print(", ");
  Serial.println(roll, 2);
  //Serial.print("Heading: "); Serial.println(heading, 2);
}
// Calculate pitch, roll, and heading.
// Pitch/roll calculations take from this app note:
// http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
// Heading calculations taken from this app note:
// http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
Calculate pitch, roll, and heading.
Calculate pitch roll and heading .
void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
  float roll = atan2(ay, az);
  float pitch = atan2(-ax, sqrt(ay * ay + az * az));
  float heading;
  if (my == 0)
    heading = (mx < 0) ? PI : 0;
  else
    heading = atan2(mx, my);
  heading -= DECLINATION * PI / 180;
  if (heading > PI) heading -= (2 * PI);
  else if (heading < -PI) heading += (2 * PI);
  heading *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll  *= 180.0 / PI;
  Serial.print(pitch, 2);
  Serial.print(", ");
  Serial.println(roll, 2);
}
void printAttitude ( float ax , float ay , float az , float mx , float my , float mz ) { float roll = atan2 ( ay , az ) ; float pitch = atan2 ( - ax , sqrt ( ay * ay + az * az ) ) ; float heading ; if ( my == 0 ) heading = ( mx < 0 ) ? PI : 0 ; else heading = atan2 ( mx , my ) ; heading -= DECLINATION * PI / 180 ; if ( heading > PI ) heading -= ( 2 * PI ) ; else if ( heading < - PI ) heading += ( 2 * PI ) ; heading *= 180.0 / PI ; pitch *= 180.0 / PI ; roll *= 180.0 / PI ; Serial . print ( pitch , 2 ) ; Serial . print ( " " ) ; Serial . println ( roll , 2 ) ; }
Calculate pitch, roll, and heading.
Calculate pitch roll and heading .
// Convert everything from radians to degrees: //Serial.print("janky estimated angle is "); //Serial.println(gyro_angle); //Serial.print("Pitch, Roll: "); //Serial.print("Heading: "); Serial.println(heading, 2);