performance optimizations

arduino/src/main.cpp

@@ -3,8 +3,7 @@
 #include <MPU9250.h>
 #include "ultrasonic.hpp"
 
-#define LOOP_INTERVAL_US (10 * 1000)    // 10 * 1000us
-#define   US_INTERVAL_US (400 * 29)     // 400cm * 29us/cm
+#define LOOP_INTERVAL_US (12 * 1000)    // 12 * 1000us
 
 MPU9250 mpu;
 
@@ -26,66 +25,52 @@ void setup() {
 
   Serial.println(F("setup mpu"));
   mpu.setup(0x68); 
-
-  delay(5000);
+  delay(1000);
 
   Serial.println(F("calibrate accel/gyro"));
   mpu.calibrateAccelGyro();
 
-  Serial.println(F("calibrate mag"));
-  mpu.calibrateMag();
   Serial.println(F("FIELDS:\tUS_0\tUS_1\tMAG_X\tMAG_Y\tMAG_Z\tACCEL_X\tACCEL_Y\tACCEL_Z\tGYRO_X\tGYRO_Y\tGYRO_Z\tTEMP\tEXEC_TIME"));
 
 }
 
-unsigned long loopStart = 0, usTransmitStart = 0;
+unsigned long loopStart = 0;
+char outputBuffer[128];
+
 void loop() {
-  if(micros() >= usTransmitStart + US_INTERVAL_US) {
-    usTransmitStart = micros();
-    //emit ultrasound pulse
-    us_transmit();
-    toggleLed();
-  }
 
   if(micros() >= loopStart + LOOP_INTERVAL_US) {
     loopStart = micros();
 
+    //emit ultrasound pulse
+    us_transmit();
+
     //get mpu values
     mpu.update();
 
-    Serial.print(F("DATA:\t"));
-
-    //acustic travel time in microseconds 
-    for(byte i=0; i<2; i++) {
-      Serial.print(us_get_duration(i));
-      Serial.print("\t");
-    }
-
-    //magnetic field
-    for(byte i=0; i<2; i++) {
-      Serial.print(mpu.getMag(i));
-      Serial.print("\t");
-    }
-
-    //accelerometer
-    for(byte i=0; i<2; i++) {
-      Serial.print(mpu.getAcc(i));
-      Serial.print("\t");
-    }
-
-    //gyroscope
-    for(byte i=0; i<2; i++) {
-      Serial.print(mpu.getGyro(i));
-      Serial.print("\t");
-    }
-
-    //temperature
-    Serial.print(mpu.getTemperature());
-    Serial.print("\t");
-
-    //execution time in microseconds
-    Serial.print(micros() - loopStart);
-    Serial.println();
+    snprintf(outputBuffer, sizeof(outputBuffer), "DATA:\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\r\n",
+      //acustic RTT 
+      us_get_duration(0), us_get_duration(1),
+      //magnetic field
+      (long)(mpu.getMagX()*1000),
+      (long)(mpu.getMagY()*1000),
+      (long)(mpu.getMagZ()*1000),
+      //accelerometer
+      (long)(mpu.getAccX()*1000),
+      (long)(mpu.getAccY()*1000),
+      (long)(mpu.getAccZ()*1000),
+      //gyroscope
+      (long)(mpu.getGyroX()*1000),
+      (long)(mpu.getGyroY()*1000),
+      (long)(mpu.getGyroZ()*1000),
+      //temperature
+      (long)(mpu.getTemperature()*1000),
+      //execution time in microseconds
+      micros() - loopStart
+    );
+    Serial.write(outputBuffer);
+
+    toggleLed();
 
   }