       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_s, 2); Serial.println(" Hz");
-      
-      Serial.print(" temp: ");
-      Serial.print(temperature);
-      Serial.print(" degC pres: ");
-      Serial.print(press);
-      Serial.print(" mbar altitude: ");
-      Serial.print(altitude);
-      Serial.println(" m");
 #endif
-      
+      if (outputMode == DEBUG)
+      {
+        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_s, 2); Serial.println(" Hz");
+        
+        Serial.print(" temp: ");
+        Serial.print(temperature);
+        Serial.print(" degC pres: ");
+        Serial.print(press);
+        Serial.print(" mbar altitude: ");
+        Serial.print(altitude);
+        Serial.println(" m");
+      }
     
       // 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.

 time loop() runs, so using delay inside loop can delay
 response.  Multiple bytes of data may be available.
 */
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read(); 
  }
}

void loop()
{   
    
    if (fabs(millis()-last_baro_read_ms)>1./BaroRate)
    {
      last_baro_read_ms = millis();

  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();
  Serial.begin(57600); // Start serial at 57600 bps
  // Reserve bytes for the inputString:
  inputString.reserve(200);

  delay(2000);            

