Changeset 20891

Timestamp:

11/06/07 07:23:41 (1 year ago)

Author:

dshipton

Message:

MFT r20886: Intersense API Fixes
- Fixes a configuration error where we send the configuration to the tracker

without it being opened yet.

- Fixes problems where other softwares would put the IS-900 in a different state

than our driver expected. (boresight fix)

- Fixes and uses Quaternions by default. If quaternions are not available

then Euler angles are used.

- Increased sample rate and added code for getting rid of the

control thread sleep.

Files:

• juggler/branches/2.2/modules/gadgeteer/drivers/Intersense/IntersenseAPI/IntersenseAPI.cpp (modified) (9 diffs)
• juggler/branches/2.2/modules/gadgeteer/drivers/Intersense/IntersenseAPI/IntersenseAPIStandalone.h (modified) (8 diffs)

juggler/branches/2.2/modules/gadgeteer/drivers/Intersense/IntersenseAPI/IntersenseAPI.cpp

@@ -127 +127 @@
-void IntersenseAPI::controlLoop()
-{
-e
+
-   while(!mDone)
-   {
-      this->sample();
-
-
-
-
-
+
+
+
+
+
+
-   }
-}
@@ -141 +142 @@
-bool IntersenseAPI::startSampling()
-{
-   // Configure the stations used by the configuration
-   for( unsigned int i = 0; i < mStations.size(); ++i )
-   {
-      int station_index = mStations[i].stationIndex;
-
-      // Load the config state from the physical tracker
-      mTracker.loadConfigState(station_index);
-      mTracker.setState(station_index, mStations[i].enabled);
-      mTracker.setAngleFormat(station_index, ISD_EULER);
-      mTracker.setInputs(station_index,
-                         mStations[i].useDigital || mStations[i].useAnalog);
-      // Save the config state to the physical tracker.
-      mTracker.saveConfigState(station_index);
-   }
-
-   // Ensure that we have not already started sampling.
-   if (this->isActive() == true)
@@ -184 +170 @@
-         << "failed to connect to tracker.\n" << vprDEBUG_FLUSH;
-      return false;
+   }
+
+   // Configure the stations used by the configuration
+   for( unsigned int i = 0; i < mStations.size(); ++i )
+   {
+      int station_index = mStations[i].stationIndex;
+      // Reset any boresight being used by trackd or ICIDO
+      // This makes a call to the tracker and is not simply setting
+      // state on a var to configure the tracker with as done below.
+      mTracker.resetStationBoresight(station_index);
+      // Load the config state from the physical tracker
+      mTracker.loadConfigState(station_index);
+      mTracker.setState(station_index, mStations[i].enabled);
+      mTracker.setInputs(station_index,
+                         mStations[i].useDigital || mStations[i].useAnalog);
+      mTracker.setTimeStamped( station_index, false );
+      mTracker.setDefaultCoordFrame( station_index );
+
+      // Save the config state to the physical tracker.
+      if ( ! mTracker.saveConfigState(station_index) )
+      {
+         vprDEBUG(vprDBG_ERROR,vprDBG_CRITICAL_LVL)
+            << clrOutNORM(clrRED,"ERROR:")
+            << " [gadget::IntersenseAPI::startSampling()] mTracker.saveConfigState() "
+            << "failed to save config state to station " << i << ".\n"
+            << vprDEBUG_FLUSH;
+
+         mTracker.close();
+         return false;
+      }
+
+      //Try to get output in quat form otherwise fallback on euler angles
+      mTracker.setAngleFormat(station_index, ISD_QUATERNION);
+      if ( ! mTracker.saveConfigState(station_index) )
+      {
+         // Failed to set quaternion format which means it is set for euler
+         mTracker.setAngleFormat(station_index, ISD_EULER);
+      }
-   }
-
@@ -219 +243 @@
-   }
-
+   // Check to see if we have new data to pull
+   if ( ! mTracker.updateData() )
+   {
+       vprDEBUG(gadgetDBG_INPUT_MGR, vprDBG_CRITICAL_LVL)
+         << clrOutBOLD(clrRED, "[gadget::IntersenseAPI::sample()]")
+         << ": Could not read data from InterSense API driver!\n"
+         << vprDEBUG_FLUSH;
+      return false;
+   }
+
+
+// This is the some code for the beginnings of trying to eliminate 
+// the sleep in the control loop. Needs more testing.
+/*
+   bool has_new_data(false);
+   for ( unsigned int i = 0 ; i < mStations.size() ; ++i )
+   {
+      // Make sure station is enabled and tracker has updated data.
+      if( mStations[i].enabled && mTracker.hasData(mStations[i].stationIndex) )
+      {
+         has_new_data = true;
+         break;
+      }
+   }
+
+   // If there wasn't any new data then reliquish control to the cpu and return
+   if( ! has_new_data )
+   {
+      vpr::Thread::yield();
+      vpr::System::msleep(10);
+   }
+*/
+
-   // Create the data buffers to put the new data into.
-   std::vector<gadget::PositionData> cur_pos_samples(mStations.size());
@@ -224 +281 @@
-   std::vector<gadget::AnalogData>   cur_analog_samples;
-
-
-   // get an initial timestamp for this entire sample. we'll copy it into
-   // each PositionData for this sample.
@@ -232 +288 @@
-   }
-
-   mTracker.updateData();
-
-   vpr::Thread::yield();
-
-   for ( unsigned int i = 0 ; i < mStations.size() ; ++i )
-   {
@@ -241 +293 @@
-      int stationIndex = mStations[i].stationIndex;
-
+
-      // Set the time of each PositionData to match the first.
-      cur_pos_samples[i].setTime( cur_pos_samples[0].getTime() );
-
+      // Don't process data from disabled stations
+      if( ! mStations[i].enabled )
+      {
+         continue;
+      }
+
+      gmtl::identity(cur_pos_samples[i].mPosData);
-      // If the Intersense is returning data in Euler format. Otherwise we
-      // assume that it is returning data in quaternion format.
-      if ( mTracker.getAngleFormat(stationIndex) == ISD_EULER )
-      {
-         gmtl::identity(cur_pos_samples[i].mPosData);
-         gmtl::EulerAngleZYXf euler( gmtl::Math::deg2Rad( mTracker.zRot( stationIndex ) ),
-                                     gmtl::Math::deg2Rad( mTracker.yRot( stationIndex ) ),
-                                     gmtl::Math::deg2Rad( mTracker.xRot( stationIndex ) ) );
-         gmtl::setRot( cur_pos_samples[i].mPosData, euler );
-         gmtl::setTrans( cur_pos_samples[i].mPosData,
-                         gmtl::Vec3f(mTracker.xPos( stationIndex ),
-                                     mTracker.yPos( stationIndex ),
-                                     mTracker.zPos( stationIndex )) );
-      }
-      else
@@ -264 +319 @@
-                               mTracker.zQuat( stationIndex ),
-                               mTracker.wQuat( stationIndex ));
-
-
+
+
+
+
+
+
+
-
-      // We start at the index of the first digital item (set in the config
@@ -299 +359 @@
-   addDigitalSample(cur_digital_samples);
-   addPositionSample(cur_pos_samples);
-
-   return true;
-}

juggler/branches/2.2/modules/gadgeteer/drivers/Intersense/IntersenseAPI/IntersenseAPIStandalone.h

@@ -116 +116 @@
-   /**
-    * Returns the format of the angles. 
-0 == ISD_EULER; 1
+1 == ISD_EULER; 2
-    */
-   int getAngleFormat(const unsigned int currentStation) const 
@@ -140 +140 @@
-   }
-
-   /////////////
-   //
-   
-   /**
-    * Return if the Station is ON or OFF.
@@ -148 +145 @@
-   void setState(const unsigned int currentStation, const bool state)
-   {
-state
+(state ? TRUE : FALSE)
-   }
-
@@ -187 +184 @@
-
-   /**
-Return
-0 == ISD_EULER; 1
+Set
+1 == ISD_EULER; 2
-    */
-   void setAngleFormat(const unsigned int currentStation,
@@ -197 +194 @@
-
-   /**
-Return
+Sets
-    */
-   void setTimeStamped(const unsigned int currentStation,
-                       const bool timeStamped)
-   {
-timeStamped
+(timeStamped ? TRUE : FALSE)
-   }
-
@@ -211 +208 @@
-   void setInputs(const unsigned int currentStation, const bool hasInputs)
-   {
-hasInputs
+(hasInputs ? TRUE : FALSE)
-   }
-
@@ -270 +267 @@
-    * Save the current configuration state to the physical tracker device.
-    */
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-   }
-
@@ -328 +362 @@
-   float xQuat(const unsigned int i) const
-   {
+      return mData.Station[i].Orientation[1];
+   }
+
+   /**
+    * Gets the y quaternion value of the i'th receiver. 
+    */
+   float yQuat(const unsigned int i) const
+   {
+      return mData.Station[i].Orientation[2];
+   }
+
+   /**
+    * Gets the z quaternion value of the i'th receiver. 
+    */
+   float zQuat(const unsigned int i) const
+   {
+      return mData.Station[i].Orientation[3];
+   }
+
+   /**
+    * Gets the w quaternion value of the i'th receiver. 
+    */
+   float wQuat(const unsigned int i) const
+   {
-      return mData.Station[i].Orientation[0];
-   }
-
-   /**
-    * Gets the y quaternion value of the i'th receiver. 
-    */
-   float yQuat(const unsigned int i) const
-   {
-      return mData.Station[i].Orientation[1];
-   }
-
-   /**
-    * Gets the z quaternion value of the i'th receiver. 
-    */
-   float zQuat(const unsigned int i) const
-   {
-      return mData.Station[i].Orientation[2];
-   }
-
-   /**
-    * Gets the w quaternion value of the i'th receiver. 
-    */
-   float wQuat(const unsigned int i) const
-   {
-      return mData.Station[i].Orientation[3];
-   }
-