phidgets_native 0.2.1 → 0.2.5

data/ext/phidgets_native/spatial_ruby.c

@@ -3,6 +3,9 @@
 const char MSG_COMPASS_CORRECTION_NOT_ARRAY[] = "compass correction must be a 13 element array";
 const char MSG_COMPASS_CORRECTION_MUST_BE_FLOAT[] = "compass correction elements must be float or fixnum";
 const char MSG_DATA_RATE_MUST_BE_NUM[] = "data rate must be fixnum";
+const char MSG_AROUND_MUST_BE_NUMERIC[] = "rotation angles must be fixnums or floats";
+const char MSG_IN_ORDER_MUST_BE_STRING[] = "rotation order must be a string";
+const char MSG_INVALID_ROTATION_AXIS[] = "invalid rotation axis in rotation order, must be x, y, or z";
 
 /*
  * Document-class: PhidgetsNative::Spatial < PhidgetsNative::Device
@@ -13,6 +16,10 @@ const char MSG_DATA_RATE_MUST_BE_NUM[] = "data rate must be fixnum";
  */
 
 void Init_phidgets_native_spatial(VALUE m_Phidget) {
+  // We need this for the orientation code mostly:
+  rb_funcall(rb_mKernel, rb_intern("require"), 1, rb_str_new2("matrix"));
+
+  // And now attach methods to the object:
   VALUE c_Device = rb_const_get(m_Phidget, rb_intern("Device"));
 
   VALUE c_Spatial = rb_define_class_under(m_Phidget,"Spatial",c_Device);
@@ -311,7 +318,134 @@ void Init_phidgets_native_spatial(VALUE m_Phidget) {
    */
   rb_define_method(c_Spatial, "data_rate", spatial_data_rate_get, 0);
   
+  /*
+   * call-seq:
+   *   direction_cosine_matrix( FixNum around_x, FixNum around_y, FixNum around_z, String in_order = 'xyz' ) -> Matrix
+   * This private method is used internally to return a single direction cosine 
+   * Matrix that represents the combined translations provided around the x,y, 
+   * and z axis. The optional in_order will translate around the axis in the 
+   * order provided. Angles are expected to be in radians.
+   */
+  rb_define_private_method(c_Spatial, "direction_cosine_matrix", spatial_direction_cosine_matrix, -1);
+
+  /*
+   * call-seq:
+   *   accelerometer_to_roll_and_pitch -> Array
+   * This private method is used internally to return an array whose first double 
+   * contains the roll and whose second contains the pitch component of the 
+   * acceleration. Values are expressed in radians.
+   */
+  rb_define_private_method(c_Spatial, "accelerometer_to_roll_and_pitch", spatial_accelerometer_to_roll_and_pitch, 0);
+
+  /*
+   * call-seq:
+   *   gravity_to_roll_and_pitch -> Array
+   * This private method is used to return an array whose first double 
+   * contains the roll and whose second contains the pitch component of gravity.
+   * Currently, this method merely calls and returns the accelerometer_to_roll_and_pitch
+   * method. Feel free to override this method with your own, for more accurate
+   * compass results. Values are expressed in radians.
+   */
+  rb_define_private_method(c_Spatial, "gravity_to_roll_and_pitch", spatial_gravity_to_roll_and_pitch, 0);
+
+  /*
+   * Document-method: accelerometer_to_euler
+   * call-seq:
+   *   accelerometer_to_euler -> Array
+   *
+   * This method returns a three element array of euler angles (yaw, roll, pitch)
+   * representing the vector of acceleration, as currently reflected by the 
+   * spatial sensor.
+   */
+  rb_define_method(c_Spatial, "accelerometer_to_euler", spatial_accelerometer_to_euler, 0);
+
+  /*
+   * Document-method: compass_bearing
+   * call-seq:
+   *   compass_bearing -> Float
+   *
+   * This method returns a float representing the compass' bearing, perpendicular to
+   * the ground plane, in radians. A radian of 'pi' is where the usb cable plugs in,
+   * and a radian of 0 is the opposite end (front) of the device. Note that this 
+   * method returns the 'magnetic' North, and not 'true' North. Also note that 
+   * accuracy is highly determined by the private method 'gravity_to_roll_and_pitch'.
+   * Currently, gravity is merely assumed to be the output of the 
+   * accelerometer_to_roll_and_pitch method. Feel free to extend the Spatial class
+   * and override the gravity_to_roll_and_pitch method, if you feel you have 
+   * a better way to determine gravity. Your override's return will be used 
+   * automatically by this method, once defined.
+   */
+  rb_define_method(c_Spatial, "compass_bearing", spatial_compass_bearing, 0);
+
+  /*
+   * Document-method: compass_bearing_to_euler
+   * call-seq:
+   *   compass_bearing_to_euler -> Float
+   *
+   * Probably this isn't too useful, but it returns a three element array, of which
+   * the third element is the compass_bearing.
+   */
+  rb_define_method(c_Spatial, "compass_bearing_to_euler", spatial_compass_bearing_to_euler, 0);
+
+  /*
+   * Document-method: accelerometer_to_dcm
+   * call-seq:
+   *   accelerometer_to_dcm -> Matrix
+   *
+   * This method returns a 3x3 Matrix containing a direction cosine Matrix which
+   * represents the rotation of the current accelerometer value.
+   */
+  rb_define_method(c_Spatial, "accelerometer_to_dcm", spatial_accelerometer_to_dcm, 0);
+
+  /*
+   * Document-method: compass_bearing_to_dcm
+   * call-seq:
+   *   compass_bearing_to_dcm -> Matrix
+   *
+   * This method returns a 3x3 direction cosine Matrix which represents the 
+   * rotation of the current compass_bearing vector in 3d space.
+   */
+  rb_define_method(c_Spatial, "compass_bearing_to_dcm", spatial_compass_bearing_to_dcm, 0);
+
+  /*
+   * Document-method: gyro_to_dcm
+   * call-seq:
+   *   gyro_to_dcm -> Matrix
+   *
+   * This method returns a 3x3 direction cosine Matrix which represents the 
+   * rotation of the current gyroscope vector.
+   */
+  rb_define_method(c_Spatial, "gyro_to_dcm", spatial_gyro_to_dcm, 0);
+
+  /*
+   * Document-method: orientation_to_quaternion
+   * call-seq:
+   *   orientation_to_quaternion -> Array
+   *
+   * This method returns a 4 element array consisting of floats, which represent
+   * the estimated AHRS orientation of the phidget.
+   */
+  rb_define_method(c_Spatial, "orientation_to_quaternion", spatial_orientation_to_quaternion, 0);
+
+  /*
+   * Document-method: orientation_to_dcm
+   * call-seq:
+   *   orientation_to_dcm -> Array
+   *
+   * This method returns a 3x3 direction cosine Matrix, which represent
+   * the estimated AHRS orientation of the phidget in 3d space.
+   */
+  rb_define_method(c_Spatial, "orientation_to_dcm", spatial_orientation_to_dcm, 0);
 
+  /*
+   * Document-method: orientation_to_euler
+   * call-seq:
+   *   orientation_to_euler -> Array
+   *
+   * This method returns a 3 element array, which represents the estimated
+   * AHRS orientation of the phidget in 3d space, in euler angles.
+   */
+  rb_define_method(c_Spatial, "orientation_to_euler", spatial_orientation_to_euler, 0);
 }
 
 VALUE spatial_initialize(VALUE self, VALUE serial) {
@@ -323,6 +457,12 @@ VALUE spatial_initialize(VALUE self, VALUE serial) {
   spatial_info->data_rate = DEFAULT_SPATIAL_DATA_RATE;
   spatial_info->sample_rate = sample_create();
 
+  // Ahrs Defaults:
+  spatial_info->orientation_q[0] = 1.0;
+  spatial_info->orientation_q[1] = 0.0;
+  spatial_info->orientation_q[2] = 0.0;
+  spatial_info->orientation_q[3] = 0.0;
+
   // Setup a spatial handle
   CPhidgetSpatialHandle spatial = 0;
   ensure(CPhidgetSpatial_create(&spatial));
@@ -446,6 +586,7 @@ VALUE spatial_zero_gyro(VALUE self) {
 
   ensure(CPhidgetSpatial_zeroGyro((CPhidgetSpatialHandle) info->handle));
   memset(spatial_info->gyroscope, 0, sizeof(double) * spatial_info->gyro_axes);
+  spatial_info->is_ahrs_initialized = false;
 
   return Qnil;
 }
@@ -481,9 +622,11 @@ VALUE spatial_compass_correction_set(VALUE self, VALUE compass_correction) {
     }
 
     spatial_info->is_compass_correction_known = true; 
-    if (info->is_attached)
+    if (info->is_attached) {
       spatial_set_compass_correction_by_array( 
         (CPhidgetSpatialHandle)info->handle, spatial_info->compass_correction);
+      spatial_info->is_ahrs_initialized = false;
+    }
   }
 
   return compass_correction;
@@ -531,3 +674,266 @@ VALUE spatial_data_rate_get(VALUE self) {
 
   return INT2FIX(spatial_info->data_rate);
 }
+
+VALUE spatial_direction_cosine_matrix(int argc, VALUE *argv, VALUE self) {
+  VALUE around_x;
+  VALUE around_y; 
+  VALUE around_z; 
+  VALUE in_order;
+
+  const char *in_order_str = NULL;
+  double dbl_around_x;
+  double dbl_around_y;
+  double dbl_around_z;
+
+  rb_scan_args(argc, argv, "31", &around_x, &around_y, &around_z, &in_order);
+
+  if (TYPE(in_order) == T_STRING)
+    in_order_str = StringValueCStr(in_order);
+  else if( TYPE(in_order) != T_NIL)
+    rb_raise(rb_eTypeError, MSG_IN_ORDER_MUST_BE_STRING);
+
+  // Validate the inputs:
+  if ( (TYPE(around_x) == T_FLOAT) || (TYPE(around_x) == T_FIXNUM ) )
+    dbl_around_x = NUM2DBL(around_x);
+  else
+    rb_raise(rb_eTypeError, MSG_AROUND_MUST_BE_NUMERIC);
+
+  if ( (TYPE(around_y) == T_FLOAT) || (TYPE(around_y) == T_FIXNUM) ) 
+    dbl_around_y = NUM2DBL(around_y);
+  else
+    rb_raise(rb_eTypeError, MSG_AROUND_MUST_BE_NUMERIC);
+
+  if ( (TYPE(around_z) == T_FLOAT) || (TYPE(around_z) == T_FIXNUM) )
+    dbl_around_z = NUM2DBL(around_z);
+  else
+    rb_raise(rb_eTypeError, MSG_AROUND_MUST_BE_NUMERIC);
+
+  // Perform the actual rotation:
+  double mRetDcm[3][3] = {{1.0,0.0,0.0}, {0.0,1.0,0.0}, {0.0,0.0,1.0}};
+
+  if (euler_to_3x3dcm((double *) &mRetDcm, dbl_around_x, dbl_around_y, 
+    dbl_around_z, (in_order_str) ? in_order_str : NULL) != 0)
+    rb_raise(rb_eTypeError, MSG_INVALID_ROTATION_AXIS);
+
+  // Return our results:
+  return double3x3_to_matrix_rb((double (*)[3])&mRetDcm);
+}
+
+VALUE spatial_accelerometer_to_roll_and_pitch(VALUE self) {
+  SpatialInfo *spatial_info = device_type_info(self);
+
+  if (!spatial_info->is_acceleration_known) return Qnil;
+
+  double dOrientEuler[3];
+  spatial_orientation_from_accel_and_compass( (double *)&dOrientEuler, 
+    spatial_info->acceleration[0], spatial_info->acceleration[1], spatial_info->acceleration[2], 
+    spatial_info->compass[0], spatial_info->compass[1], spatial_info->compass[2]);
+
+  // Construct a splat-able return of roll and pitch:
+  VALUE aryRet = rb_ary_new2(2);
+
+  rb_ary_store(aryRet, 0, rb_float_new(dOrientEuler[1])); // Roll
+  rb_ary_store(aryRet, 1, rb_float_new(dOrientEuler[0])); // Pitch
+
+  return aryRet;
+}
+
+VALUE spatial_gravity_to_roll_and_pitch(VALUE self) {
+  VALUE aryRollPitch = rb_funcall(self, rb_intern("accelerometer_to_roll_and_pitch"), 0);
+
+  return aryRollPitch;
+}
+
+VALUE spatial_accelerometer_to_euler(VALUE self) {
+  SpatialInfo *spatial_info = device_type_info(self);
+
+  if (!spatial_info->is_acceleration_known)
+    return Qnil;
+
+  VALUE aryRollPitch = rb_funcall(self, rb_intern("accelerometer_to_roll_and_pitch"), 0);
+
+  VALUE roll = rb_ary_shift(aryRollPitch);
+  VALUE pitch = rb_ary_shift(aryRollPitch);
+  
+  VALUE aryRet = rb_ary_new2(3);
+
+  rb_ary_store(aryRet, 0, rb_float_new(NUM2DBL(roll) * -1.00 + M_PI_2));
+  rb_ary_store(aryRet, 1, rb_float_new(0.0));
+  rb_ary_store(aryRet, 2, pitch);
+
+  return aryRet;
+}
+
+VALUE spatial_compass_bearing(VALUE self) {
+  // Get the Compass Values:
+  VALUE aryCompass = rb_funcall(self, rb_intern("compass"), 0);
+
+  if ( (TYPE(aryCompass) == T_NIL) || (RARRAY_LEN(aryCompass) != 3) )
+    return Qnil;
+  
+  double dbl_comp_x = NUM2DBL(rb_ary_shift(aryCompass));
+  double dbl_comp_y = NUM2DBL(rb_ary_shift(aryCompass));
+  double dbl_comp_z = NUM2DBL(rb_ary_shift(aryCompass));
+
+  // Get the Acceleration Values (All we really need here is 'z'):
+  VALUE aryAccel = rb_funcall(self, rb_intern("accelerometer"), 0);
+
+  if ( (TYPE(aryAccel) == T_NIL) || (RARRAY_LEN(aryAccel) != 3) )
+    return Qnil;
+
+  double dbl_accel_x = NUM2DBL(rb_ary_shift(aryAccel));
+  double dbl_accel_y = NUM2DBL(rb_ary_shift(aryAccel));
+  double dbl_accel_z = NUM2DBL(rb_ary_shift(aryAccel));
+
+  // Get the Gravity Vector:
+  VALUE aryGrvRollPitch = rb_funcall(self, rb_intern("gravity_to_roll_and_pitch"), 0);
+
+  double dbl_grv_roll = NUM2DBL(rb_ary_shift(aryGrvRollPitch));
+  double dbl_grv_pitch = NUM2DBL(rb_ary_shift(aryGrvRollPitch));
+
+  /*
+   * Yaw Angle - about axis 2
+   *   tan(yaw) = (mz * sin(roll) – my * cos(roll)) / 
+   *              (mx * cos(pitch) + my * sin(pitch) * sin(roll) + mz * sin(pitch) * cos(roll))
+   *   Use Atan2 to get our range in (-180 - 180)
+   *
+   *   Yaw angle == 0 degrees when axis 0 is pointing at magnetic north
+   */
+  double dbl_yaw = atan2( (dbl_comp_z * sin(dbl_grv_roll)) - (dbl_comp_y * cos(dbl_grv_roll)),
+     (dbl_comp_x * cos(dbl_grv_pitch)) + 
+     (dbl_comp_y * sin(dbl_grv_pitch) * sin(dbl_grv_roll)) + 
+     (dbl_comp_z * sin(dbl_grv_pitch) * cos(dbl_grv_roll)) );
+
+  // And we're done:
+  return rb_float_new( (dbl_accel_z < 0) ? ( dbl_yaw - M_PI_2 ) : ( dbl_yaw - M_PI * 1.5 ) );
+}
+
+VALUE spatial_compass_bearing_to_euler(VALUE self) {
+  VALUE compass_bearing = rb_funcall(self, rb_intern("compass_bearing"), 0);
+
+  if (TYPE(compass_bearing) == T_NIL)
+    return Qnil;
+  
+  VALUE aryRet = rb_ary_new2(3);
+
+  rb_ary_store(aryRet, 0, rb_float_new(0.0));
+  rb_ary_store(aryRet, 1, rb_float_new(0.0));
+  rb_ary_store(aryRet, 2, compass_bearing);
+
+  return aryRet;
+}
+
+VALUE spatial_accelerometer_to_dcm(VALUE self) {
+  VALUE aryAccelEu = rb_funcall(self, rb_intern("accelerometer_to_euler"), 0);
+
+  if ( (TYPE(aryAccelEu) == T_NIL) || (RARRAY_LEN(aryAccelEu) != 3) )
+    return Qnil;
+
+  VALUE angles[3];
+  for(unsigned int i=0; i<3; i++)
+    angles[i] = rb_ary_shift(aryAccelEu);
+
+  return rb_funcall(self, rb_intern("direction_cosine_matrix"), 3, angles[0], angles[1], angles[2]);
+}
+
+VALUE spatial_compass_bearing_to_dcm(VALUE self) {
+  VALUE aryCompassEu = rb_funcall(self, rb_intern("compass_bearing_to_euler"), 0);
+
+  if ( (TYPE(aryCompassEu) == T_NIL) || (RARRAY_LEN(aryCompassEu) != 3) )
+    return Qnil;
+
+  VALUE angles[3];
+  for(unsigned int i=0; i<3; i++)
+    angles[i] = rb_ary_shift(aryCompassEu);
+
+  return rb_funcall(self, rb_intern("direction_cosine_matrix"), 3, angles[0], angles[1], angles[2]);
+}
+
+VALUE spatial_gyro_to_dcm(VALUE self) {
+  SpatialInfo *spatial_info = device_type_info(self);
+
+  if ( (!spatial_info->is_gyroscope_known) || (spatial_info->gyro_axes != 3) )
+    return Qnil;
+
+  double mRetDcm[3][3];
+  memcpy(&mRetDcm, &spatial_info->gyroscope_dcm, sizeof(mRetDcm));
+
+  // Return our results:
+  return double3x3_to_matrix_rb((double (*)[3])&mRetDcm);
+}
+
+VALUE spatial_orientation_to_quaternion(VALUE self) {
+  SpatialInfo *spatial_info = device_type_info(self);
+
+  // NOTE: I'm reusing this conditional from the dcm test. I think we don't need
+  // to implement a separated is_orientation_known function as the two conditions
+  // are identical:
+  if ( (!spatial_info->is_gyroscope_known) || (spatial_info->gyro_axes != 3) )
+    return Qnil;
+
+  float fOrientationQ[4];
+  float fTranslatedQ[4];
+  double dblRetQ[4];
+
+  // Grabe the magwick quaternion:
+  memcpy(&fOrientationQ, &spatial_info->orientation_q, sizeof(fOrientationQ));
+
+  // Translate it into 'our' (aka opengl's) cordinate space:
+  spatial_madgeq_to_openglq((float *) &fOrientationQ, (float *) &fTranslatedQ);
+
+  // The Madgwick algorithem uses floats, and ruby wants doubles. Let's cast!
+  for(int i=0; i<4; i++)
+    dblRetQ[i] = (double) fTranslatedQ[i];
+
+  // Return our results:
+  return double_array_to_rb((double *) &dblRetQ, 4);
+}
+
+VALUE spatial_orientation_to_dcm(VALUE self) {
+  SpatialInfo *spatial_info = device_type_info(self);
+
+  if ( (!spatial_info->is_gyroscope_known) || (spatial_info->gyro_axes != 3) )
+    return Qnil;
+
+  float fOrientationQ[4];
+  float fTranslatedQ[4];
+  double dblRetDcm[3][3];
+
+  // Grabe the magwick quaternion:
+  memcpy(&fOrientationQ, &spatial_info->orientation_q, sizeof(fOrientationQ));
+
+  // Translate it into 'our' (aka opengl's) cordinate space:
+  spatial_madgeq_to_openglq((float *) &fOrientationQ, (float *) &fTranslatedQ);
+
+  quat_to_dcm((float *)&fTranslatedQ, (double (*)[3]) &dblRetDcm);
+
+  return double3x3_to_matrix_rb((double (*)[3])&dblRetDcm);
+}
+
+VALUE spatial_orientation_to_euler(VALUE self) {
+  SpatialInfo *spatial_info = device_type_info(self);
+
+  if ( (!spatial_info->is_gyroscope_known) || (spatial_info->gyro_axes != 3) )
+    return Qnil;
+
+  float fOrientationQ[4];
+  float fTranslatedQ[4];
+  float fEuler[3];
+  double dblRetEuler[3];
+
+  // Grabe the magwick quaternion:
+  memcpy(&fOrientationQ, &spatial_info->orientation_q, sizeof(fOrientationQ));
+
+  // Translate it into 'our' (aka opengl's) cordinate space:
+  spatial_madgeq_to_openglq((float *) &fOrientationQ, (float *) &fTranslatedQ);
+
+  quat_to_euler((float *)&fTranslatedQ, (float (*))&fEuler);
+
+  // The Madgwick algorithem uses floats, and ruby wants doubles. Let's cast!
+  for(int i=0; i<3; i++)
+    dblRetEuler[i] = (double) fEuler[i];
+
+  // Return our results:
+  return double_array_to_rb((double *) &dblRetEuler, 3);
+}