Add 10dof and 9dof AHRS example, bunnyrotate processing sketch, and fix bugs to make code work.

.gitignore

@@ -0,0 +1 @@
+serialconfig.txt

Adafruit_10DOF.h

@@ -8,12 +8,16 @@
 #include "Adafruit_Sensor_Set.h"
 
 // 10DOF board with an accelerometer, magnetometer, gyroscope, and pressure sensor.
-class Adafruit_10DOF: Adafruit_Sensor_Set
+class Adafruit_10DOF: public Adafruit_Sensor_Set
 {
 public:
   Adafruit_10DOF();
   bool begin();
   Adafruit_Sensor* getSensor(sensors_type_t type);
+  Adafruit_LSM303_Accel_Unified& getAccel() { return _accel; }
+  Adafruit_LSM303_Mag_Unified& getMag() { return _mag; }
+  Adafruit_L3GD20_Unified& getGyro() { return _gyro; }
+  Adafruit_BMP085_Unified& getBMP() { return _bmp; }
   
 private:
   Adafruit_LSM303_Accel_Unified _accel;

Adafruit_9DOF.h

@@ -13,12 +13,15 @@
 #include "Adafruit_Sensor_Set.h"
 
 // 9DOF board with an accelerometer, magnetometer, and gyroscope.
-class Adafruit_9DOF: Adafruit_Sensor_Set
+class Adafruit_9DOF: public Adafruit_Sensor_Set
 {
 public:
   Adafruit_9DOF();
   bool begin();
   Adafruit_Sensor* getSensor(sensors_type_t type);
+  Adafruit_LSM303_Accel_Unified& getAccel() { return _accel; }
+  Adafruit_LSM303_Mag_Unified& getMag() { return _mag; }
+  Adafruit_L3GD20_Unified& getGyro() { return _gyro; }
   
 private:
   Adafruit_LSM303_Accel_Unified _accel;

Adafruit_Simple_AHRS.cpp

@@ -7,7 +7,7 @@ Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafr
 {}
 
 // Create a simple AHRS from a device with multiple sensors.
-Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(const Adafruit_Sensor_Set& sensors):
+Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor_Set& sensors):
   _accel(sensors.getSensor(SENSOR_TYPE_ACCELEROMETER)),
   _mag(sensors.getSensor(SENSOR_TYPE_MAGNETIC_FIELD))
 {}
@@ -18,10 +18,10 @@ bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t* orientation) {
   if (orientation == NULL || _accel == NULL || _mag == NULL) return false;
 
   // Grab an acceleromter and magnetometer reading.
-  sensor_event_t accel_event;
-  _accel.getEvent(&accel_event);
-  sensor_event_t mag_event;
-  _mag.getEvent(&mag_event);
+  sensors_event_t accel_event;
+  _accel->getEvent(&accel_event);
+  sensors_event_t mag_event;
+  _mag->getEvent(&mag_event);
 
   float const PI_F = 3.14159265F;
 
@@ -33,7 +33,7 @@ bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t* orientation) {
   //                    z                                                                           
   //                                                                                                
   // where:  y, z are returned value from accelerometer sensor                                      
-  orientation->roll = (float)atan2(accel_event->acceleration.y, accel_event->acceleration.z);
+  orientation->roll = (float)atan2(accel_event.acceleration.y, accel_event.acceleration.z);
 
   // pitch: Rotation around the Y-axis. -180 <= roll <= 180                                         
   // a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis         
@@ -43,11 +43,11 @@ bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t* orientation) {
   //                    y * sin(roll) + z * cos(roll)                                               
   //                                                                                                
   // where:  x, y, z are returned value from accelerometer sensor                                   
-  if (accel_event->acceleration.y * sin(orientation->roll) + accel_event->acceleration.z * cos(orientation->roll) == 0)
-    orientation->pitch = accel_event->acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
+  if (accel_event.acceleration.y * sin(orientation->roll) + accel_event.acceleration.z * cos(orientation->roll) == 0)
+    orientation->pitch = accel_event.acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
   else
-    orientation->pitch = (float)atan(-accel_event->acceleration.x / (accel_event->acceleration.y * sin(orientation->roll) + \
-                                                                     accel_event->acceleration.z * cos(orientation->roll)));
+    orientation->pitch = (float)atan(-accel_event.acceleration.x / (accel_event.acceleration.y * sin(orientation->roll) + \
+                                                                     accel_event.acceleration.z * cos(orientation->roll)));
 
   // heading: Rotation around the Z-axis. -180 <= roll <= 180                                       
   // a positive heading angle is defined to be a clockwise rotation about the positive Z-axis       
@@ -57,10 +57,10 @@ bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t* orientation) {
   //                    x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll))   
   //                                                                                                
   // where:  x, y, z are returned value from magnetometer sensor                                    
-  orientation->heading = (float)atan2(mag_event->magnetic.z * sin(orientation->roll) - mag_event->magnetic.y * cos(orientation->roll), \
-                                      mag_event->magnetic.x * cos(orientation->pitch) + \
-                                      mag_event->magnetic.y * sin(orientation->pitch) * sin(orientation->roll) + \
-                                      mag_event->magnetic.z * sin(orientation->pitch) * cos(orientation->roll));
+  orientation->heading = (float)atan2(mag_event.magnetic.z * sin(orientation->roll) - mag_event.magnetic.y * cos(orientation->roll), \
+                                      mag_event.magnetic.x * cos(orientation->pitch) + \
+                                      mag_event.magnetic.y * sin(orientation->pitch) * sin(orientation->roll) + \
+                                      mag_event.magnetic.z * sin(orientation->pitch) * cos(orientation->roll));
 
 
   // Convert angular data to degree 

Adafruit_Simple_AHRS.h

@@ -9,7 +9,7 @@ class Adafruit_Simple_AHRS
 {
 public:
   Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafruit_Sensor* magnetometer);
-  Adafruit_Simple_AHRS(const Adafruit_Sensor_Set& sensors);
+  Adafruit_Simple_AHRS(Adafruit_Sensor_Set& sensors);
   bool getOrientation(sensors_vec_t* orientation);
 
 private:

examples/ahrs_10dof/ahrs_10dof.ino

@@ -0,0 +1,62 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_L3GD20_U.h>
+#include <Adafruit_LSM303_U.h>
+#include <Adafruit_BMP085_U.h>
+#include <Adafruit_10DOF.h>
+#include <Adafruit_Simple_AHRS.h>
+
+Adafruit_10DOF board;
+Adafruit_Simple_AHRS ahrs(board);
+
+// Update this with the correct SLP for accurate altitude measurements
+float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial.println(F("Adafruit 10 DOF Pitch/Roll/Heading Example")); Serial.println("");
+  
+  // Initialize the board.
+  board.begin();
+}
+
+void loop(void)
+{
+  sensors_vec_t   orientation;
+
+  // Use the simple AHRS function to get the current orientation.
+  if (ahrs.getOrientation(&orientation))
+  {
+    /* 'orientation' should have valid .roll and .pitch fields */
+    Serial.print(F("Orientation: "));
+    Serial.print(orientation.roll);
+    Serial.print(F(" "));
+    Serial.print(orientation.pitch);
+    Serial.print(F(" "));
+    Serial.print(orientation.heading);
+    Serial.println(F(""));
+  }
+
+  // Calculate the altitude using the barometric pressure sensor
+  sensors_event_t bmp_event;
+  board.getBMP().getEvent(&bmp_event);
+  if (bmp_event.pressure)
+  {
+    /* Get ambient temperature in C */
+    float temperature;
+    board.getBMP().getTemperature(&temperature);
+    /* Convert atmospheric pressure, SLP and temp to altitude */
+    Serial.print(F("Alt: "));
+    Serial.print(board.getBMP().pressureToAltitude(seaLevelPressure,
+                                        bmp_event.pressure,
+                                        temperature)); 
+    Serial.println(F(""));
+    /* Display the temperature */
+    Serial.print(F("Temp: "));
+    Serial.print(temperature);
+    Serial.println(F(""));
+  }
+  
+  delay(100);
+}

examples/ahrs_9dof/ahrs_9dof.ino

@@ -0,0 +1,42 @@
+#include <Wire.h>
+#include <Adafruit_Sensor.h>
+#include <Adafruit_L3GD20_U.h>
+#include <Adafruit_LSM303_U.h>
+#include <Adafruit_BMP085_U.h>
+#include <Adafruit_9DOF.h>
+#include <Adafruit_Simple_AHRS.h>
+
+Adafruit_9DOF board;
+Adafruit_Simple_AHRS ahrs(board);
+
+// Update this with the correct SLP for accurate altitude measurements
+float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial.println(F("Adafruit 10 DOF Pitch/Roll/Heading Example")); Serial.println("");
+  
+  // Initialize the board.
+  board.begin();
+}
+
+void loop(void)
+{
+  sensors_vec_t   orientation;
+
+  // Use the simple AHRS function to get the current orientation.
+  if (ahrs.getOrientation(&orientation))
+  {
+    /* 'orientation' should have valid .roll and .pitch fields */
+    Serial.print(F("Orientation: "));
+    Serial.print(orientation.roll);
+    Serial.print(F(" "));
+    Serial.print(orientation.pitch);
+    Serial.print(F(" "));
+    Serial.print(orientation.heading);
+    Serial.println(F(""));
+  }
+  
+  delay(100);
+}

processing/bunnyrotate/bunnyrotate.pde

@@ -0,0 +1,165 @@
+import processing.serial.*;
+import java.awt.datatransfer.*;
+import java.awt.Toolkit;
+import processing.opengl.*;
+import saito.objloader.*;
+import g4p_controls.*;
+
+float roll  = 0.0F;
+float pitch = 0.0F;
+float yaw   = 0.0F;
+float temp  = 0.0F;
+float alt   = 0.0F;
+
+OBJModel model;
+
+// Serial port state.
+Serial       port;
+String       buffer = "";
+final String serialConfigFile = "serialconfig.txt";
+boolean      printSerial = false;
+
+// UI controls.
+GPanel    configPanel;
+GDropList serialList;
+GLabel    serialLabel;
+GCheckbox printSerialCheckbox;
+
+void setup()
+{
+  size(400, 500, OPENGL);
+  frameRate(30);
+  model = new OBJModel(this);
+  model.load("bunny.obj");
+  model.scale(20);
+  
+  // Serial port setup.
+  // Grab list of serial ports and choose one that was persisted earlier or default to the first port.
+  int selectedPort = 0;
+  String[] availablePorts = Serial.list();
+  if (availablePorts == null) {
+    println("ERROR: No serial ports available!");
+    exit();
+  }
+  String[] serialConfig = loadStrings(serialConfigFile);
+  if (serialConfig != null && serialConfig.length > 0) {
+    String savedPort = serialConfig[0];
+    // Check if saved port is in available ports.
+    for (int i = 0; i < availablePorts.length; ++i) {
+      if (availablePorts[i].equals(savedPort)) {
+        selectedPort = i;
+      } 
+    }
+  }
+  // Build serial config UI.
+  configPanel = new GPanel(this, 10, 10, width-20, 90, "Configuration (click to hide/show)");
+  serialLabel = new GLabel(this,  0, 20, 80, 25, "Serial port:");
+  configPanel.addControl(serialLabel);
+  serialList = new GDropList(this, 90, 20, 200, 200, 6);
+  serialList.setItems(availablePorts, selectedPort);
+  configPanel.addControl(serialList);
+  printSerialCheckbox = new GCheckbox(this, 5, 50, 200, 20, "Print serial data");
+  printSerialCheckbox.setSelected(printSerial);
+  configPanel.addControl(printSerialCheckbox);
+  // Set serial port.
+  setSerialPort(serialList.getSelectedText());
+}
+ 
+void draw()
+{
+  background(0,0, 0);
+
+  // Set a new co-ordinate space
+  pushMatrix();
+
+  // Simple 3 point lighting for dramatic effect.
+  // Slightly red light in upper right, slightly blue light in upper left, and white light from behind.
+  pointLight(255, 200, 200,  400, 400,  500);
+  pointLight(200, 200, 255, -400, 400,  500);
+  pointLight(255, 255, 255,    0,   0, -500);
+  
+  // Displace objects from 0,0
+  translate(200, 350, 0);
+  
+  // Rotate shapes around the X/Y/Z axis (values in radians, 0..Pi*2)
+  rotateX(radians(roll));
+  rotateZ(radians(pitch));
+  rotateY(radians(yaw));
+
+  pushMatrix();
+  noStroke();
+  model.draw();
+  popMatrix();
+  popMatrix();
+  //print("draw");
+}
+
+void serialEvent(Serial p) 
+{
+  String incoming = p.readString();
+  if (printSerial) {
+    println(incoming);
+  }
+  
+  if ((incoming.length() > 8))
+  {
+    String[] list = split(incoming, " ");
+    if ( (list.length > 0) && (list[0].equals("Orientation:")) ) 
+    {
+      roll  = float(list[1]);
+      pitch = float(list[2]);
+      yaw   = float(list[3]);
+      buffer = incoming;
+    }
+    if ( (list.length > 0) && (list[0].equals("Alt:")) ) 
+    {
+      alt  = float(list[1]);
+      buffer = incoming;
+    }
+    if ( (list.length > 0) && (list[0].equals("Temp:")) ) 
+    {
+      temp  = float(list[1]);
+      buffer = incoming;
+    }
+  }
+}
+
+// Set serial port to desired value.
+void setSerialPort(String portName) {
+  // Close the port if it's currently open.
+  if (port != null) {
+    port.stop();
+  }
+  try {
+    // Open port.
+    port = new Serial(this, portName, 115200);
+    port.bufferUntil('\n');
+    // Persist port in configuration.
+    saveStrings(serialConfigFile, new String[] { portName });
+  }
+  catch (RuntimeException ex) {
+    // Swallow error if port can't be opened, keep port closed.
+    port = null; 
+  }
+}
+
+// UI event handlers
+
+void handlePanelEvents(GPanel panel, GEvent event) {
+  // Panel events, do nothing.
+}
+
+void handleDropListEvents(GDropList list, GEvent event) { 
+  // Drop list events, check if new serial port is selected.
+  if (list == serialList) {
+    setSerialPort(serialList.getSelectedText()); 
+  }
+}
+
+void handleToggleControlEvents(GToggleControl checkbox, GEvent event) { 
+  // Checkbox toggle events, check if print events is toggled.
+  if (checkbox == printSerialCheckbox) {
+    printSerial = printSerialCheckbox.isSelected(); 
+  }
+}
+

processing/bunnyrotate/data/bunny.mtl

@@ -0,0 +1,13 @@
+# 3ds Max Wavefront OBJ Exporter v0.94b - (c)2007 guruware
+# File Created: 04.07.2010 10:41:39
+
+newmtl Body
+Ns 32
+d 1
+Tr 1
+Tf 1 1 1
+illum 2
+Ka 0.0000 0.0000 0.0000
+Kd 0.7412 0.4784 0.4765
+Ks 0.3500 0.3500 0.6500
+