 		Serial.write((byte)(loadTime >> 8));
 		Serial.write((byte) loadTime & 0xFF);
 	}
+	
+	accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+  	accel2.getAcceleration(&ax2, &ay2, &az2);
+  	mag.getHeading(&mx, &my, &mz);
+	float heading = atan2(my, mx);
+  	int16_t headingint = (heading * 180/M_PI);
+
+  	// process mrl commands
+  	// servo control, sensor, control send & recieve
+  	// oscope, analog polling, digital polling etc..
+  	// mrl messages
+  	//mrl.process();
+ 
+  	// example how to
+  	// send 3 vars to mrl
+  
+  	cnt += 1;
+  	if (cnt%10 == 0) {
+    		startMsg();
+    		append(ay);
+    		append(ax2);
+    		append(headingint);
+    		sendMsg();
+    		cnt = 0;
+	}
 
 
 } // end of big loop

}

void setup() {
	Serial.begin(57600);        // connect to the serial port

	softReset();

*
*/

#include "I2Cdev.h"
#include "MPU6050.h"
#include "ADXL345.h"
#include <HMC5883L.h>

#include <Servo.h>
 
MPU6050 accelgyro;
ADXL345 accel2;
HMC5883L mag;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
 
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t ax2, ay2, az2;
int16_t mx, my, mz;
int cnt;

// ----------  MRLCOMM FUNCTION INTERFACE BEGIN -----------
#define MRLCOMM_VERSION				20
