Changeset 186 in flair-src for trunk/lib/FlairSensorActuator/src/SimuImu.cpp

Timestamp:

Jun 23, 2017, 5:33:03 PM (7 years ago)

Author:

Sanahuja Guillaume

Message:

maj imu

File:

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

trunk/lib/FlairSensorActuator/src/SimuImu.cpp

@@ -79 +79 @@
     state.wy = input->ValueNoMutex(8, 0);
     state.wz = input->ValueNoMutex(9, 0);
+    state.ax = input->ValueNoMutex(13, 0);
+    state.ay = input->ValueNoMutex(14, 0);
+    state.az = input->ValueNoMutex(15, 0);
+    state.mx = input->ValueNoMutex(16, 0);
+    state.my = input->ValueNoMutex(17, 0);
+    state.mz = input->ValueNoMutex(18, 0);
     input->ReleaseMutex();
 
@@ -108 +114 @@
     Quaternion  quaternion(state.q0, state.q1, state.q2, state.q3);
     Vector3Df angRate(state.wx, state.wy, state.wz);
+    Vector3Df rawAcc(state.ax, state.ay, state.az);
+    Vector3Df rawMag(state.mx, state.my, state.mz);
+    Vector3Df rawGyr(state.wx, state.wy, state.wz);
     ApplyRotation(angRate);
     ApplyRotation(quaternion);
+    ApplyRotation(rawAcc);
+    ApplyRotation(rawMag);
+    ApplyRotation(rawGyr);
     ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
-
+    imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
     imuData->SetDataTime(GetTime());
     ahrsData->SetDataTime(GetTime());