   if (CALIBACC) {withGyro.doCalibrateAcc(CALIB_TEST);}
   if (CALIBMAG) {withGyro.doCalibrateMag(CALIB_TEST);}
  
-  if (PRINTSCREEN || RPH  || RPH2 || DEVTYPE) {
+  if (PRINTSCREEN || RPH  || RPH2 || DEVTYPE || I2C) {
     withGyro.getRollPitchHeading();  
     noGyro.getRollPitchHeading();               
     printResults();
   }
   
   delay(msecPerCycle);
+}
+
+
+void fillI2CRegisters() {
+  data[ 0] = (int16_t)withGyro.roll  >>   8;  // MSB
+  data[ 1] = (int16_t)withGyro.roll  & 0xFF;  // LSB
+  data[ 2] = (int16_t)withGyro.pitch >>   8;
+  data[ 3] = (int16_t)withGyro.pitch & 0xFF;
+  data[ 4] = (int16_t)withGyro.head  >>   8;
+  data[ 5] = (int16_t)withGyro.head  & 0xFF; 
+  data[ 6] = (int16_t)noGyro.roll    >>   8;
+  data[ 7] = (int16_t)noGyro.roll    & 0xFF;
+  data[ 8] = (int16_t)noGyro.pitch   >>   8;
+  data[ 9] = (int16_t)noGyro.pitch   & 0xFF;    
+  data[10] = (int16_t)noGyro.head    >>   8;
+  data[11] = (int16_t)noGyro.head    & 0xFF;
 }
 
 

   Serial.println(report);
  }
  
 if (RPH)
  {
    Serial.print("RPH ");
    Serial.print((int)withGyro.roll);
    Serial.print(",");

  }

 // comparison of gyro fusion and no gyro, format for realtimeplotter
 if (RPH2) {
    //Serial.print("RPH2 ");
    Serial.print((int)noGyro.roll);
    Serial.print(" ");
