[Playerstage-commit] code/player/server/drivers/imu XSensMT.cc, 1.2, 1.3

[Radu B. R. <ve...@us...>]

Update of /cvsroot/playerstage/code/player/server/drivers/imu
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv21356

Modified Files:
	XSensMT.cc 
Log Message:
Implemented orientation reset.


Index: XSensMT.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/imu/XSensMT.cc,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** XSensMT.cc	8 Aug 2006 18:52:04 -0000	1.2
--- XSensMT.cc	18 Sep 2006 12:51:32 -0000	1.3
***************
*** 47,50 ****
--- 47,51 ----
  
  - PLAYER_IMU_REQ_SET_DATATYPE
+ - PLAYER_IMU_REQ_RESET_ORIENTATION
  
  @par Configuration file options
***************
*** 189,193 ****
  
      // Put MTi/MTx in Config State
!     if(mtcomm.writeMessage (MID_GOTOCONFIG) != MTRV_OK){
          PLAYER_ERROR ("No device connected!");
          return false;
--- 190,194 ----
  
      // Put MTi/MTx in Config State
!     if (mtcomm.writeMessage (MID_GOTOCONFIG) != MTRV_OK){
          PLAYER_ERROR ("No device connected!");
          return false;
***************
*** 279,282 ****
--- 280,287 ----
      assert (hdr);
      assert (data);
+     
+     // this holds possible error messages returned by mtcomm.writeMessage
+     int err;
+     
      if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
          PLAYER_IMU_REQ_SET_DATATYPE, device_addr))
***************
*** 289,292 ****
--- 294,380 ----
          {
              dataType = datatype->value;
+ 	    
+ 	    int outputSettings = OUTPUTSETTINGS_ORIENTMODE_EULER;
+ 	    switch (dataType)
+ 	    {
+ 		case 1:
+ 		{
+ 		    outputSettings = OUTPUTSETTINGS_ORIENTMODE_EULER;
+ 		    break;
+ 		}
+ 		case 2:
+ 		{
+ 		    break;
+ 		}
+ 		case 3:
+ 		{
+ 		    outputSettings = OUTPUTSETTINGS_ORIENTMODE_QUATERNION;
+ 		    break;
+ 		}
+ 		case 4:
+ 		{
+ 		    outputSettings = OUTPUTSETTINGS_ORIENTMODE_EULER;
+ 		    break;
+ 		}
+ 		default:
+ 		{
+ 		    outputSettings = OUTPUTSETTINGS_ORIENTMODE_EULER;
+ 		}
+ 		
+ 	    }
+ 	    // Put MTi/MTx in Config State
+ 	    if (mtcomm.writeMessage (MID_GOTOCONFIG) != MTRV_OK)
+ 	    {
+     		PLAYER_ERROR ("No device connected!");
+         	Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+                      hdr->subtype);
+ 		return (-1);
+ 	    }
+ 	    
+ 	    int outputMode = OUTPUTMODE_CALIB + OUTPUTMODE_ORIENT;
+ 	    // Set output mode and output settings for the MTi/MTx
+ 	    if (mtcomm.setDeviceMode (outputMode, outputSettings, BID_MASTER) != MTRV_OK) {
+     		PLAYER_ERROR ("Could not set device mode(s)!");
+     		return false;
+ 	    }
+ 
+ 	    // Put MTi/MTx in Measurement State
+ 	    mtcomm.writeMessage (MID_GOTOMEASUREMENT);
+             
+ 	    Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
+                      hdr->subtype);
+         }
+         else
+             Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+                      hdr->subtype);
+ 
+         return 0;
+     }
+     else if (Message::MatchMessage (hdr, PLAYER_MSGTYPE_REQ, 
+         PLAYER_IMU_REQ_RESET_ORIENTATION, device_addr))
+     {
+         // Change the data type according to the user's preferences
+         player_imu_reset_orientation_config *rconfig =
+                 (player_imu_reset_orientation_config*)data;
+ 
+ 	// 0 = store current settings
+ 	// 1 = heading reset
+ 	// 2 = global reset
+ 	// 3 = object reset
+ 	// 4 = align reset
+         if ((rconfig->value >= 0) && (rconfig->value <= 4))
+         {
+ 	    // Force <global reset> until further tests.
+             rconfig->value = 2;
+ 	    
+ 	    if ((err = mtcomm.writeMessage (MID_RESETORIENTATION, 
+ 		RESETORIENTATION_GLOBAL, LEN_RESETORIENTATION, BID_MASTER)) != MTRV_OK)
+ 	    {
+     		PLAYER_ERROR1 ("Could not put reset orientation on device! Error 0x%x\n", err);
+         	Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_NACK,
+                      hdr->subtype);
+ 		return (-1);
+ 	    }
+ 	    
              Publish (device_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
                       hdr->subtype);
***************
*** 349,355 ****
              float quaternion_data[4] = {0};
              // Parse and get value (quaternion orientation)
-             mtcomm.getValue (VALUE_ORIENT_QUAT, quaternion_data, data, BID_MASTER);
- 
              imu_data_quat.calib_data = GetCalibValues (data);
              imu_data_quat.q0 = quaternion_data[0];
              imu_data_quat.q1 = quaternion_data[1];
--- 437,443 ----
              float quaternion_data[4] = {0};
              // Parse and get value (quaternion orientation)
              imu_data_quat.calib_data = GetCalibValues (data);
+ 	    
+             mtcomm.getValue (VALUE_ORIENT_QUAT, quaternion_data, data, BID_MASTER);
              imu_data_quat.q0 = quaternion_data[0];
              imu_data_quat.q1 = quaternion_data[1];
***************
*** 407,423 ****
      return calib_data;
  }
- 
- #if 0
- //------------------------------------------------------------------------------
- ////////////////////////////////////////////////////////////////////////////////
- // Extra stuff for building a shared object.
- 
- /* need the extern to avoid C++ name-mangling  */
- extern "C" {
-     int player_driver_init (DriverTable* table)
-     {
-         XSensMT_Register (table);
-         return(0);
-     }
- }
- #endif
--- 495,496 ----