Upload New File

Arduino Code/XSENS_Test_self_2/XSENS_Test_self_2.ino

+// Xsens Test ohne externe Library
+#include "XSENS_header.h"
+
+// Pins
+#define RXD2 16
+#define TXD2 17
+
+// Message Buffer
+const int messageSize = 65;           // Size of the buffer: 64 bytes of data in total + 1 Byte buffer = 65
+uint8_t message[messageSize];
+
+// For cylic reading of IMU-data
+const unsigned long processInterval_ms = 10;      // Processing interval in ms    // ******* LÖSCHEN *******
+unsigned long lastProcessTime_ms = 0;             // only for debug reason
+unsigned long lastProcessTime_us = 0;             // only for debug reason
+
+// Normal arduino setup/loop functions
+void setup( void )
+{
+  Serial.begin(115200);
+
+  Serial.println("Willkommen im seriellen Monitor!");
+  Serial.println("Starte 3 Sekunden Start-Up-Pause (warten auf IMU)...");
+  delay(3000);
+  Serial.println("Pause fertig, beginne mit Programmcode:");
+
+  Serial2.setRxBufferSize(128);     // set the buffer-size in bytes of Serial2 (default is 64 bytes)
+  Serial2.begin(115200, SERIAL_8N1, RXD2, TXD2);
+}
+
+void loop( void )
+{
+  // While loop is executed as soon as a new data packet has arrived from the IMU (message Size plus Preamble 4 Bytes)
+  while(Serial2.available() >= messageSize + 4)
+  {
+    // Check for the 4 Preamble-Bytes of the IMU packet
+    // if these bytes come one after the other, then the following 64 bytes are the data bytes 
+    if(Serial2.read() == PREAMBLE_BYTE_1 && Serial2.read() == PREAMBLE_BYTE_2 && Serial2.read() == PREAMBLE_BYTE_3 && Serial2.read() == PREAMBLE_BYTE_4)
+    {
+      Serial.println("********************************");
+      lastProcessTime_ms = millis();
+      //Serial.println(lastProcessTime);
+      Serial.println(micros() - lastProcessTime_us);
+      lastProcessTime_us = micros();
+
+      if(Serial2.available() >= messageSize)
+      {
+        Serial2.readBytes(message, Serial2.available());
+      }
+      else
+      {
+        Serial.print("FATAL ERROR: Not enough Bytes recieved.");
+      }
+
+      // process the data bytes
+      xsens_processData(message, messageSize);
+
+      // delete the rest of the data bytes in the serial-buffer
+      while(Serial2.available() > 0)
+      {
+        Serial2.read(); 
+      }
+
+      // in future XXX_isProcessed flag will be in CAN sending
+      acceleration_isProcessed  = 0;
+      rateofturn_isProcessed    = 0;
+      magneticfield_isProcessed = 0;
+      quaternion_isProcessed    = 0;
+
+      if(millis() - lastProcessTime_ms > 1)
+        Serial.println("\tDELAYED");
+      //Serial.println(millis());
+      Serial.println(micros() - lastProcessTime_us);
+    }
+  }
+}
+
+// B43A
+// 3A55
+// 550D
+// BB72
+// 725D
+// 5D0F
+// F3A
+// 3A5C
+// 5C16
+// 16C4
+// C43F
+// 3F07
+// 701
+// 10B
+// BF0
+// F0FA
+// FAFF
+// FF36
+// 3640
+
+