Changeset 428 in flair-src for trunk/lib/FlairSimulator/src

Timestamp:

Jun 10, 2021, 4:35:17 PM (3 years ago)

Author:

Sanahuja Guillaume

Message:

add accelerometer to simulation

File:

• trunk/lib/FlairSimulator/src/Model_impl.cpp (modified) (2 diffs)

trunk/lib/FlairSimulator/src/Model_impl.cpp

@@ -314 +314 @@
     states_mutex->GetMutex();
     self->CalcModel();
+   
+    Vector3Df acc(self->state[0].Vel.x-self->state[-1].Vel.x,
+                    self->state[0].Vel.y-self->state[-1].Vel.y,
+                    self->state[0].Vel.z-self->state[-1].Vel.z);
+    acc=acc/dT->Value()+Vector3Df(0,0,-9.81);            
+    acc.Rotate(self->state[0].Quat.GetConjugate());
 
     output->GetMutex();
@@ -329 +335 @@
     output->SetValueNoMutex(11, 0, self->state[0].Vel.y);
     output->SetValueNoMutex(12, 0, self->state[0].Vel.z);
-    //todo: put acc and mag
-    output->SetValueNoMutex(13, 0, 0);
-    output->SetValueNoMutex(14, 0, 0);
-    output->SetValueNoMutex(15, 0, 0);
+    output->SetValueNoMutex(13, 0, acc.x);
+    output->SetValueNoMutex(14, 0, acc.y);
+    output->SetValueNoMutex(15, 0, acc.z);
+    //todo: put mag
     output->SetValueNoMutex(16, 0, 0);
     output->SetValueNoMutex(17, 0, 0);