Changeset 214 in flair-src for trunk/lib/FlairSimulator/src/X4.cpp

Timestamp:

Feb 7, 2018, 5:49:27 PM (7 years ago)

Author:

Sanahuja Guillaume

Message:

matrix

File:

• trunk/lib/FlairSimulator/src/X4.cpp (modified) (4 diffs)

trunk/lib/FlairSimulator/src/X4.cpp

@@ -20 +20 @@
 #include <Tab.h>
 #include <DoubleSpinBox.h>
+#include <SpinBox.h>
 #include <GroupBox.h>
 #include <math.h>
@@ -73 +74 @@
   j_yaw = new DoubleSpinBox(setup_tab->LastRowLastCol(), "j_yaw:", 0, 1, 0.001,
                             5); // moment d'inertie d'un axe (N.m.s²/rad)
+                            
+  motorTimeout = new SpinBox(setup_tab->NewRow(), "motor timeout:","ms", 0, 1000, 100,100);
 
   motors = new SimuBldc(this, name, 4, modelId,0);
@@ -199 +202 @@
   float fl_speed, fr_speed, rl_speed, rr_speed;
   float u_roll, u_pitch, u_yaw, u_thrust;
+  Time motorTime;
 #ifdef GL
   motor_speed_mutex->GetMutex();
 #endif // GL
-  motors->GetSpeeds(motor_speed);
+  motors->GetSpeeds(motor_speed,&motorTime);
+  if((GetTime()-motorTime)/1000000>motorTimeout->Value()) {
+    for(int i=0;i<4;i++) {
+      if(motor_speed[i]!=0) {
+         //Printf("timout\n");
+        for(int i=0;i<4;i++) motor_speed[i]=0;
+        break;
+      }
+    }
+  }
 #ifdef GL
   motor_speed_mutex->ReleaseMutex();
 #endif // GL
+  
   fl_speed = motor_speed[0];
   fr_speed = motor_speed[1];
@@ -212 +226 @@
 
   /*
-      ** ===================================================================
-      **    u roll: roll torque
-      **
-      ** ===================================================================
-      */
+  ** ===================================================================
+  **    u roll: roll torque
+  **
+  ** ===================================================================
+  */
   u_roll = arm_length->Value() * k_mot->Value() *
            (fl_speed * fl_speed + rl_speed * rl_speed - fr_speed * fr_speed -