Merge pull request #41 from arduino/fix_quaternion

Fix quaternion data parsing
arduino-libraries · Nov 18, 2021 · 8d23f6c · 8d23f6c
2 parents a656f7a + 817a51e
commit 8d23f6c
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 23 deletions.
diff --git a/examples/Standalone/Standalone.ino b/examples/Standalone/Standalone.ino
@@ -12,6 +12,7 @@ SensorXYZ accel(SENSOR_ID_ACC);
 SensorXYZ gyro(SENSOR_ID_GYRO);
 Sensor temp(SENSOR_ID_TEMP);
 Sensor gas(SENSOR_ID_GAS);
+SensorQuaternion rotation(SENSOR_ID_RV);
 
 void setup()
 {
@@ -24,6 +25,7 @@ void setup()
   gyro.begin();
   temp.begin();
   gas.begin();
+  rotation.begin();
 }
 
 void loop()
@@ -40,5 +42,6 @@ void loop()
     Serial.println(String("gyroscope: ") + gyro.toString());
     Serial.println(String("temperature: ") + String(temp.value(),3));
     Serial.println(String("gas: ") + String(gas.value(),3));
+    Serial.println(String("rotation: ") + rotation.toString());
   }
 }
diff --git a/src/sensors/DataParser.cpp b/src/sensors/DataParser.cpp
@@ -16,12 +16,12 @@ void DataParser::parseEuler(SensorDataPacket& data, DataOrientation& vector) {
   parseEuler(data, vector, 1.f);
 }
 
-void DataParser::parseQuaternion(SensorDataPacket& data, DataQuaternion& vector) {
-  vector.x = data.getInt16(0);
-  vector.y = data.getInt16(2);
-  vector.z = data.getInt16(4);
-  vector.w = data.getInt16(6);
-  vector.accuracy = data.getUint16(8);
+void DataParser::parseQuaternion(SensorDataPacket& data, DataQuaternion& vector, float scaleFactor) {
+  vector.x = data.getInt16(0) * scaleFactor;
+  vector.y = data.getInt16(2) * scaleFactor;
+  vector.z = data.getInt16(4) * scaleFactor;
+  vector.w = data.getInt16(6) * scaleFactor;
+  vector.accuracy = data.getUint16(8) * scaleFactor;
 }
 
 void DataParser::parseBSEC(SensorDataPacket& data, DataBSEC& vector) {

diff --git a/src/sensors/DataParser.h b/src/sensors/DataParser.h
@@ -30,18 +30,18 @@ struct DataOrientation {
 };
 
 struct DataQuaternion {
-  int16_t x;
-  int16_t y;
-  int16_t z;
-  int16_t w;
-  uint16_t accuracy;
+  float x;
+  float y;
+  float z;
+  float w;
+  float accuracy;
 
   String toString() {
-    return (String)("Quaternion values - X: " + String(x)
-                    + "   Y: " + String(y)
-                    + "   Z: " + String(z)
-                    + "   W: " + String(w)
-                    + "   Accuracy: " + String(accuracy)
+    return (String)("Quaternion values - X: " + String(x, 3)
+                    + "   Y: " + String(y, 3)
+                    + "   Z: " + String(z, 3)
+                    + "   W: " + String(w, 3)
+                    + "   Accuracy: " + String(accuracy, 3)
                     + "\n");
   }
 };
@@ -75,7 +75,7 @@ class DataParser {
   static void parse3DVector(SensorDataPacket& data, DataXYZ& vector);
   static void parseEuler(SensorDataPacket& data, DataOrientation& vector);
   static void parseEuler(SensorDataPacket& data, DataOrientation& vector, float scaleFactor);
-  static void parseQuaternion(SensorDataPacket& data, DataQuaternion& vector);
+  static void parseQuaternion(SensorDataPacket& data, DataQuaternion& vector, float scaleFactor);
   static void parseBSEC(SensorDataPacket& data, DataBSEC& vector);
   static void parseBSECLegacy(SensorDataPacket& data, DataBSEC& vector);
   static void parseData(SensorDataPacket& data, float& value, float scaleFactor, SensorPayload format);

diff --git a/src/sensors/SensorQuaternion.h b/src/sensors/SensorQuaternion.h
@@ -6,28 +6,37 @@
 class SensorQuaternion : public SensorClass {
 public:
   SensorQuaternion() {} 
-  SensorQuaternion(uint8_t id) : SensorClass(id), _data() {}
+  SensorQuaternion(uint8_t id) : SensorClass(id), _data(), _factor(0.000061035) {}
 
-  int16_t x() 
+  float x() 
   { 
     return _data.x; 
   }
-  int16_t y()
+  float y()
   {
     return _data.y;
   }
-  int16_t z()
+  float z()
   {
     return _data.z;
   }
-  int16_t w()
+  float w()
   {
     return _data.w;
   }
+  float accuracy()
+  {
+    return _data.accuracy;
+  }
+
+  float getFactor()
+  {
+    return _factor;
+  }
 
   void setData(SensorDataPacket &data)
   {
-    DataParser::parseQuaternion(data, _data);
+    DataParser::parseQuaternion(data, _data, _factor);
   }
 
   String toString()
@@ -36,6 +45,7 @@ class SensorQuaternion : public SensorClass {
   }
 
 private:
+  float _factor;
   DataQuaternion _data;
 };