 
 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"

            mcount++;
           // read the raw sensor data
            mpu.getAcceleration  ( &a1, &a2, &a3  );
            ax = a1*2.0f/32768.0f; // 2 g full range for accelerometer
            ay = a2*2.0f/32768.0f;
            az = a3*2.0f/32768.0f;

            mpu.getRotation  ( &g1, &g2, &g3  );
            gx = g1*250.0f/32768.0f; // 250 deg/s full range for gyroscope
            gy = g2*250.0f/32768.0f;
            gz = g3*250.0f/32768.0f;
//  The gyros and accelerometers can in principle be calibrated in addition to any factory calibration but they are generally
//  pretty accurate. You can check the accelerometer by making sure the reading is +1 g in the positive direction for each axis.
//  The gyro should read zero for each axis when the sensor is at rest. Small or zero adjustment should be needed for these sensors.

uint16_t lastUpdate = 0; // used to calculate integration interval
uint16_t now = 0;        // used to calculate integration interval

float ax_g, ay_g, az_g, gx_degps, gy_degps, gz_degps, mx, my, mz; // variables to hold latest sensor data values 
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method

uint8_t system_id = 100;
uint8_t component_id = 200;
long time_boot_ms;

void setup()
{
