Changeset 102 in flair-src for trunk/lib/FlairSensorActuator

Timestamp:

Oct 6, 2016, 11:21:23 AM (8 years ago)

Author:

Sanahuja Guillaume

Message:

3dmgx3 quaternion

Location:

trunk/lib/FlairSensorActuator/src

Files:

• Gx3_25_imu.cpp (modified) (1 diff)
• Gx3_25_imu.h (modified) (1 diff)
• Gx3_25_imu_impl.cpp (modified) (2 diffs)

trunk/lib/FlairSensorActuator/src/Gx3_25_imu.cpp

@@ -38 +38 @@
   } else if (command == AccelerationAngularRateAndOrientationMatrix) {
     //Thread::Err("oneaxis rotation of rotation matrix is not yet implemented\n");
+  } else if (command == Quaternion) {
+
   } else {
     Thread::Err("command not supported (%i)\n", command);

trunk/lib/FlairSensorActuator/src/Gx3_25_imu.h

@@ -44 +44 @@
     AccelerationAngularRateAndOrientationMatrix =
         0xc8, /*!< Acceleration, angular rate and orientation matrix */
+    Quaternion = 0xdf, /*!< Quaternion */
   };
 

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