Exemple #1
0
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;
}
Exemple #2
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);
}
Exemple #3
0
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;
}