 
 Distributed as-is; no warranty is given.
 *****************************************************************/
+
+#define USE_MAVLINK //Uncomment this line to use mavlink
+#ifdef USE_MAVLINK
+  #include "/home/bat/sketchbook/libraries/mavlink/ardupilotmega/mavlink.h"        // Mavlink interface
+#endif
 
 #include <Wire.h>
 #include "I2Cdev.h"

         }
   
  now = micros();
  deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
  lastUpdate = now;
  // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer;

  // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention.
  // This is ok by aircraft orientation standards!  
  // Pass gyro rate as rad/s
   MadgwickQuaternionUpdate(ax_g, ay_g, az_g, gx_degps*PI/180.0f, gy_degps*PI/180.0f, gz_degps*PI/180.0f,  my,  mx, mz);
// MahonyQuaternionUpdate(ax_g, ay, az, gx_degps*PI/180.0f, gy_degps*PI/180.0f, gz_degps*PI/180.0f, my, mx, mz);

    // Serial print and/or display at 0.05 s rate independent of data rates
    delt_t = millis() - count;
    if (delt_t > 50) { //update at 20Hz

    // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
    // In this coordinate system, the positive z-axis is down toward Earth. 
    // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
    // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
    // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
    // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
    // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
    // applied in the correct order which for this configuration is yaw, pitch, and then roll.
    // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
      yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
      pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
      roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
      yaw = yaw + LOCAL_DECLINATION_DEG*PI/180.0f; // Declination is difference between magnetic and true north
  
#ifdef USE_MAVLINK
      // Initialize the required buffers 
      mavlink_message_t msg; 
      uint8_t buf[MAVLINK_MAX_PACKET_LEN];
      mavlink_msg_heartbeat_pack(system_id, component_id, &msg, MAV_TYPE_KITE, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
      uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
      Serial.write(buf, len);
      
      //mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
      // uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
      mavlink_msg_raw_imu_pack(system_id, component_id, &msg, time_boot_ms, a1, a2, a3, g1, g2, g3, m1, m2, m3);
      len = mavlink_msg_to_send_buffer(buf, &msg);
      Serial.write(buf, len);
      
      mavlink_msg_scaled_imu_pack(system_id, component_id, &msg, time_boot_ms, ax_g*1000, ay_g*1000, az_g*1000, gx_degps*PI/180.0f*1000, gy_degps*PI/180.0f*1000, gz_degps*PI/180.0f*1000, mx, my, mz);
      len = mavlink_msg_to_send_buffer(buf, &msg);
      Serial.write(buf, len);
      //static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
      //  uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
      mavlink_msg_attitude_pack(system_id, component_id, &msg, time_boot_ms, roll, pitch, yaw, gx_degps*PI/180.0f, gy_degps*PI/180.0f, gz_degps*PI/180.0f);
      len = mavlink_msg_to_send_buffer(buf, &msg);
      Serial.write(buf, len);
      
#else
      Serial.print("ax = "); Serial.print((int)1000*ax_g);  
      Serial.print(" ay = "); Serial.print((int)1000*ay_g); 
      Serial.print(" az = "); Serial.print((int)1000*az_g); Serial.println(" mg");
      Serial.print("gx = "); Serial.print( gx_degps, 2); 
      Serial.print(" gy = "); Serial.print( gy_degps, 2); 
      Serial.print(" gz = "); Serial.print( gz_degps, 2); Serial.println(" deg/s");
      Serial.print("mx = "); Serial.print( (int)mx ); 
      Serial.print(" my = "); Serial.print( (int)my ); 
      Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
      
      Serial.print("q0 = "); Serial.print(q[0]);
      Serial.print(" qx = "); Serial.print(q[1]); 
      Serial.print(" qy = "); Serial.print(q[2]); 
      Serial.print(" qz = "); Serial.println(q[3]); 
      Serial.print("Yaw, Pitch, Roll: ");
      Serial.print(yaw*180.0f / PI, 2);
      Serial.print(", ");
      Serial.print(pitch*180.0f / PI, 2);
      Serial.print(", ");
      Serial.println(roll*180.0f / PI, 2);
      
      Serial.print("rate = "); Serial.print((float)1.0f/deltat, 2); Serial.println(" Hz");
#endif
      
    
      // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and 
      // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz.
      // The filter update rate is determined mostly by the mathematical steps in the respective algorithms, 
      // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:
      // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
      // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively. 
      // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
      // This filter update rate should be fast enough to maintain accurate platform orientation for 
      // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
      // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
      // The 3.3 V 8 MHz Pro Mini is doing pretty well!
  
      count = millis();
    }
}

