eKatana test code

eKatana.c

#include <Arduino.h>
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"
#include <Adafruit_LSM9DS0.h>
#include <Adafruit_Simple_AHRS.h>

#include "BluefruitConfig.h"

Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);

// Assign a unique base ID for this dOF sensor
Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0(1334);  // Use I2C, ID #1334

// Create simple AHRS algorithm using the LSM9DS0 instance's accelerometer and magnetometer.
Adafruit_Simple_AHRS ahrs(&lsm.getAccel(), &lsm.getMag());

void setup(void) {
  setupBLE();  
  setupLSM();
}

/**
 * Setup LSM DOF sensors
 */
void setupLSM(void) {
  // Initialise the sensor
  if ( !lsm.begin() ) {
    // Ooops, no LSM9DS0 detected ... Check your wiring or I2C ADDR!
    while(1);
  }
  
  // 1.) Set the accelerometer range
  lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_16G);
  
  // 2.) Set the magnetometer sensitivity
  lsm.setupMag(lsm.LSM9DS0_MAGGAIN_12GAUSS);

  // 3.) Setup the gyroscope
  lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_2000DPS);
}

/**
 * Setup Bluetooth LE
 */
void setupBLE(void) {
  if ( !ble.begin(VERBOSE_MODE) ) {
    //error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
  }

  ble.factoryReset();

  /* Disable command echo from Bluefruit */
  ble.echo(false);

  ble.verbose(false);  // debug info is a little annoying after this point!

  delay(2000);
  ble.print("AT+GAPDEVNAME=");
  ble.println("eKatana");
  
  /* Wait for connection */
  while (! ble.isConnected()) {
    delay(500);
  }  

  delay(2000);
  ble.print("AT+BLEUARTTX=");
  ble.println("eKatana Demo, ver 0.2.0                                               "); 
  ble.waitForOK();  

  ble.print("AT+BLEUARTTX=");
  ble.println("                                                                      "); 
  ble.waitForOK();  
  delay(1000);  
}

void loop(void) {
  char buf[256];
  sensors_vec_t orientation;  
  
  if (ahrs.getOrientation(&orientation)) {
    char roll[10];
    char pitch[10];
    char heading[10];
    
    dtostrf(orientation.roll, 3, 2, roll);
    dtostrf(orientation.pitch, 3, 2, pitch);
    dtostrf(orientation.heading, 3, 2, heading);
    sprintf(buf, "RO: %s   PI: %s   HD: %s                                          ", 
      roll,
      pitch,
      heading
     );
     
    ble.print("AT+BLEUARTTX=");
    ble.println(buf); 
    ble.waitForOK();
  }
  
  delay(500);
}