Merge pull request #41 from arduino/fix_quaternion

Fix quaternion data parsing

Author: giulcioffi

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::parseData(SensorDataPacket& data, float& value, float scaleFactor, SensorPayload format) {

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");
   }
 };
@@ -51,7 +51,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 parseData(SensorDataPacket& data, float& value, float scaleFactor, SensorPayload format);
   static void parseActivity(SensorDataPacket& data, uint16_t value);
 };

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;
 };