Update of /cvsroot/playerstage/code/player/client_libs/libplayerc
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv965
Modified Files:
Makefile.am playerc.h utils.c
Added Files:
dev_imu.c
Log Message:
Added support for IMU.
--- NEW FILE: dev_imu.c ---
/*
* libplayerc : a Player client library
* Copyright (C) Andrew Howard 2002-2003
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
*/
/***************************************************************************
* Desc: IMU proxy
* Author: Radu Bogdan Rusu
* Date: 8th of September 2006
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <netinet/in.h>
#include "playerc.h"
#include "error.h"
// Process incoming data
void playerc_imu_putmsg (playerc_imu_t *device,
player_msghdr_t *header,
void *data);
// Create a new imu proxy
playerc_imu_t *playerc_imu_create(playerc_client_t *client, int index)
{
playerc_imu_t *device;
device = malloc(sizeof(playerc_imu_t));
memset(device, 0, sizeof(playerc_imu_t));
playerc_device_init(&device->info, client, PLAYER_IMU_CODE, index,
(playerc_putmsg_fn_t) playerc_imu_putmsg);
return device;
}
// Destroy a imu proxy
void playerc_imu_destroy(playerc_imu_t *device)
{
playerc_device_term(&device->info);
free(device);
}
// Subscribe to the imu device
int playerc_imu_subscribe(playerc_imu_t *device, int access)
{
return playerc_device_subscribe(&device->info, access);
}
// Un-subscribe from the imu device
int playerc_imu_unsubscribe(playerc_imu_t *device)
{
return playerc_device_unsubscribe(&device->info);
}
// Process incoming data
void playerc_imu_putmsg (playerc_imu_t *device,
player_msghdr_t *header,
void *data)
{
// int i, j;
if (header->type == PLAYER_MSGTYPE_DATA)
switch (header->subtype)
{
case PLAYER_IMU_DATA_STATE:
{
player_imu_data_state_t* imu_data =
(player_imu_data_state_t*)data;
device->pose = imu_data->pose;
break;
}
case PLAYER_IMU_DATA_CALIB:
{
player_imu_data_calib_t* imu_data =
(player_imu_data_calib_t*)data;
device->calib_data.accel_x = imu_data->accel_x;
device->calib_data.accel_y = imu_data->accel_y;
device->calib_data.accel_z = imu_data->accel_z;
device->calib_data.gyro_x = imu_data->gyro_x;
device->calib_data.gyro_y = imu_data->gyro_y;
device->calib_data.gyro_z = imu_data->gyro_z;
device->calib_data.magn_x = imu_data->magn_x;
device->calib_data.magn_y = imu_data->magn_y;
device->calib_data.magn_z = imu_data->magn_z;
break;
}
case PLAYER_IMU_DATA_QUAT:
{
player_imu_data_quat_t* imu_data =
(player_imu_data_quat_t*)data;
device->calib_data = imu_data->calib_data;
device->q0 = imu_data->q0;
device->q1 = imu_data->q1;
device->q2 = imu_data->q2;
device->q3 = imu_data->q3;
break;
}
case PLAYER_IMU_DATA_EULER:
{
player_imu_data_euler_t* imu_data =
(player_imu_data_euler_t*)data;
device->calib_data = imu_data->calib_data;
device->pose.proll = imu_data->orientation.proll;
device->pose.ppitch = imu_data->orientation.ppitch;
device->pose.pyaw = imu_data->orientation.pyaw;
break;
}
default:
{
PLAYERC_WARN1 ("skipping imu message with unknown data subtype:
%d\n",
header->subtype);
break;
}
}
else
PLAYERC_WARN2 ("skipping imu message with unknown type/subtype:
%d/%d\n",
header->type, header->subtype);
}
// Change the data type to one of the predefined data structures.
int
playerc_imu_datatype (playerc_imu_t *device, int value)
{
player_imu_datatype_config_t config;
config.value = value;
return (playerc_client_request(device->info.client,
&device->info,
PLAYER_IMU_REQ_SET_DATATYPE,
&config, NULL, 0));
}
// Reset orientation
int
playerc_imu_reset_orientation (playerc_imu_t *device, int value)
{
player_imu_reset_orientation_config_t config;
config.value = value;
return (playerc_client_request(device->info.client,
&device->info,
PLAYER_IMU_REQ_RESET_ORIENTATION,
&config, NULL, 0));
}
Index: utils.c
===================================================================
RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/utils.c,v
retrieving revision 1.25
retrieving revision 1.26
diff -C2 -d -r1.25 -r1.26
*** utils.c 31 May 2006 14:04:51 -0000 1.25
--- utils.c 11 Sep 2006 19:57:37 -0000 1.26
***************
*** 113,116 ****
--- 113,118 ----
case PLAYER_SIMULATION_CODE:
return PLAYER_SIMULATION_STRING;
+ case PLAYER_IMU_CODE:
+ return PLAYER_IMU_STRING;
default:
***************
*** 182,185 ****
--- 184,189 ----
if (strcmp(name, PLAYER_SIMULATION_STRING) == 0)
return PLAYER_SIMULATION_CODE;
+ if (strcmp(name, PLAYER_IMU_STRING) == 0)
+ return PLAYER_IMU_CODE;
return -1;
}
Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/Makefile.am,v
retrieving revision 1.93
retrieving revision 1.94
diff -C2 -d -r1.93 -r1.94
*** Makefile.am 6 Sep 2006 16:54:51 -0000 1.93
--- Makefile.am 11 Sep 2006 19:57:37 -0000 1.94
***************
*** 37,40 ****
--- 37,41 ----
dev_gripper.c \
dev_health.c \
+ dev_imu.c \
dev_ir.c \
dev_laser.c \
Index: playerc.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/client_libs/libplayerc/playerc.h,v
retrieving revision 1.206
retrieving revision 1.207
diff -C2 -d -r1.206 -r1.207
*** playerc.h 6 Sep 2006 16:54:51 -0000 1.206
--- playerc.h 11 Sep 2006 19:57:37 -0000 1.207
***************
*** 2921,2924 ****
--- 2921,2970 ----
/**************************************************************************/
/** @ingroup playerc_proxies
+ @defgroup playerc_proxy_imu imu
+
+ The imu proxy provides an interface to an Inertial Measurement Unit.
+
+ @{
+ */
+
+ /** @brief IMU proxy state data. */
+ typedef struct
+ {
+ /** Device info; must be at the start of all device structures. */
+ playerc_device_t info;
+
+ /** The complete pose of the IMU in 3D coordinates + angles */
+ player_pose3d_t pose;
+
+ /** Calibrated IMU data (accel, gyro, magnetometer) */
+ player_imu_data_calib_t calib_data;
+
+ /** Orientation data as quaternions */
+ float q0, q1, q2, q3;
+ } playerc_imu_t;
+
+ /** @brief Create a imu proxy. */
+ playerc_imu_t *playerc_imu_create (playerc_client_t *client, int index);
+
+ /** @brief Destroy a imu proxy. */
+ void playerc_imu_destroy (playerc_imu_t *device);
+
+ /** @brief Subscribe to the imu device. */
+ int playerc_imu_subscribe (playerc_imu_t *device, int access);
+
+ /** @brief Un-subscribe from the imu device. */
+ int playerc_imu_unsubscribe (playerc_imu_t *device);
+
+ /** Change the data type to one of the predefined data structures. */
+ int playerc_imu_datatype (playerc_imu_t *device, int value);
+
+ /** Reset orientation. */
+ int playerc_imu_reset_orientation (playerc_imu_t *device, int value);
+
+ /** @} */
+ /***************************************************************************/
+
+ /**************************************************************************/
+ /** @ingroup playerc_proxies
@defgroup playerc_proxy_wsn wsn
-------------------------------------------------------------------------
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
