int display_properties(CPhidgetSpatialHandle phid)
{
int serialNo, version;
const char* ptr;
int numAccelAxes, numGyroAxes, numCompassAxes;
int dataRateMax, dataRateMin;
CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);
CPhidgetSpatial_getAccelerationAxisCount((CPhidgetSpatialHandle)phid,
&numAccelAxes);
CPhidgetSpatial_getGyroAxisCount((CPhidgetSpatialHandle)phid,
&numGyroAxes);
CPhidgetSpatial_getCompassAxisCount((CPhidgetSpatialHandle)phid,
&numCompassAxes);
CPhidgetSpatial_getDataRateMax((CPhidgetSpatialHandle)phid,
&dataRateMax);
CPhidgetSpatial_getDataRateMin((CPhidgetSpatialHandle)phid,
&dataRateMin);
ROS_INFO("%s\n", ptr);
ROS_INFO("Serial Number: %10d\nVersion: %8d\n",
serialNo, version);
ROS_INFO("Number of Accel Axes: %i\n", numAccelAxes);
ROS_INFO("Number of Gyro Axes: %i\n", numGyroAxes);
ROS_INFO("Number of Compass Axes: %i\n", numCompassAxes);
ROS_INFO("datarate> Max: %d Min: %d\n",
dataRateMax, dataRateMin);
return 0;
}
void imuPrintInfo( void* handle )
{
if( !handle )
return;
CPhidgetHandle phid = (CPhidgetHandle)handle;
int serialNo = 0;
int version = 0;
const char* ptr;
int numAccelAxes = 0, numGyroAxes = 0, numCompassAxes = 0;
int dataRateMax = 0, dataRateMin = 0, dataRateCurr = 0;
//CPhidget_getDeviceType(phid, &ptr);
//printf("IMU %s\n", ptr);
CPhidget_getSerialNumber(phid, &serialNo);
CPhidget_getDeviceVersion(phid, &version);
CPhidgetSpatial_getAccelerationAxisCount((CPhidgetSpatialHandle)phid, &numAccelAxes);
CPhidgetSpatial_getGyroAxisCount((CPhidgetSpatialHandle)phid, &numGyroAxes);
CPhidgetSpatial_getCompassAxisCount((CPhidgetSpatialHandle)phid, &numCompassAxes);
CPhidgetSpatial_getDataRateMax((CPhidgetSpatialHandle)phid, &dataRateMax);
CPhidgetSpatial_getDataRateMin((CPhidgetSpatialHandle)phid, &dataRateMin);
CPhidgetSpatial_getDataRate((CPhidgetSpatialHandle)phid, &dataRateCurr);
printf("IMU Serial Number: %10i\n", serialNo);
printf("IMU Version: %8i\n", version);
printf("IMU Number of Accel Axes: %i\n", numAccelAxes);
printf("IMU Number of Gyro Axes: %i\n", numGyroAxes);
printf("IMU Number of Compass Axes: %i\n", numCompassAxes);
printf("IMU datarate> Min: %d Max: %d Current: %d\n", dataRateMin, dataRateMax, dataRateCurr);
}
int CCONV spatial_on_attach(CPhidgetHandle phid, void *userptr) {
PhidgetInfo *info = userptr;
SpatialInfo *spatial_info = info->type_info;
// Accelerometer Attributes:
report(CPhidgetSpatial_getAccelerationAxisCount((CPhidgetSpatialHandle)phid, &spatial_info->accelerometer_axes));
report(CPhidgetSpatial_getGyroAxisCount((CPhidgetSpatialHandle)phid, &spatial_info->gyro_axes));
report(CPhidgetSpatial_getCompassAxisCount((CPhidgetSpatialHandle)phid, &spatial_info->compass_axes));
report(CPhidgetSpatial_getDataRateMax((CPhidgetSpatialHandle)phid, &spatial_info->data_rate_max));
report(CPhidgetSpatial_getDataRateMin((CPhidgetSpatialHandle)phid, &spatial_info->data_rate_min));
// Dealloc if we're alloc'd, this will prevent memory leaks on device re-attachment:
if (spatial_info->acceleration) xfree(spatial_info->acceleration);
if (spatial_info->acceleration_min) xfree(spatial_info->acceleration_min);
if (spatial_info->acceleration_max) xfree(spatial_info->acceleration_max);
if (spatial_info->compass) xfree(spatial_info->compass);
if (spatial_info->compass_min) xfree(spatial_info->compass_min);
if (spatial_info->compass_max) xfree(spatial_info->compass_max);
if (spatial_info->gyroscope) xfree(spatial_info->gyroscope);
if (spatial_info->gyroscope_min) xfree(spatial_info->gyroscope_min);
if (spatial_info->gyroscope_max) xfree(spatial_info->gyroscope_max);
// Allocate space for our extents:
spatial_info->acceleration = ALLOC_N(double, spatial_info->accelerometer_axes);
spatial_info->acceleration_min = ALLOC_N(double, spatial_info->accelerometer_axes);
spatial_info->acceleration_max = ALLOC_N(double, spatial_info->accelerometer_axes);
spatial_info->compass = ALLOC_N(double, spatial_info->compass_axes);
spatial_info->compass_min = ALLOC_N(double, spatial_info->compass_axes);
spatial_info->compass_max = ALLOC_N(double, spatial_info->compass_axes);
spatial_info->gyroscope = ALLOC_N(double, spatial_info->gyro_axes);
spatial_info->gyroscope_min = ALLOC_N(double, spatial_info->gyro_axes);
spatial_info->gyroscope_max = ALLOC_N(double, spatial_info->gyro_axes);
// Initialize the gyro dcm:
if (spatial_info->gyro_axes == 3) {
double identity3x3[3][3] = {{1.0,0.0,0.0}, {0.0,1.0,0.0}, {0.0,0.0,1.0}};
memcpy(&spatial_info->gyroscope_dcm, &identity3x3, sizeof(identity3x3));
}
// Accelerometer
for(int i=0; i < spatial_info->accelerometer_axes; i++) {
report(CPhidgetSpatial_getAccelerationMin((CPhidgetSpatialHandle)phid, i, &spatial_info->acceleration_min[i]));
report(CPhidgetSpatial_getAccelerationMax((CPhidgetSpatialHandle)phid, i, &spatial_info->acceleration_max[i]));
}
for(int i=0; i < spatial_info->compass_axes; i++) {
report(CPhidgetSpatial_getMagneticFieldMin((CPhidgetSpatialHandle)phid, i, &spatial_info->compass_min[i]));
report(CPhidgetSpatial_getMagneticFieldMax((CPhidgetSpatialHandle)phid, i, &spatial_info->compass_max[i]));
}
for(int i=0; i < spatial_info->gyro_axes; i++) {
report(CPhidgetSpatial_getAngularRateMin((CPhidgetSpatialHandle)phid, i, &spatial_info->gyroscope_min[i]));
report(CPhidgetSpatial_getAngularRateMax((CPhidgetSpatialHandle)phid, i, &spatial_info->gyroscope_max[i]));
}
// Ahrs Values:
spatial_info->is_ahrs_initialized = false;
// Set the data rate for the spatial events in milliseconds.
// Note that 1000/16 = 62.5 Hz
report(CPhidgetSpatial_setDataRate((CPhidgetSpatialHandle)phid, spatial_info->data_rate));
// Strictly speaking, this is entirely optional:
if (spatial_info->is_compass_correction_known)
spatial_set_compass_correction_by_array( (CPhidgetSpatialHandle)phid,
spatial_info->compass_correction);
return 0;
}
