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 ----
-------------------------------------------------------------------------
Using Tomcat but need to do more? Need to support web services, security?
Get stuff done quickly with pre-integrated technology to make your job easier
Download IBM WebSphere Application Server v.1.0.1 based on Apache Geronimo
http://sel.as-us.falkag.net/sel?cmd=lnk&kid=120709&bid=263057&dat=121642
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
