Changeset 214 in flair-src for trunk/lib/FlairSensorActuator/src/SimuBldc.cpp

Timestamp:

02/07/18 17:49:27 (6 years ago)

Author:

Sanahuja Guillaume

Message:

matrix

File:

• trunk/lib/FlairSensorActuator/src/SimuBldc.cpp (modified) (4 diffs)

trunk/lib/FlairSensorActuator/src/SimuBldc.cpp

@@ -21 +21 @@
 #include <GroupBox.h>
 #include <SharedMem.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <sstream>
+#include <string.h>
 
 using std::string;
@@ -36 +37 @@
     : Bldc(parent, layout, name, motors_count) {
   shmem =
-      new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float));
+      new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float)+sizeof(Time));
 
   GroupBox *groupbox = new GroupBox(layout->NewRow(), "simubldc");
   k = new DoubleSpinBox(groupbox->NewRow(), "k driver:", 0, 10000, 1);
   
+  buf=(char*)malloc(motors_count * sizeof(float)+sizeof(Time));
+  if(buf==NULL) {
+    Err("error creating buffer\n");
+    return;
+  }
   SetIsReady(true);
 }
@@ -48 +54 @@
     : Bldc(parent, name, motors_count) {
   shmem =
-      new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float));
+      new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float)+sizeof(Time));
+      
+  buf=(char*)malloc(motors_count * sizeof(float)+sizeof(Time));
+  if(buf==NULL) {
+    Err("error creating buffer\n");
+    return;
+  } 
+  
+  // reset values
+  float *values=(float*)buf;
+  for (int i = 0; i < motors_count; i++) values[i] = 0;
+  Time time=GetTime();
+  memcpy(buf+motors_count * sizeof(float),&time,sizeof(Time));
 
-  // reset values
-  float values[motors_count];
-  for (int i = 0; i < motors_count; i++)
-    values[i] = 0;
-
-  shmem->Write((char *)&values, motors_count * sizeof(float));
+  shmem->Write(buf, motors_count * sizeof(float)+sizeof(Time));
   
   SetIsReady(true);
 }
 
-SimuBldc::~SimuBldc() {}
+SimuBldc::~SimuBldc() {
+  if(buf!=NULL) free(buf);
+}
 
 string SimuBldc::ShMemName(uint32_t modelId,uint32_t deviceId) {
@@ -69 +84 @@
 
 void SimuBldc::SetMotors(float *value) {
-  float values[MotorsCount()];
+  float *values=(float*)buf;
+  for (int i = 0; i < MotorsCount(); i++) values[i] = k->Value() * value[i];
+  Time time=GetTime();
+  memcpy(buf+MotorsCount() * sizeof(float),&time,sizeof(Time));
 
-  for (int i = 0; i < MotorsCount(); i++)
-    values[i] = k->Value() * value[i];
-
-  shmem->Write((char *)&values, MotorsCount() * sizeof(float));
+  shmem->Write(buf, MotorsCount() * sizeof(float)+sizeof(Time));
 
   // on prend une fois pour toute le mutex et on fait des accès directs
   output->GetMutex();
-  for (int i = 0; i < MotorsCount(); i++)
+  for (int i = 0; i < MotorsCount(); i++) {
     output->SetValueNoMutex(i, 0, values[i]);
+  }
   output->ReleaseMutex();
 }
 
-void SimuBldc::GetSpeeds(float *value) const {
-  float values[MotorsCount()];
-  shmem->Read((char *)&values, MotorsCount() * sizeof(float));
+void SimuBldc::GetSpeeds(float *value,Time* time) const {
+  float *values=(float*)buf;
+  shmem->Read(buf, MotorsCount() * sizeof(float)+sizeof(Time));
+  memcpy(time,buf+MotorsCount() * sizeof(float),sizeof(Time));
 
-  for (int i = 0; i < MotorsCount(); i++)
+  for (int i = 0; i < MotorsCount(); i++) {
     value[i] = values[i];
+  }
 }