Changeset 102 in flair-src for trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

Timestamp:

10/06/16 11:21:23 (7 years ago)

Author:

Sanahuja Guillaume

Message:

3dmgx3 quaternion

File:

• trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp (modified) (2 diffs)

trunk/lib/FlairSensorActuator/src/Gx3_25_imu_impl.cpp

@@ -113 +113 @@
   }
 
-  // periode a passer an argument (reglable)
+  // periode a passer en argument (reglable)
   // ou plutot laisser la periode geree par le centrale (polling)
   // self->SetPeriodMS(2);
@@ -168 +168 @@
                                              filteredAngRates);
       imuData->SetRawAcc(rawAcc);
+    } else if (command == Gx3_25_imu::Quaternion) {
+      uint8_t response[23] = {0};
+      uint8_t *buf = &response[1];
+      GetData(response, sizeof(response), &imuTime);
+
+      Quaternion quaternion;
+      quaternion.q0 = Dequeue(&buf);
+      quaternion.q1 = Dequeue(&buf);
+      quaternion.q2 = Dequeue(&buf);
+      quaternion.q3 = Dequeue(&buf);
+
+      Vector3D filteredAngRates;
+      filteredAngRates.x = 0;
+      filteredAngRates.y = 0;
+      filteredAngRates.z = 0;
+
+      ahrsData->SetQuaternionAndAngularRates(quaternion,
+                                             filteredAngRates);
     }