   az_f = az / 16384.0;
 
   // display tab-separated accel/gyro x/y/z values
-   Serial.print("a/g:\t");
-   Serial.print(ax_f); Serial.print("\t");
-   Serial.print(ay_f); Serial.print("\t");
-   Serial.print(az_f); Serial.print("\t");
-   Serial.print(gx); Serial.print("\t");
-   Serial.print(gy); Serial.print("\t");
-   Serial.print(gz);Serial.print("\t");
-   
-   Serial.print("mag:\t");
-   Serial.print(mx); Serial.print("\t");
-   Serial.print(my); Serial.print("\t");
-   Serial.print(mz); Serial.print("\t");
+  Serial.print("a/g:\t");
+  Serial.print(ax_f); Serial.print("\t");
+  Serial.print(ay_f); Serial.print("\t");
+  Serial.print(az_f); Serial.print("\t");
+  Serial.print(gx); Serial.print("\t");
+  Serial.print(gy); Serial.print("\t");
+  Serial.print(gz); Serial.print("\t");
+
+  Serial.print("mag:\t");
+  Serial.print(mx); Serial.print("\t");
+  Serial.print(my); Serial.print("\t");
+  Serial.print(mz); Serial.print("\t");
 
   // To calculate heading in degrees. 0 degree indicates North
-   float heading = atan2(my, mz);
-   if(heading < 0)
-     heading += 2 * M_PI;
-   heading *= 180/M_PI;
-   Serial.print("heading:\t");
-   Serial.print(heading); Serial.print("\t");
-
-   // display measured values if appropriate
-   Serial.print("T/P/A\t");
-   Serial.print(temperature); Serial.print("\t");
-   Serial.print(pressure); Serial.print("\t");
-   Serial.print(altitude);
-   Serial.println("");
-
-  dtostrf(ax_f, 4, 2, tempbuffer[0]);   // -x.x
-  dtostrf(ay_f, 4, 2, tempbuffer[1]);   // -x.x
-  dtostrf(az_f, 4, 2, tempbuffer[2]);   // -x.x
+  float heading = atan2(my, mz);
+  if (heading < 0)
+    heading += 2 * M_PI;
+  heading *= 180 / M_PI;
+  Serial.print("heading:\t");
+  Serial.print(heading); Serial.print("\t");
+
+  // display measured values if appropriate
+  Serial.print("T/P/A\t");
+  Serial.print(temperature); Serial.print("\t");
+  Serial.print(pressure); Serial.print("\t");
+  Serial.print(altitude);
+  Serial.println("");
+
+  dtostrf_sign(ax_f, 4, 2, tempbuffer[0]);   // -x.x
+  dtostrf_sign(ay_f, 4, 2, tempbuffer[1]);   // -x.x
+  dtostrf_sign(az_f, 4, 2, tempbuffer[2]);   // -x.x
   snprintf(displaybuffer[0], 21, "A %s %s %s", tempbuffer[0], tempbuffer[1], tempbuffer[2]);
-  
+
   snprintf(displaybuffer[1], 21, "G: %+4d %+4d %+4d", gx, gy, gz);
 
   dtostrf(heading, 3, 0, tempbuffer[0]);   // xxx
   snprintf(displaybuffer[2], 21, "H: %s", tempbuffer[0]);
 
-  dtostrf(temperature, 5, 2, tempbuffer[0]);   // -xx.x  
-  dtostrf(pressure, 5, 2, tempbuffer[1]);   // -xx.x  
+  dtostrf(temperature, 5, 2, tempbuffer[0]);   // -xx.x
+  dtostrf(pressure, 5, 2, tempbuffer[1]);   // -xx.x
   snprintf(displaybuffer[3], 21, "T: %s, P: %s", tempbuffer[0], tempbuffer[1]);
 
-//   Serial.println(displaybuffer[0]);
-//   Serial.println(displaybuffer[1]);
-//   Serial.println(displaybuffer[2]);
-//   Serial.println(displaybuffer[3]);
+  //   Serial.println(displaybuffer[0]);
+  //   Serial.println(displaybuffer[1]);
+  //   Serial.println(displaybuffer[2]);
+  //   Serial.println(displaybuffer[3]);
 
   display.clearDisplay();
-  display.setCursor(0,0);
+  display.setCursor(0, 0);
   display.println(displaybuffer[0]);
-//  display.println(displaybuffer[1]);
-//  display.println(displaybuffer[2]);
-//  display.println(displaybuffer[3]);
+  //  display.println(displaybuffer[1]);
+  display.println(displaybuffer[2]);
+  //  display.println(displaybuffer[3]);
   delay(10);
   yield();
   display.display();
 
-  delay(100);  
+  delay(100);
 }

  pinMode(BUTTON_A, INPUT_PULLUP);
  pinMode(BUTTON_B, INPUT_PULLUP);
  pinMode(BUTTON_C, INPUT_PULLUP);
  
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.display();
  
  accelgyro.setI2CMasterModeEnabled(false);
  accelgyro.setI2CBypassEnabled(true) ;
  accelgyro.setSleepEnabled(false);

int32_t lastMicros;

#if defined(ESP8266)
#define BUTTON_A 0
#define BUTTON_B 16
#define BUTTON_C 2
#define LED      0
#elif defined(ARDUINO_STM32F2_FEATHER)
#define BUTTON_A PA15
#define BUTTON_B PC7
#define BUTTON_C PC5
#define LED PB5
#else
#define BUTTON_A 9
#define BUTTON_B 6
#define BUTTON_C 5
#define LED      13
#endif

#if (SSD1306_LCDHEIGHT != 32)
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
#endif


// = dtostre() function experimental ============================
char * dtostrf_sign(double number, signed char width, unsigned char prec, char *s) {
    bool negative = false;

    if (isnan(number)) {
        strcpy(s, "nan");
        return s;
    }
    if (isinf(number)) {
        strcpy(s, "inf");
        return s;
    }

    char* out = s;

    int fillme = width; // how many cells to fill for the integer part
    if (prec > 0) {
        fillme -= (prec+1);
    }

    // Handle negative numbers
    if (number < 0.0) {
        negative = true;
        fillme--;
        number = -number;
    } else {
      fillme--;
    }

    // Round correctly so that print(1.999, 2) prints as "2.00"
    // I optimized out most of the divisions
    double rounding = 2.0;
    for (uint8_t i = 0; i < prec; ++i)
        rounding *= 10.0;
    rounding = 1.0 / rounding;

    number += rounding;

    // Figure out how big our number really is
    double tenpow = 1.0;
    int digitcount = 1;
    while (number >= 10.0 * tenpow) {
        tenpow *= 10.0;
        digitcount++;
    }

    number /= tenpow;
    fillme -= digitcount;

    // Pad unused cells with spaces
    while (fillme-- > 0) {
        *out++ = ' ';
    }

    // Handle negative sign
    if (negative) {
      *out++ = '-';
    } else {
      *out++ = ' ';
    }

    // Print the digits, and if necessary, the decimal point
    digitcount += prec;
    int8_t digit = 0;
    while (digitcount-- > 0) {
        digit = (int8_t)number;
        if (digit > 9) digit = 9; // insurance
        *out++ = (char)('0' | digit);
        if ((digitcount == prec) && (prec > 0)) {
            *out++ = '.';
        }
        number -= digit;
        number *= 10.0;
    }

    // make sure the string is terminated
    *out = 0;
    return s;
}
// ====================================================================


void setup() {
#if !defined(ESP8266)
