Upload New File

Arduino Code/Schwimmwinkel_FINAL/XSENS_header.cpp

+/*
+  XSENS_header.cpp file for XSENS-Communication with RS232
+  Written by Ricco Müller
+  01.02.2024
+*/
+
+
+// Include .h file
+#include "XSENS_header.h"
+
+
+
+// ************************************** variables ************************************** //
+
+// isProcessed flags for messages storing
+bool acceleration_isProcessed   = 0;
+bool rateofturn_isProcessed     = 0;
+bool magneticfield_isProcessed  = 0;
+bool quaternion_isProcessed     = 0;
+
+// number of recieved IMU data-packets
+uint8_t received_IMU_packets = 0;
+
+// Cache a copy of IMU data
+UnionAcceleration acceleration;     // in m/s^2
+UnionRateOfTurn rate_of_turn;
+UnionMagneticField magnetic_field;
+UnionQuaternion quaternion;
+UnionEulerPRY euler_pry;            // -180 to +180 degress
+
+
+// ************************************** functions ************************************** //
+
+void xsens_processData(uint8_t* message, int messageSize)
+{
+  // Loop that goes through every byte in message 
+  for (int i = 0; i < messageSize; i++)   // -1 damit i+1 verwendet werden darf
+  {
+    // Save the current and next byte as the identifier
+    uint16_t identifier = word(message[i] , message[i+1]);
+    //Serial.println(identifier, HEX);
+    
+    // if() else if().. query whether the current identifier and DLC corresponds to a message identifier and the corresponding DLC
+    if (identifier == ACCELERATION_ID && message[i+2] == ACCELERATION_DLC)
+    {
+      //Serial.println("ACC");   // Debug
+
+      // Query whether this message has already been processed in the entire message ()
+      if(acceleration_isProcessed == 0)
+      {
+        //Serial.println("ACC_1");   // Debug
+
+        // Check whether there are enough bytes for a complete message (Note: "+ message_length" points to the next Byte after this message, therfore -1 is needed)
+        if (i + ACCELERATION_LENGTH - 1 <= messageSize)
+        {
+          //Serial.println("ACC_1_1");   // Debug
+          uint8_t extractedMessage[ACCELERATION_LENGTH];
+          for (int j = 0; j < ACCELERATION_LENGTH; j++)
+          {
+            extractedMessage[j] = message[i + j];
+          }
+
+          // Save the next 3 times 4 bytes
+          for (int j = 0; j <= 2; j++)
+          {
+            acceleration.uint32[j] =  (uint32_t)extractedMessage[3 + j*4] << 24 | // Shift the first data-byte 24 bits to the left
+                                      (uint32_t)extractedMessage[4 + j*4] << 16 | // Shift the second data-byte 16 bits to the left
+                                      (uint32_t)extractedMessage[5 + j*4] << 8  | // Shift the third data-byte 8 bits to the left
+                                      (uint32_t)extractedMessage[6 + j*4];        // Fourth data-byte remains unchanged
+          }
+
+          // Serial.println("X-Beschleunigung:\t" + String(acceleration.uint32[0], HEX) + "\t" + String(acceleration.float32[0]));
+          // Serial.println("Y-Beschleunigung:\t" + String(acceleration.uint32[1], HEX) + "\t" + String(acceleration.float32[1]));
+          // Serial.println("Z-Beschleunigung:\t" + String(acceleration.uint32[2], HEX) + "\t" + String(acceleration.float32[2]));
+
+          acceleration_isProcessed = 1;   // Storing the flag for finishing the message processing  
+          i += ACCELERATION_LENGTH - 1;   // Move index by the message length
+        }
+      }
+    }
+    else if (identifier == RATEOFTURN_ID && message[i+2] == RATEOFTURN_DLC) // RateOfTurn
+    {
+      //Serial.println("RATE");   // Debug
+
+      // Query whether this message has already been processed in the entire message
+      if(rateofturn_isProcessed == 0)
+      {
+        //Serial.println("RATE_1");   // Debug
+
+        // Check whether there are enough bytes for a complete message (Note: "+ message_length" points to the next Byte after this message, therfore -1 is needed)
+        if (i + RATEOFTURN_LENGTH - 1 <= messageSize)
+        {
+          //Serial.println("RATE_1_1");   // Debug
+          uint8_t extractedMessage[RATEOFTURN_LENGTH];
+          for (int j = 0; j < RATEOFTURN_LENGTH; j++)
+          {
+            extractedMessage[j] = message[i + j];
+          }
+
+          // Save the next 3 times 4 bytes
+          for (int j = 0; j <= 2; j++)
+          {
+            rate_of_turn.uint32[j] =  (uint32_t)extractedMessage[3 + j*4] << 24 | // Shift the first data-byte 24 bits to the left
+                                      (uint32_t)extractedMessage[4 + j*4] << 16 | // Shift the second data-byte 16 bits to the left
+                                      (uint32_t)extractedMessage[5 + j*4] << 8  | // Shift the third data-byte 8 bits to the left
+                                      (uint32_t)extractedMessage[6 + j*4];        // Fourth data-byte remains unchanged
+          }
+          
+          // Serial.println("X-Turningrate:\t\t" + String(rate_of_turn.uint32[0], HEX) + "\t" + String(rate_of_turn.float32[0]));
+          // Serial.println("Y-Turningrate:\t\t" + String(rate_of_turn.uint32[1], HEX) + "\t" + String(rate_of_turn.float32[1]));
+          // Serial.println("Z-Turningrate:\t\t" + String(rate_of_turn.uint32[2], HEX) + "\t" + String(rate_of_turn.float32[2]));
+
+          rateofturn_isProcessed = 1;   // Storing the flag for finishing the message processing
+          i += RATEOFTURN_LENGTH - 1;   // Move index by the message length
+        }
+      }
+    }
+    else if (identifier == MAGNETICFIELD_ID && message[i+2] == MAGNETICFIELD_DLC) // MagneticField
+    {
+      //Serial.println("MAG");   // Debug
+
+      // Query whether this message has already been processed in the entire message
+      if(magneticfield_isProcessed == 0)   // Nachricht wurde in diesem Message_Buffer noch nicht verarbeitet
+      {
+        //Serial.println("MAG_1");   // Debug
+
+        // Check whether there are enough bytes for a complete message (Note: "+ message_length" points to the next Byte after this message, therfore -1 is needed)
+        if (i + MAGNETICFIELD_LENGTH + 1 <= messageSize)
+        {
+          //Serial.println("MAG_1_1");   // Debug
+          uint8_t extractedMessage[MAGNETICFIELD_LENGTH];
+          for (int j = 0; j < MAGNETICFIELD_LENGTH; j++)
+          {
+            extractedMessage[j] = message[i + j];
+          }
+
+          // Save the next 3 times 4 bytes
+          for (int j = 0; j <= 2; j++)
+          {
+            magnetic_field.uint32[j] =  (uint32_t)extractedMessage[3 + j*4] << 24 | // Shift the first data-byte 24 bits to the left
+                                        (uint32_t)extractedMessage[4 + j*4] << 16 | // Shift the second data-byte 16 bits to the left
+                                        (uint32_t)extractedMessage[5 + j*4] << 8  | // Shift the third data-byte 8 bits to the left
+                                        (uint32_t)extractedMessage[6 + j*4];        // Fourth data-byte remains unchanged
+          }
+
+          // Serial.println("X-MagneticField:\t" + String(magnetic_field.uint32[0], HEX) + "\t" + String(magnetic_field.float32[0]));
+          // Serial.println("Y-MagneticField:\t" + String(magnetic_field.uint32[1], HEX) + "\t" + String(magnetic_field.float32[1]));
+          // Serial.println("Z-MagneticField:\t" + String(magnetic_field.uint32[2], HEX) + "\t" + String(magnetic_field.float32[2]));
+
+          magneticfield_isProcessed = 1;   // Storing the flag for finishing the message processing
+          i += MAGNETICFIELD_LENGTH - 1;   // Move index by the message length (Note: "-1" because there is a "+1" in the for()-Line above)
+        }
+      }
+    }
+    else if (identifier == QUATERNION_ID && message[i+2] == QUATERNION_DLC) // Quaternion
+    {
+      //Serial.println("QUAT");   // Debug
+
+      // Query whether this message has already been processed in the entire message
+      if(quaternion_isProcessed == 0)
+      {
+        //Serial.println("QUAT_1");   // Debug
+
+        // Check whether there are enough bytes for a complete message (Note: "+ message_length" points to the next Byte after this message, therfore -1 is needed)
+        if (i + QUATERNION_LENGTH + 1 <= messageSize)
+        {
+          //Serial.println("QUAT_1_1");   // Debug
+          uint8_t extractedMessage[QUATERNION_LENGTH];
+          for (int j = 0; j < QUATERNION_LENGTH; j++)
+          {
+            extractedMessage[j] = message[i + j];
+          }
+
+          // Save the next 3 times 4 bytes
+          for (int j = 0; j <= 3; j++)
+          {
+            quaternion.uint32[j] =  (uint32_t)extractedMessage[3 + j*4] << 24 | // Shift the first data-byte 24 bits to the left
+                                    (uint32_t)extractedMessage[4 + j*4] << 16 | // Shift the second data-byte 16 bits to the left
+                                    (uint32_t)extractedMessage[5 + j*4] << 8  | // Shift the third data-byte 8 bits to the left
+                                    (uint32_t)extractedMessage[6 + j*4];        // Fourth data-byte remains unchanged
+            
+          }
+          xsens_quaternion_to_euler(quaternion.float32, euler_pry.float32);
+
+          // Serial.println("Q0-Quaternion:\t\t" + String(quaternion.uint32[0], HEX) + "\t" + String(quaternion.float32[0]));
+          // Serial.println("Q1-Quaternion:\t\t" + String(quaternion.uint32[1], HEX) + "\t" + String(quaternion.float32[1]));
+          // Serial.println("Q2-Quaternion:\t\t" + String(quaternion.uint32[2], HEX) + "\t" + String(quaternion.float32[2]));
+          // Serial.println("Q3-Quaternion:\t\t" + String(quaternion.uint32[3], HEX) + "\t" + String(quaternion.float32[3]));
+
+          // Serial.println("X-Euler:\t\t" + String(euler_pry.uint32[0], HEX) + "\t" + String(euler_pry.float32[0]));
+          // Serial.println("Y-Euler:\t\t" + String(euler_pry.uint32[1], HEX) + "\t" + String(euler_pry.float32[1]));
+          // Serial.println("Z-Euler:\t\t" + String(euler_pry.uint32[2], HEX) + "\t" + String(euler_pry.float32[2]));
+
+
+          quaternion_isProcessed = 1;   // Storing the flag for finishing the message processing
+          i += QUATERNION_LENGTH - 1;   // Move index by the message length
+        }
+      }
+    }
+  }
+  received_IMU_packets = acceleration_isProcessed + rateofturn_isProcessed + magneticfield_isProcessed + quaternion_isProcessed;
+  // if(received_IMU_packets < 4)
+  //   Serial.println("Num:\t--" + String(received_IMU_packets));
+  // else
+  //   Serial.println("Num:\t" + String(received_IMU_packets));
+
+  // Serial.println("X-Euler:\t" + String(euler_pry.float32[0]));
+
+  // Set the hole Array "message" to 0 -> Delete all entries to indicate that the array has been processed
+  memset(message, 0, messageSize);
+}
+
+bool xsens_isValidMessage(uint16_t identifier)
+{
+  if(identifier == ACCELERATION_ID || identifier == RATEOFTURN_ID || identifier == MAGNETICFIELD_ID || identifier == QUATERNION_ID)
+    return 1;
+  else
+    return 0;
+}
+
+void xsens_quaternion_to_euler( float* quaternion, float* euler )
+{
+  float w = quaternion[0];
+  float x = quaternion[1];
+  float y = quaternion[2];
+  float z = quaternion[3];
+
+  // Roll: x-axis
+  float sinr_cosp = 2 * ( w * x + y * z );
+  float cosr_cosp = 1 - 2 * ( x * x + y * y );
+  euler[0]        = (float)atan2( sinr_cosp, cosr_cosp );
+
+  // Pitch: y-axis
+  float sinp = 2 * ( w * y - z * x );
+  if( fabs( sinp ) >= 1 )
+  {
+    euler[1] = (float)copysign( M_PI / 2, sinp );    // use 90 degrees if out of range
+  }
+  else
+  {
+    euler[1] = (float)asin( sinp );
+  }
+
+  // Yaw: z-axis
+  float siny_cosp = 2 * ( w * z + x * y );
+  float cosy_cosp = 1 - 2 * ( y * y + z * z );
+  euler[2]        = (float)atan2( siny_cosp, cosy_cosp );
+   
+  // Convert from radians to degrees
+  euler[0] *= (180.0 / M_PI);
+  euler[1] *= (180.0 / M_PI);
+  euler[2] *= (180.0 / M_PI);
+}
+
+void convertLittleEndianToBigEndian(uint8_t* array, int length)
+{
+  for (int i = 0; i < length / 2; i++) {
+    uint8_t temp = array[i];
+    array[i] = array[length - 1 - i];
+    array[length - 1 - i] = temp;
+  }
+}
\ No newline at end of file