phidgets_native 0.2.1 → 0.2.5

checksums.yaml

@@ -0,0 +1,15 @@
+---
+!binary "U0hBMQ==":
+  metadata.gz: !binary |-
+    ZTJhOGE5ZGUyNDM2ZjExODdjMjVjMWRhOTU0YjZjMjdhZjRlNWRiNw==
+  data.tar.gz: !binary |-
+    MDMwMDllMGFlMzQ2NzhjZTdlOWJlNzgxZWFkOTQxYzFmNTczMzhlYQ==
+SHA512:
+  metadata.gz: !binary |-
+    ZjAzYTVkYTIyNzRjMTg3NjUyOTQyOGQwMTFmM2Y4MzZlM2I4MDcwMDJlOTU0
+    NTlmMTNhODM2Y2QzOWViMzYxYzA0MTI1OWRjMGU2YzFmYWRjYjhmYzBmZmI4
+    MTZhN2RlYmEzZTNkZGU3NGIxODQ1MjQ2ZTJlYzVmOGEwNjljYjA=
+  data.tar.gz: !binary |-
+    OGNlOGE5MTk0MDMxZWY1ZTM3Mzg2MzU5MWI5ZDEzNTVmYWEzM2E3YjJmNDY2
+    MDZjMTJhZDM1ODljMzFiZmQzYWEwOGVmYWJkZWE1MGI3Mzk0ZWEwNTIxMjA4
+    YTk5ODdjMDFhMWMyZWFkNWUzOWJkZDEwZDIwYjRiNTJmNzU2MzA=

data/Gemfile

@@ -1,3 +1,6 @@
 source 'https://rubygems.org'
 
+gem 'rake'
+gem 'rdoc'
 gem 'rake-compiler'
+gem 'rspec'

data/Gemfile.lock

@@ -1,12 +1,27 @@
 GEM
   remote: https://rubygems.org/
   specs:
-    rake (10.1.0)
-    rake-compiler (0.8.3)
+    diff-lcs (1.2.5)
+    json (1.8.1)
+    rake (10.1.1)
+    rake-compiler (0.9.2)
       rake
+    rdoc (4.1.1)
+      json (~> 1.4)
+    rspec (2.14.1)
+      rspec-core (~> 2.14.0)
+      rspec-expectations (~> 2.14.0)
+      rspec-mocks (~> 2.14.0)
+    rspec-core (2.14.8)
+    rspec-expectations (2.14.5)
+      diff-lcs (>= 1.1.3, < 2.0)
+    rspec-mocks (2.14.6)
 
 PLATFORMS
   ruby
 
 DEPENDENCIES
+  rake
   rake-compiler
+  rdoc
+  rspec

data/README.rdoc

@@ -175,8 +175,16 @@ Note that you can also use the log method to send your own messages to the logge
 if you think you have something you need to add.
 
 =Changelog
+== Version 0.2.5
+- Added orientation tracking code to the Spatial sensor via the Madgwick AHRS library
+- Fixed compile issues on ARM
+
 == Version 0.2.1
 - Added dual-ratiometric mode features and interfaces to the InterfaceKit
 
 == Version 0.2.0
-- First gem release, supporting all api functions of the Spatial, GPS, InterfaceKit, & AdvancedServo phidgets
+- Added support for all api functions of the AdvancedServo Phidget 
+- Added support for all api functions of the InterfaceKit Phidget
+
+== Version 0.1.0
+- First gem release, supporting all api functions of the Spatial & GPS Phidgets

data/Rakefile

@@ -1,17 +1,14 @@
 # File: Rakefile
 
+require 'rake'
 require 'rake/extensiontask'
 require 'rake/packagetask'
 require 'rdoc/task'
+require 'rspec/core/rake_task'
 
 spec = Gem::Specification.load('phidgets_native.gemspec')
-
 Rake::ExtensionTask.new 'phidgets_native', spec
-
-Gem::PackageTask.new(spec) do |pkg|
-  
-
-end
+Gem::PackageTask.new spec
 
 RDOC_FILES = FileList["README.rdoc", "ext/phidgets_native/phidgets_native_ruby.c", 
   "ext/phidgets_native/*_ruby.c"]
@@ -29,3 +26,7 @@ Rake::RDocTask.new(:ri) do |rd|
   rd.rdoc_files.include(RDOC_FILES)
 end
 
+RSpec::Core::RakeTask.new('spec')
+
+task :spec => :compile
+

data/examples/spatial.rb

@@ -41,11 +41,12 @@ phidgets_example_for( PhidgetsNative::Spatial, additional_attribs ) do |spatial|
     'Sample Rate', 
     '%-40s' % 'Accelerometer', 
     '%-40s' % 'Gyroscope',
-    '%-40s' % 'Compass'
+    '%-40s' % 'Compass', 
+    '%-40s' % 'OrientationQ', 
   ]).output(:header => (i == 0), :separator => false) do |columns|
     i+=1
     [ ['%d Hz' % spatial.sample_rate]+[ 
-      spatial.accelerometer, spatial.gyro, spatial.compass ].collect{ |axes|
+      spatial.accelerometer, spatial.gyro, spatial.compass, spatial.orientation_to_quaternion ].collect{ |axes|
       (axes.kind_of? Array) ? 
         '[%s]' % axes.collect{|a| '%0.6f' % a}.join(', ') : axes.inspect
     } ]

data/examples/test_throttle.rb

@@ -0,0 +1,40 @@
+#!/usr/bin/env ruby
+# encoding: UTF-8
+
+require '%s/lib/common' % File.dirname(__FILE__)
+
+additional_attribs = [
+  %w(Motor\ Count motor_count),
+  %w(Acceleration\ Max acceleration_max),
+  %w(Acceleration\ Min acceleration_min),
+  %w(Velocity\ Max velocity_max),
+  %w(Velocity\ Min velocity_min)
+]
+
+phidgets_example_for(PhidgetsNative::AdvancedServo, additional_attribs) do |servo|
+  #servo.acceleration 0, servo.acceleration_max[0]
+  #servo.velocity_limit 0, servo.velocity_max[0]
+  # NOTE that these doesn't seem to be supported on all servo types
+  #servo.position_max 0, 220 
+  #servo.position_min 0, 0 
+  # /NOTE
+  servo.servo_type 0, :default
+  servo.speed_ramping 0, false
+  servo.engaged 0, true
+
+  #servo_range = servo.position_max(0) - servo.position_min(0)
+  #servo_middle = servo.position_min(0) + servo_range/2
+  #servo_left = servo.position_min(0) + servo_middle - servo_range / 4
+  #servo_right = servo.position_min(0) + servo_middle + servo_range / 4
+
+  servo.position 0, 0
+
+
+  puts "\nPolled Values:"
+  i = 0
+  begin
+    i+=1
+    puts "Hai #{i}"
+    servo.position 0, (i % 2 == 0) ? 0 : 92 # NOTE: 92 seems to be the lowest that goes
+  end while sleep(3)
+end

data/ext/phidgets_native/extconf.rb

@@ -1,5 +1,6 @@
 # File: extconf.rb
 require 'mkmf'
+require 'rbconfig'
 
 extension_name = 'phidgets_native'
 
@@ -8,11 +9,17 @@ HEADER_DIRS = [ '/Library/Frameworks/Phidget21.framework/Headers' ]
 find_header 'phidget21.h', *HEADER_DIRS
 find_library 'libphidget21', 'CPhidget_getLibraryVersion', '/usr/lib/'
 
-have_framework 'Phidget21' 
-have_library('phidget21') 
+case RbConfig::CONFIG['host_os']
+  when /mswin|msys|mingw|cygwin|bccwin|wince|emc/
+    raise StandardError, "Though possible to compile in win32, this extconf wasn't designed for it.... Fix me?"
+  when /darwin|mac os/
+    have_framework 'Phidget21' 
+  else
+    have_library('phidget21') 
+end
 
 dir_config extension_name
 
-$CFLAGS << ' -Wno-unused-variable -std=gnu99'
+$CFLAGS << ' -Wno-unused-variable -Wno-declaration-after-statement -std=gnu99'
 
 create_makefile extension_name

data/ext/phidgets_native/phidgets_native.c

@@ -44,6 +44,29 @@ VALUE phidgetbool_array_to_rb(int *bool_array, int length) {
   return rb_ary;
 }
 
+/* This converts a 3x3 array of doubles into a ruby matrix, or into
+ * nil for the case of an invalid dbl_array
+ */
+VALUE double3x3_to_matrix_rb(double m3x3[][3]) {
+  if (!m3x3) return Qnil;
+
+  // To compose a Matrix, we need a 3x3 array of floats:
+  VALUE aryRet = rb_ary_new2(3);
+  for (unsigned int i=0; i < 3; i++) {
+    VALUE arRow = rb_ary_new2(3);
+
+    for (unsigned int j=0; j < 3; j++)
+      rb_ary_store(arRow, j, rb_float_new(m3x3[i][j]));
+
+    rb_ary_store(aryRet, i, arRow);
+  }
+
+  VALUE c_Matrix = rb_const_get( rb_cObject, rb_intern("Matrix") );
+  VALUE rbMatrixRet = rb_funcall(c_Matrix, rb_intern("rows"), 2, aryRet, Qfalse);     
+  
+  return rbMatrixRet;  
+}
+
 // The name is ambiguous, but the main purpose here is dry things out a bit
 // and let us do a better job of reporting errors to ruby
 int ensure(int result) {
@@ -195,3 +218,104 @@ int sample_tick(SampleRate *sample_rate, CPhidget_Timestamp *ts) {
   return 0;
 }
 
+// There's a number of ways to implement this, at the moment, I'm more interested
+// in accuracy... so we use the close-to-optimal method with low cost from :
+//  http://pizer.wordpress.com/2008/10/12/fast-inverse-square-root 
+// as opposed to: 1.0f / sqrtf(x)
+float inv_sqrt(float x) {
+  unsigned int i = 0x5F1F1412 - (*(unsigned int*)&x >> 1);
+  float tmp = *(float*)&i;
+  return tmp * (1.69000231f - 0.714158168f * x * tmp * tmp);
+}
+
+
+// Quaternion Multiplication operator. Expects its 4-element arrays in wxyz order 
+void quat_mult(float a[4], float b[4], float ret[4]) {
+  ret[0] = (b[0] * a[0]) - (b[1] * a[1]) - (b[2] * a[2]) - (b[3] * a[3]);
+  ret[1] = (b[0] * a[1]) + (b[1] * a[0]) + (b[2] * a[3]) - (b[3] * a[2]);
+  ret[2] = (b[0] * a[2]) + (b[2] * a[0]) + (b[3] * a[1]) - (b[1] * a[3]);
+  ret[3] = (b[0] * a[3]) + (b[3] * a[0]) + (b[1] * a[2]) - (b[2] * a[1]);
+
+  return;
+}
+
+// Quaternion Normalization operator. Expects its 4-element arrays in wxyz order 
+void quat_norm(float a[4]) {
+  float n = a[1]*a[1] + a[2]*a[2] + a[3]*a[3] + a[0]*a[0];
+
+  if (n == 1.0f) 
+    return ;
+
+  n = inv_sqrt(n);
+
+  a[1]*=n;
+  a[2]*=n;
+  a[3]*=n;
+  a[0]*=n;
+
+  return;
+}
+
+// Quaternion to direction cosine matrix. Simple enough: 
+void quat_to_dcm(float q[4], double dcm[][3]) {
+  float q_w = q[0];
+  float q_x = q[1];
+  float q_y = q[2];
+  float q_z = q[3];
+
+  float sqw = q_w*q_w;
+  float sqx = q_x*q_x;
+  float sqy = q_y*q_y;
+  float sqz = q_z*q_z;
+
+  // invs (inverse square length) is only required if quaternion is not already normalised
+  float invs = 1 / (sqx + sqy + sqz + sqw);
+  dcm[0][0] = ( sqx - sqy - sqz + sqw)*invs; // since sqw + sqx + sqy + sqz =1/invs*invs
+  dcm[1][1] = (-sqx + sqy - sqz + sqw)*invs;
+  dcm[2][2] = (-sqx - sqy + sqz + sqw)*invs;
+
+  float tmp1 = q_x*q_y;
+  float tmp2 = q_z*q_w;
+
+  dcm[1][0] = 2.0 * (tmp1 + tmp2)*invs;
+  dcm[0][1] = 2.0 * (tmp1 - tmp2)*invs;
+
+  tmp1 = q_x*q_z;
+  tmp2 = q_y*q_w;
+  dcm[2][0] = 2.0 * (tmp1 - tmp2)*invs;
+  dcm[0][2] = 2.0 * (tmp1 + tmp2)*invs;
+
+  tmp1 = q_y*q_z;
+  tmp2 = q_x*q_w;
+  dcm[2][1] = 2.0 * (tmp1 + tmp2)*invs;
+  dcm[1][2] = 2.0 * (tmp1 - tmp2)*invs;      
+
+  return;
+}
+
+
+// Quaternion to euler:
+void quat_to_euler(float q[4], float e[3]) {
+  float sqw = q[0]*q[0];
+  float sqx = q[1]*q[1];
+  float sqy = q[2]*q[2];
+  float sqz = q[3]*q[3];
+  e[2] = -1.0f * atan2f(2.f * (q[1]*q[2] + q[3]*q[0]), sqx - sqy - sqz + sqw);
+  e[1] = asinf(-2.f * (q[1]*q[3] - q[2]*q[0]));
+  e[0] = -1.0f * atan2f(2.f * (q[2]*q[3] + q[1]*q[0]), -sqx - sqy + sqz + sqw);
+
+  return;
+}
+
+// Given an axis, and a angle (in radians), construct a quat
+// NOTE: Assumes axis is already normalised
+void quat_from_axis_and_angle(double axis[3], double angle, float fRetQ[4]) {
+  double s = sin(angle/2);
+  fRetQ[0] = cos(angle/2); // w
+  fRetQ[1] = axis[0] * s;  // x
+  fRetQ[2] = axis[1] * s;  // y
+  fRetQ[3] = axis[2] * s;  // z
+
+  return;
+}
+

data/ext/phidgets_native/phidgets_native.h

@@ -1,8 +1,10 @@
 #include <stdio.h>
 #include <stdbool.h>
 #include <math.h>
+#include <sys/time.h>
 #include <time.h>
 #include <unistd.h>
+#include <ctype.h>
 
 #include <ruby.h>
 #include <phidget21.h>
@@ -15,6 +17,8 @@ static double const MICROSECONDS_IN_SECOND = 1000000.0;
 static int const DEGREES_IN_CIRCLE = 360;
 static int const DEFAULT_SPATIAL_DATA_RATE = 16;
 
+static float const SPATIAL_AHRS_BETA = 0.1f;	// 2 * proportional gain (Kp)
+
 // Make sure one of these is non-zero and the other is 0
 static int const DEFAULT_INTERFACEKIT_DATA_RATE = 8;
 static int const DEFAULT_INTERFACEKIT_CHANGE_TRIGGER = 0;
@@ -90,9 +94,16 @@ typedef struct spatial_info {
 
   bool is_gyroscope_known;
   double *gyroscope;
+  double gyroscope_dcm[3][3];
 
   // This is used by the gyro:
   double last_microsecond;
+
+  // quaternion elements representing the estimated orientation
+  float orientation_q[4];
+
+  // This tells us whether it's our first pass
+  bool is_ahrs_initialized;
 } SpatialInfo;
 
 typedef struct gps_info {
@@ -212,12 +223,19 @@ VALUE double_array_to_rb(double *dbl_array, int length);
 VALUE int_array_to_rb(int *int_array, int length);
 VALUE int_array_zeronils_to_rb(int *int_array, int length);
 VALUE phidgetbool_array_to_rb(int *bool_array, int length);
+VALUE double3x3_to_matrix_rb(double m3x3[][3]);
 int ensure(int result);
 int report(int result);
 SampleRate *sample_create();
 int sample_free(SampleRate *sample_rate);
 int sample_zero(SampleRate *sample_rate);
 int sample_tick(SampleRate *sample_rate, CPhidget_Timestamp *ts);
+float inv_sqrt(float x);
+void quat_mult(float a[4], float b[4], float ret[4]);
+void quat_norm(float a[4]);
+void quat_to_dcm(float q[4], double dcm[][3]);
+void quat_to_euler(float q[4], float e[3]);
+void quat_from_axis_and_angle(double axis[3], double angle, float fRetQ[4]);
 
 // Phidget Module
 VALUE phidget_enable_logging(int argc, VALUE *argv, VALUE class);
@@ -252,6 +270,12 @@ int CCONV spatial_on_attach(CPhidgetHandle phid, void *userptr);
 int CCONV spatial_on_detach(CPhidgetHandle phid, void *userptr);
 int CCONV spatial_on_data(CPhidgetSpatialHandle spatial, void *userptr, CPhidgetSpatial_SpatialEventDataHandle *data, int count);
 int spatial_set_compass_correction_by_array(CPhidgetSpatialHandle phid, double *correction);
+void spatial_ahrs_update(SpatialInfo *spatial_info, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+void spatial_ahrs_update_imu(SpatialInfo *spatial_info, float gx, float gy, float gz, float ax, float ay, float az);
+void spatial_ahrs_init(SpatialInfo *spatial_info, double ax, double ay, double az, double mx, double my, double mz);
+void spatial_orientation_from_accel_and_compass(double dRetEuler[3], double ax, double ay, double az, double mx, double my, double mz);
+void spatial_madgeq_to_openglq(float *fMadgQ, float *fRetQ);
+
 VALUE spatial_initialize(VALUE self, VALUE serial);
 VALUE spatial_close(VALUE self);
 VALUE spatial_sample_rate(VALUE self);
@@ -280,6 +304,21 @@ VALUE spatial_data_rate_max(VALUE self);
 VALUE spatial_data_rate_set(VALUE self, VALUE data_rate);
 VALUE spatial_data_rate_get(VALUE self);
 
+VALUE spatial_direction_cosine_matrix(int argc, VALUE *argv, VALUE self);
+VALUE spatial_accelerometer_to_roll_and_pitch(VALUE self);
+VALUE spatial_accelerometer_to_euler(VALUE self);
+VALUE spatial_accelerometer_to_dcm(VALUE self);
+VALUE spatial_gravity_to_roll_and_pitch(VALUE self);
+VALUE spatial_compass_bearing(VALUE self);
+VALUE spatial_compass_bearing_to_euler(VALUE self);
+VALUE spatial_compass_bearing_to_dcm(VALUE self);
+VALUE spatial_gyro_to_dcm(VALUE self);
+VALUE spatial_orientation_to_quaternion(VALUE self);
+VALUE spatial_orientation_to_dcm(VALUE self);
+VALUE spatial_orientation_to_euler(VALUE self);
+
+int euler_to_3x3dcm(double *mRet, double around_x, double around_y, double around_z, const char *in_order);
+
 // PhidgetsNative::InterfaceKit
 void interfacekit_on_free(void *type_info);
 int CCONV interfacekit_on_attach(CPhidgetHandle phid, void *userptr);

data/ext/phidgets_native/spatial.c

@@ -1,5 +1,7 @@
 #include "phidgets_native.h"
 
+const char DEFAULT_IN_ORDER_STR[] = "xyz";
+
 void spatial_on_free(void *type_info) {
   SpatialInfo *spatial_info = type_info;
   if (spatial_info->acceleration)
@@ -34,6 +36,57 @@ int spatial_set_compass_correction_by_array(CPhidgetSpatialHandle phid, double *
     cc[10], cc[11], cc[12] ));
 }
 
+/*
+ * This method will rotate the double[3][3] mRet around the axial rotations, in order of in_order )
+ */
+int euler_to_3x3dcm(double *mRet, double around_x, double around_y, double around_z, const char *in_order) {
+  double mA[3][3];
+  double mB[3][3];
+  double mAccum[3][3]; 
+
+  memcpy(&mAccum, mRet, sizeof(mAccum));
+
+  unsigned long in_order_length = strlen((in_order) ? in_order : DEFAULT_IN_ORDER_STR); 
+
+  for (unsigned long i = 0; i < in_order_length; i++) {
+    switch (tolower((in_order) ? in_order[i] : DEFAULT_IN_ORDER_STR[i])) {
+      case 'x':
+        mB[0][0] = 1.0; mB[0][1] = 0.0;           mB[0][2] = 0.0;
+        mB[1][0] = 0.0; mB[1][1] = cos(around_x); mB[1][2] = sin(around_x) * (-1.0);
+        mB[2][0] = 0.0; mB[2][1] = sin(around_x); mB[2][2] = cos(around_x);
+        break;
+      case 'y':
+        mB[0][0] = cos(around_y);          mB[0][1] = 0.0; mB[0][2] = sin(around_y);
+        mB[1][0] = 0.0;                    mB[1][1] = 1.0; mB[1][2] = 0.0;
+        mB[2][0] = sin(around_y) * (-1.0); mB[2][1] = 0.0; mB[2][2] = cos(around_y);
+        break;
+      case 'z':
+        mB[0][0] = cos(around_z); mB[0][1] = sin(around_z) * (-1.0); mB[0][2] = 0.0;
+        mB[1][0] = sin(around_z); mB[1][1] = cos(around_z);          mB[1][2] = 0.0;
+        mB[2][0] = 0.0;           mB[2][1] = 0.0;                    mB[2][2] = 1.0;
+        break;
+      default:
+        // Fatal Error - return without setting mRet
+        return 1;
+        break;
+    }
+
+    // Accumulate into ret, via multiplication
+    memcpy(&mA, &mAccum, sizeof(mAccum));
+    memset(&mAccum, 0, sizeof(mAccum));
+ 
+    for (unsigned int i = 0; i < 3; i++)
+      for (unsigned int j = 0; j < 3; j++)
+        for (unsigned int k = 0; k < 3; k++)
+          mAccum[i][j] += mA[i][k] * mB[k][j];
+  }
+  
+  // Success:
+  memcpy(mRet, &mAccum, sizeof(mAccum));
+
+  return 0;
+}
+
 int CCONV spatial_on_attach(CPhidgetHandle phid, void *userptr) {
   PhidgetInfo *info = userptr;
   SpatialInfo *spatial_info = info->type_info;
@@ -67,6 +120,12 @@ int CCONV spatial_on_attach(CPhidgetHandle phid, void *userptr) {
   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]));
@@ -83,6 +142,9 @@ int CCONV spatial_on_attach(CPhidgetHandle phid, void *userptr) {
     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));
@@ -104,6 +166,7 @@ int CCONV spatial_on_detach(CPhidgetHandle phidget, void *userptr) {
   memset(spatial_info->acceleration, 0, sizeof(double) * spatial_info->accelerometer_axes);
   memset(spatial_info->gyroscope, 0, sizeof(double) * spatial_info->gyro_axes);
   memset(spatial_info->compass, 0, sizeof(double) * spatial_info->compass_axes);
+  memset(&spatial_info->gyroscope_dcm, 0, sizeof(spatial_info->gyroscope_dcm));
 
   spatial_info->last_microsecond = 0;
   spatial_info->is_acceleration_known = false;
@@ -145,24 +208,357 @@ int CCONV spatial_on_data(CPhidgetSpatialHandle spatial, void *userptr, CPhidget
       spatial_info->is_compass_known = true;
     } 
 
-    // Gyros get handled slightly different:
-    // NOTE: Other people may have a better way to do this, but this is the method
-    // I grabbed from the phidget sample. Maybe I should report these in radians...
+    // Gyro Time! these get handled slightly different...
     double timestamp = data[i]->timestamp.seconds + data[i]->timestamp.microseconds/MICROSECONDS_IN_SECOND;
 
-    if (spatial_info->last_microsecond > 0) {
-      double timechange = timestamp - spatial_info->last_microsecond;
-
-      for(int j=0; j < spatial_info->gyro_axes; j++)
-        spatial_info->gyroscope[j] += data[i]->angularRate[j] * timechange;
-    }
+    double timechange = (spatial_info->last_microsecond > 0) ? 
+      timestamp - spatial_info->last_microsecond : 0;
+    
+    // This is kind of superceeded by the orientation_ functions, but I'm 
+    // leaving it in for posterity:
+    for(int j=0; j < spatial_info->gyro_axes; j++)
+      spatial_info->gyroscope[j] += data[i]->angularRate[j] * timechange;
 
     spatial_info->is_gyroscope_known = true;
 
+    // If we're dealing with 3 dimensional sensors, we can calculate orientation:
+    if (spatial_info->gyro_axes == 3) {
+      // This is a shortcut to keep things DRY below:
+      double angular_rate_in_rad[3];
+      for(int j=0; j < 3; j++)
+        angular_rate_in_rad[j] = data[i]->angularRate[j] * M_PI/180;
+
+      euler_to_3x3dcm((double *) &spatial_info->gyroscope_dcm, 
+        angular_rate_in_rad[0] * timechange,
+        angular_rate_in_rad[1] * timechange,
+        angular_rate_in_rad[2] * timechange,
+        NULL);
+
+      // Madgwick Orientation time - Update the ahrs: 
+      if (!spatial_info->is_ahrs_initialized) {
+        if (spatial_info->is_compass_known)
+          spatial_ahrs_init(spatial_info,
+            data[i]->acceleration[0],
+            data[i]->acceleration[1],
+            data[i]->acceleration[2],
+            data[i]->magneticField[0], 
+            data[i]->magneticField[1], 
+            data[i]->magneticField[2]);
+      } else {
+        float gx = (float) angular_rate_in_rad[0];
+        float gy = (float) angular_rate_in_rad[1];
+        float gz = (float) angular_rate_in_rad[2];
+
+        float ax = (float) data[i]->acceleration[0];
+        float ay = (float) data[i]->acceleration[1];
+        float az = (float) data[i]->acceleration[2];
+
+        if (spatial_info->is_compass_known) 
+          // We have enough info for a MARG update:
+          spatial_ahrs_update(spatial_info, gx, gy, gz, ax, ay, az,
+            (float) data[i]->magneticField[0], 
+            (float) data[i]->magneticField[1], 
+            (float) data[i]->magneticField[2]);
+        else
+          // We only have enough info for a IMU update:
+          spatial_ahrs_update_imu(spatial_info, gx, gy, gz, ax, ay, az);
+      }
+    }
+
     // We'll need this on the next go around:
     spatial_info->last_microsecond = timestamp;
+
   }
 
   return 0;
 }
 
+void spatial_orientation_from_accel_and_compass(double dRetEuler[3], 
+  double ax, double ay, double az, 
+  double mx, double my, double mz) {
+
+  /*
+   * Roll Angle - about axis 0
+   *  tan(roll) = gy/gz
+   *  Use Atan2 so we have an output os (-180 - 180) degrees
+   */
+  double dbl_grv_roll = atan2(ay, az);
+
+  /*
+   * Pitch Angle - about axis 1
+   *   tan(pitch) = -gx / (gy * sin(roll angle) * gz * cos(roll angle))
+   *   Pitch angle range is (-90 - 90) degrees
+   */
+  double dbl_grv_pitch = atan(-1.0 * ax / (ay * sin(dbl_grv_roll) + az * cos(dbl_grv_roll)));
+
+  /*
+   * 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( (mz * sin(dbl_grv_roll)) - (my * cos(dbl_grv_roll)),
+     (mx * cos(dbl_grv_pitch)) + 
+     (my * sin(dbl_grv_pitch) * sin(dbl_grv_roll)) + 
+     (mz * sin(dbl_grv_pitch) * cos(dbl_grv_roll)) );
+
+  dRetEuler[0] = dbl_grv_pitch;
+  dRetEuler[1] = dbl_grv_roll;
+  dRetEuler[2] = dbl_yaw;
+
+  return;
+}
+
+
+// NOTE: The spatial_ahrs_* functions were largely copied from Madgwick's 
+// Quaternion implementation of the 'DCM filter' 
+// https://code.google.com/p/imumargalgorithm30042010sohm/
+void spatial_ahrs_init(SpatialInfo *spatial_info, 
+  double ax, double ay, double az, 
+  double mx, double my, double mz) {
+
+  double xaxis[3] = {1.0, 0.0, 0.0};
+  double yaxis[3] = {0.0, 1.0, 0.0};
+  double zaxis[3] = {0.0, 0.0, 1.0};
+
+  // We'll assume the current state by way of the accelerometer and compass readings:
+  double dOrientEuler[3];
+  spatial_orientation_from_accel_and_compass((double *)&dOrientEuler, ax, ay, az, mx, my, mz);
+
+  // Now we construct a Roll & Pitch Quat from the Euler's
+  float fRollQ[4];
+  quat_from_axis_and_angle((double *) &xaxis, dOrientEuler[1], (float *) &fRollQ);
+
+  float fPitchQ[4];
+  quat_from_axis_and_angle((double *) &yaxis, dOrientEuler[0], (float *) &fPitchQ);
+
+  float fRollPitchQ[4];
+  quat_mult((float *) &fRollQ, (float *) &fPitchQ, (float *)&fRollPitchQ);
+
+  // Rotate the the Quat around our 'Z' so that we're facing North:
+  // NOTE, Due to the madgwick origin, -0.5*M_PI is North, and we add the 
+  // bearing offset to this angle
+  double bearing_in_rad = 2*M_PI+dOrientEuler[2]; 
+
+  float fCompassQ[4];
+  quat_from_axis_and_angle((double *) &zaxis, bearing_in_rad, (float *) &fCompassQ);
+
+  // Commit this to the persisting orientation state:
+  quat_mult((float *) &fRollPitchQ, (float *) &fCompassQ, (float *) spatial_info->orientation_q);
+
+  spatial_info->is_ahrs_initialized = true;
+}
+
+void spatial_ahrs_update(SpatialInfo *spatial_info, 
+  float gx, float gy, float gz, 
+  float ax, float ay, float az, 
+  float mx, float my, float mz) {
+
+  // Just a shorthand meant to make this a bit easier to work with, and transact
+  // commits (eventually)
+  float q0 = spatial_info->orientation_q[0];
+  float q1 = spatial_info->orientation_q[1];
+  float q2 = spatial_info->orientation_q[2];
+  float q3 = spatial_info->orientation_q[3];
+
+  float sample_rate_in_hz = 1000.0f / spatial_info->data_rate;
+
+	float recipNorm;
+	float s0, s1, s2, s3;
+	float qDot1, qDot2, qDot3, qDot4;
+	float hx, hy;
+	float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+	// Rate of change of quaternion from gyroscope
+	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+		// Normalise accelerometer measurement
+		recipNorm = inv_sqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;   
+
+		// Normalise magnetometer measurement
+		recipNorm = inv_sqrt(mx * mx + my * my + mz * mz);
+		mx *= recipNorm;
+		my *= recipNorm;
+		mz *= recipNorm;
+
+		// Auxiliary variables to avoid repeated arithmetic
+		_2q0mx = 2.0f * q0 * mx;
+		_2q0my = 2.0f * q0 * my;
+		_2q0mz = 2.0f * q0 * mz;
+		_2q1mx = 2.0f * q1 * mx;
+		_2q0 = 2.0f * q0;
+		_2q1 = 2.0f * q1;
+		_2q2 = 2.0f * q2;
+		_2q3 = 2.0f * q3;
+		_2q0q2 = 2.0f * q0 * q2;
+		_2q2q3 = 2.0f * q2 * q3;
+		q0q0 = q0 * q0;
+		q0q1 = q0 * q1;
+		q0q2 = q0 * q2;
+		q0q3 = q0 * q3;
+		q1q1 = q1 * q1;
+		q1q2 = q1 * q2;
+		q1q3 = q1 * q3;
+		q2q2 = q2 * q2;
+		q2q3 = q2 * q3;
+		q3q3 = q3 * q3;
+
+		// Reference direction of Earth's magnetic field
+		hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+		hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+		_2bx = sqrt(hx * hx + hy * hy);
+		_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+		_4bx = 2.0f * _2bx;
+		_4bz = 2.0f * _2bz;
+
+		// Gradient decent algorithm corrective step
+		s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		recipNorm = inv_sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+		s0 *= recipNorm;
+		s1 *= recipNorm;
+		s2 *= recipNorm;
+		s3 *= recipNorm;
+
+		// Apply feedback step
+		qDot1 -= SPATIAL_AHRS_BETA * s0;
+		qDot2 -= SPATIAL_AHRS_BETA * s1;
+		qDot3 -= SPATIAL_AHRS_BETA * s2;
+		qDot4 -= SPATIAL_AHRS_BETA * s3;
+	}
+
+	// Integrate rate of change of quaternion to yield quaternion
+	q0 += qDot1 * (1.0f / sample_rate_in_hz);
+	q1 += qDot2 * (1.0f / sample_rate_in_hz);
+	q2 += qDot3 * (1.0f / sample_rate_in_hz);
+	q3 += qDot4 * (1.0f / sample_rate_in_hz);
+
+	// Normalise quaternion
+	recipNorm = inv_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+	q0 *= recipNorm;
+	q1 *= recipNorm;
+	q2 *= recipNorm;
+	q3 *= recipNorm;
+
+	spatial_info->orientation_q[0] = q0;
+	spatial_info->orientation_q[1] = q1;
+	spatial_info->orientation_q[2] = q2;
+	spatial_info->orientation_q[3] = q3;
+}
+
+void spatial_ahrs_update_imu(SpatialInfo *spatial_info, 
+    float gx, float gy, float gz, 
+    float ax, float ay, float az) {
+
+  // Just a shorthand meant to make this a bit easier to work with, and transact
+  // commits (eventually)
+  float q0 = spatial_info->orientation_q[0];
+  float q1 = spatial_info->orientation_q[1];
+  float q2 = spatial_info->orientation_q[2];
+  float q3 = spatial_info->orientation_q[3];
+
+  float sample_rate_in_hz = 1000.0f / spatial_info->data_rate;
+
+	float recipNorm;
+	float s0, s1, s2, s3;
+	float qDot1, qDot2, qDot3, qDot4;
+	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+	// Rate of change of quaternion from gyroscope
+	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+		// Normalise accelerometer measurement
+		recipNorm = inv_sqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;   
+
+		// Auxiliary variables to avoid repeated arithmetic
+		_2q0 = 2.0f * q0;
+		_2q1 = 2.0f * q1;
+		_2q2 = 2.0f * q2;
+		_2q3 = 2.0f * q3;
+		_4q0 = 4.0f * q0;
+		_4q1 = 4.0f * q1;
+		_4q2 = 4.0f * q2;
+		_8q1 = 8.0f * q1;
+		_8q2 = 8.0f * q2;
+		q0q0 = q0 * q0;
+		q1q1 = q1 * q1;
+		q2q2 = q2 * q2;
+		q3q3 = q3 * q3;
+
+		// Gradient decent algorithm corrective step
+		s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+		s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+		s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+		s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+		recipNorm = inv_sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+		s0 *= recipNorm;
+		s1 *= recipNorm;
+		s2 *= recipNorm;
+		s3 *= recipNorm;
+
+		// Apply feedback step
+		qDot1 -= SPATIAL_AHRS_BETA * s0;
+		qDot2 -= SPATIAL_AHRS_BETA * s1;
+		qDot3 -= SPATIAL_AHRS_BETA * s2;
+		qDot4 -= SPATIAL_AHRS_BETA * s3;
+	}
+
+	// Integrate rate of change of quaternion to yield quaternion
+	q0 += qDot1 * (1.0f / sample_rate_in_hz);
+	q1 += qDot2 * (1.0f / sample_rate_in_hz);
+	q2 += qDot3 * (1.0f / sample_rate_in_hz);
+	q3 += qDot4 * (1.0f / sample_rate_in_hz);
+
+	// Normalise quaternion
+	recipNorm = inv_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+	q0 *= recipNorm;
+	q1 *= recipNorm;
+	q2 *= recipNorm;
+	q3 *= recipNorm;
+
+	spatial_info->orientation_q[0] = q0;
+	spatial_info->orientation_q[1] = q1;
+	spatial_info->orientation_q[2] = q2;
+	spatial_info->orientation_q[3] = q3;
+}
+
+/*
+  The Madwick algorithm :
+   * Uses +x to indicate North, we want +y which means we need a -90 degree rotation around z. 
+   * Uses the following coordinate system: 
+     http://stackoverflow.com/questions/17788043/madgwicks-sensor-fusion-algorithm-on-ios
+*/
+void spatial_madgeq_to_openglq(float *fMadgQ, float *fRetQ) {
+  float fTmpQ[4];
+  float fXYRotationQ[4] = { sqrt(0.5), 0, 0, -1.0*sqrt(0.5) }; // wxyz
+
+  fTmpQ[0] = fMadgQ[0];
+  fTmpQ[1] = fMadgQ[1] * -1.0f;
+  fTmpQ[2] = fMadgQ[2];
+  fTmpQ[3] = fMadgQ[3] * -1.0f;
+
+  // And then store the Rotation into fTranslatedQ
+  quat_mult((float *) &fTmpQ, (float *) &fXYRotationQ, fRetQ);
+}