Changeset 136 in flair-src

Timestamp:

Feb 13, 2017, 3:02:54 PM (8 years ago)

Author:

Sanahuja Guillaume

Message:

vrpn: improved timeout

Location:

trunk

Files:

• demos/CircleFollower/uav/src/CircleFollower.cpp (modified) (1 diff)
• demos/SimpleFleet/uav/src/SimpleFleet.cpp (modified) (1 diff)
• lib/FlairSensorActuator/src/VrpnClient.cpp (modified) (2 diffs)
• lib/FlairSensorActuator/src/VrpnClient.h (modified) (1 diff)
• lib/FlairSensorActuator/src/VrpnClient_impl.cpp (modified) (4 diffs)
• lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h (modified) (2 diffs)

trunk/demos/CircleFollower/uav/src/CircleFollower.cpp

@@ -42 +42 @@
     Uav* uav=GetUav();
 
-                VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),10000, 80);
+                VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),80);
                 uavVrpn = new MetaVrpnObject(uav->ObjectName());
                 getFrameworkManager()->AddDeviceToLog(uavVrpn);

trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp

@@ -53 +53 @@
     Uav* uav=GetUav();
                 
-        VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),10000, 80);
+        VrpnClient* vrpnclient=new VrpnClient("vrpn", uav->GetDefaultVrpnAddress(),80);
         uavVrpn = new MetaVrpnObject(uav->ObjectName());
         getFrameworkManager()->AddDeviceToLog(uavVrpn);

trunk/lib/FlairSensorActuator/src/VrpnClient.cpp

@@ -37 +37 @@
         
 VrpnClient::VrpnClient(string name,
-                       string address, uint16_t us_period, uint8_t priority)
+                       string address, uint8_t priority)
     : Thread(getFrameworkManager(), name, priority) {
         if (singleton != NULL) {
@@ -45 +45 @@
 
   singleton = this;
-  pimpl_ = new VrpnClient_impl(this, name, address, us_period);
+  pimpl_ = new VrpnClient_impl(this, name, address);
 }
 

trunk/lib/FlairSensorActuator/src/VrpnClient.h

@@ -49 +49 @@
   * \param name name
   * \param address server address
-  * \param us_period Thread period in us
   * \param priority priority of the Thread
   */
   VrpnClient(std::string name,
-             std::string address, uint16_t us_period, uint8_t priority);
+             std::string address,  uint8_t priority);
 
   /*!

trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

@@ -42 +42 @@
 
 VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
-                                 std::string address, uint16_t us_period) {
-  this->us_period = us_period;
+                                 std::string address) {
   this->self = self;
   serialport = NULL;
+        isConnected=false;
 
   mutex = new Mutex(self, name);
@@ -162 +162 @@
 
 void VrpnClient_impl::Run(void) {
+        struct timeval timeout;
+        timeout.tv_sec=0;
+        timeout.tv_usec=100000;
+                        
   while (!self->ToBeStopped()) {
     if (UseXbee()) {
@@ -200 +204 @@
               t.quat[i] = quat[i];
             if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
-              printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
+              Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
             } else {
               // self->Printf("%i %f %f %f
@@ -210 +214 @@
         }
       }
-    } else {
-      connection->mainloop();
-      mutex->GetMutex();
-      for (unsigned int i = 0; i < trackables.size(); i++)
-        trackables.at(i)->mainloop();
-      mutex->ReleaseMutex();
-
-      self->SleepUS(us_period);
+    } else {//!UseXbee()
+                        if(connection->connected()==vrpn_true && !isConnected) {
+                                isConnected=true;
+                                Printf("VRPN connected\n");
+                        }
+                        if(connection->connected()==vrpn_false && isConnected) {
+                                isConnected=false;
+                                Printf("VRPN disconnected\n");
+                        }
+                        
+                        //timeout in mainloop is not well handled if not connected...
+                        if(isConnected) {
+                                //this is when trackables callbacks are called:
+                                if(connection->mainloop(&timeout)!=0) {
+                                        self->Warn("connection dropped\n");
+                                }
+                                //printf("%lld\n",GetTime()/(1000*1000));
+                                mutex->GetMutex();
+                                for (unsigned int i = 0; i < trackables.size(); i++)
+                                        trackables.at(i)->mainloop();
+                                mutex->ReleaseMutex();
+                        } else {
+                                connection->mainloop();
+                                self->SleepMS(10);
+                        }
     }
   }

trunk/lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h

@@ -47 +47 @@
 public:
   VrpnClient_impl(flair::sensor::VrpnClient *self, std::string name,
-                  std::string address, uint16_t us_period);
+                  std::string address);
   VrpnClient_impl(flair::sensor::VrpnClient *self, std::string name,
                   flair::core::SerialPort *serialport, uint16_t us_period);
@@ -77 +77 @@
   flair::core::OneAxisRotation *rotation_1, *rotation_2;
   flair::core::SerialPort *serialport;
+        bool isConnected;//only for ip connection, not for xbee
 };