Update of /cvsroot/playerstage/code/player/server/drivers/shell
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7744/drivers/shell
Modified Files:
readlog.cc writelog.cc
Log Message:
Added IMU logging capabilities.
Index: writelog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/writelog.cc,v
retrieving revision 1.72
retrieving revision 1.73
diff -C2 -d -r1.72 -r1.73
*** writelog.cc 1 Jul 2006 12:31:38 -0000 1.72
--- writelog.cc 11 Sep 2006 20:14:13 -0000 1.73
***************
*** 76,79 ****
--- 76,80 ----
- @ref interface_wifi
- @ref interface_wsn
+ - @ref interface_imu
The following interfaces are supported in principle but are currently
***************
*** 125,129 ****
@endverbatim
! @author Andrew Howard
*/
--- 126,130 ----
@endverbatim
! @author Andrew Howard, Radu Bogdan Rusu
*/
***************
*** 208,212 ****
// Write WSN data to file
! private: int WriteWSN(player_msghdr_t* hdr, void *data);
#if 0
// Write blobfinder data to file
--- 209,216 ----
// Write WSN data to file
! private: int WriteWSN (player_msghdr_t* hdr, void *data);
!
! // Write IMU data to file
! private: int WriteIMU (player_msghdr_t* hdr, void *data);
#if 0
// Write blobfinder data to file
***************
*** 708,711 ****
--- 712,718 ----
retval = this->WriteWSN(hdr, data);
break;
+ case PLAYER_IMU_CODE:
+ retval = this->WriteIMU (hdr, data);
+ break;
#if 0
case PLAYER_BLOBFINDER_CODE:
***************
*** 1201,1204 ****
--- 1208,1314 ----
}
+ /** @ingroup tutorial_datalog
+ * @defgroup player_driver_writelog_imu IMU format
+
+ @brief IMU log format
+
+ The format for each @ref interface_wsn message is:
+ - magn_x (float): magnetic measurement on X-axis from a magnetometer
+ - magn_y (float): magnetic measurement on Y-axis from a magnetometer
+ - magn_z (float): magnetic measurement on Z-axis from a magnetometer
+ - temperature (float): temperature measurement from a temperature sensor
+ - battery (float): remaining battery voltage
+ */
+ int
+ WriteLog::WriteIMU (player_msghdr_t* hdr, void *data)
+ {
+ // Check the type
+ switch(hdr->type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ // Check the subtype
+ switch(hdr->subtype)
+ {
+ case PLAYER_IMU_DATA_STATE:
+ {
+ player_imu_data_state_t* idata;
+ idata = (player_imu_data_state_t*)data;
+ fprintf (this->file,"%f %f %f %f %f %f",
+ idata->pose.px,
+ idata->pose.py,
+ idata->pose.pz,
+ idata->pose.proll,
+ idata->pose.ppitch,
+ idata->pose.pyaw);
+ return (0);
+ }
+
+ case PLAYER_IMU_DATA_CALIB:
+ {
+ player_imu_data_calib_t* idata;
+ idata = (player_imu_data_calib_t*)data;
+ fprintf (this->file,"%f %f %f %f %f %f %f %f %f",
+ idata->accel_x,
+ idata->accel_y,
+ idata->accel_z,
+ idata->gyro_x,
+ idata->gyro_y,
+ idata->gyro_z,
+ idata->magn_x,
+ idata->magn_y,
+ idata->magn_z);
+ return (0);
+ }
+
+ case PLAYER_IMU_DATA_QUAT:
+ {
+ player_imu_data_quat_t* idata;
+ idata = (player_imu_data_quat_t*)data;
+ fprintf (this->file,"%f %f %f %f %f %f %f %f %f %f %f %f
%f",
+ idata->calib_data.accel_x,
+ idata->calib_data.accel_y,
+ idata->calib_data.accel_z,
+ idata->calib_data.gyro_x,
+ idata->calib_data.gyro_y,
+ idata->calib_data.gyro_z,
+ idata->calib_data.magn_x,
+ idata->calib_data.magn_y,
+ idata->calib_data.magn_z,
+ idata->q0,
+ idata->q1,
+ idata->q2,
+ idata->q3);
+ return (0);
+ }
+
+ case PLAYER_IMU_DATA_EULER:
+ {
+ player_imu_data_euler_t* idata;
+ idata = (player_imu_data_euler_t*)data;
+ fprintf (this->file,"%f %f %f %f %f %f %f %f %f %f %f %f",
+ idata->calib_data.accel_x,
+ idata->calib_data.accel_y,
+ idata->calib_data.accel_z,
+ idata->calib_data.gyro_x,
+ idata->calib_data.gyro_y,
+ idata->calib_data.gyro_z,
+ idata->calib_data.magn_x,
+ idata->calib_data.magn_y,
+ idata->calib_data.magn_z,
+ idata->orientation.proll,
+ idata->orientation.ppitch,
+ idata->orientation.pyaw);
+ return (0);
+ }
+
+ default:
+ return (-1);
+ }
+
+ default:
+ return (-1);
+ }
+ }
+
#if 0
/** @ingroup tutorial_datalog
Index: readlog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/readlog.cc,v
retrieving revision 1.40
retrieving revision 1.41
diff -C2 -d -r1.40 -r1.41
*** readlog.cc 12 Apr 2006 13:33:31 -0000 1.40
--- readlog.cc 11 Sep 2006 20:14:13 -0000 1.41
***************
*** 62,65 ****
--- 62,66 ----
- @ref interface_wifi
- @ref interface_wsn
+ - @ref interface_imu
The following interfaces are supported in principle but are currently
***************
*** 119,123 ****
@endverbatim
! @author Andrew Howard
*/
--- 120,124 ----
@endverbatim
! @author Andrew Howard, Radu Bogdan Rusu
*/
***************
*** 200,203 ****
--- 201,209 ----
void * data);
+ // Process IMU interface configuration requests
+ private: int ProcessIMUConfig(MessageQueue * resp_queue,
+ player_msghdr_t * hdr,
+ void * data);
+
// Parse the header info
private: int ParseHeader(int linenum, int token_count, char **tokens,
***************
*** 269,272 ****
--- 275,284 ----
int linenum,
int token_count, char **tokens, double time);
+
+ // Parse IMU data
+ private: int ParseIMU(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time);
#if 0
***************
*** 969,972 ****
--- 981,1018 ----
int
+ ReadLog::ProcessIMUConfig(MessageQueue * resp_queue,
+ player_msghdr_t * hdr,
+ void * data)
+ {
+ switch(hdr->subtype)
+ {
+ case PLAYER_IMU_REQ_SET_DATATYPE:
+ {
+ // Find the right place from which to retrieve it
+ int j;
+ for(j=0;j<this->provide_count;j++)
+ {
+ if(Device::MatchDeviceAddress(this->provide_ids[j],
hdr->addr))
+ break;
+ }
+ if(j>=this->provide_count)
+ return(-1);
+
+ if(!this->provide_metadata[j])
+ return(-1);
+
+ this->Publish(this->provide_ids[j], resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK, hdr->subtype,
+ this->provide_metadata[j],
+ sizeof(player_imu_datatype_config_t),
+ NULL);
+ return(0);
+ }
+ default:
+ return(-1);
+ }
+ }
+
+ int
ReadLog::ProcessMessage(MessageQueue * resp_queue,
player_msghdr_t * hdr,
***************
*** 995,998 ****
--- 1041,1049 ----
}
else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
+ (hdr->addr.interf == PLAYER_IMU_CODE))
+ {
+ return(this->ProcessIMUConfig(resp_queue, hdr, data));
+ }
+ else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
(hdr->addr.interf == PLAYER_POSITION2D_CODE))
{
***************
*** 1095,1098 ****
--- 1146,1152 ----
return this->ParseWSN(id, type, subtype, linenum,
token_count, tokens, time);
+ else if (id.interf == PLAYER_IMU_CODE)
+ return this->ParseIMU (id, type, subtype, linenum,
+ token_count, tokens, time);
#if 0
***************
*** 1783,1786 ****
--- 1837,1961 ----
}
+ ////////////////////////////////////////////////////////////////////////////
+ // Parse IMU data
+ int ReadLog::ParseIMU (player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time)
+ {
+ switch(type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ switch(subtype)
+ {
+ case PLAYER_IMU_DATA_STATE:
+ {
+ if (token_count < 13)
+ {
+ PLAYER_ERROR2("invalid line at %s:%d",
this->filename, linenum);
+ return -1;
+ }
+ player_imu_data_state_t data;
+
+ data.pose.px = atof (tokens[7]);
+ data.pose.py = atof (tokens[8]);
+ data.pose.pz = atof (tokens[9]);
+ data.pose.proll = atof (tokens[10]);
+ data.pose.ppitch = atof (tokens[11]);
+ data.pose.pyaw = atof (tokens[12]);
+
+ this->Publish (id, NULL, type, subtype,
+ (void*)&data, sizeof(data), &time);
+ return (0);
+ }
+
+ case PLAYER_IMU_DATA_CALIB:
+ {
+ if (token_count < 16)
+ {
+ PLAYER_ERROR2("invalid line at %s:%d",
this->filename, linenum);
+ return -1;
+ }
+ player_imu_data_calib_t data;
+
+ data.accel_x = atof (tokens[7]);
+ data.accel_y = atof (tokens[8]);
+ data.accel_z = atof (tokens[9]);
+ data.gyro_x = atof (tokens[10]);
+ data.gyro_y = atof (tokens[11]);
+ data.gyro_z = atof (tokens[12]);
+ data.magn_x = atof (tokens[13]);
+ data.magn_y = atof (tokens[14]);
+ data.magn_z = atof (tokens[15]);
+
+ this->Publish (id, NULL, type, subtype,
+ (void*)&data, sizeof(data), &time);
+ return (0);
+ }
+
+ case PLAYER_IMU_DATA_QUAT:
+ {
+ if (token_count < 20)
+ {
+ PLAYER_ERROR2("invalid line at %s:%d",
this->filename, linenum);
+ return -1;
+ }
+ player_imu_data_quat_t data;
+
+ data.calib_data.accel_x = atof (tokens[7]);
+ data.calib_data.accel_y = atof (tokens[8]);
+ data.calib_data.accel_z = atof (tokens[9]);
+ data.calib_data.gyro_x = atof (tokens[10]);
+ data.calib_data.gyro_y = atof (tokens[11]);
+ data.calib_data.gyro_z = atof (tokens[12]);
+ data.calib_data.magn_x = atof (tokens[13]);
+ data.calib_data.magn_y = atof (tokens[14]);
+ data.calib_data.magn_z = atof (tokens[15]);
+ data.q0 = atof (tokens[16]);
+ data.q1 = atof (tokens[17]);
+ data.q2 = atof (tokens[18]);
+ data.q3 = atof (tokens[19]);
+
+ this->Publish (id, NULL, type, subtype,
+ (void*)&data, sizeof(data), &time);
+ return (0);
+ }
+
+ case PLAYER_IMU_DATA_EULER:
+ {
+ if (token_count < 19)
+ {
+ PLAYER_ERROR2("invalid line at %s:%d",
this->filename, linenum);
+ return -1;
+ }
+ player_imu_data_euler_t data;
+
+ data.calib_data.accel_x = atof (tokens[7]);
+ data.calib_data.accel_y = atof (tokens[8]);
+ data.calib_data.accel_z = atof (tokens[9]);
+ data.calib_data.gyro_x = atof (tokens[10]);
+ data.calib_data.gyro_y = atof (tokens[11]);
+ data.calib_data.gyro_z = atof (tokens[12]);
+ data.calib_data.magn_x = atof (tokens[13]);
+ data.calib_data.magn_y = atof (tokens[14]);
+ data.calib_data.magn_z = atof (tokens[15]);
+ data.orientation.proll = atof (tokens[16]);
+ data.orientation.ppitch = atof (tokens[17]);
+ data.orientation.pyaw = atof (tokens[18]);
+
+ this->Publish (id, NULL, type, subtype,
+ (void*)&data, sizeof(data), &time);
+ return (0);
+ }
+ default:
+ PLAYER_ERROR1 ("unknown IMU data subtype %d\n", subtype);
+ return (-1);
+ }
+ default:
+ PLAYER_ERROR1 ("unknown IMU message type %d\n", type);
+ return (-1);
+ }
+ }
+
#if 0
////////////////////////////////////////////////////////////////////////////
-------------------------------------------------------------------------
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
