Sunday 20 January 2013

uSD on the Arduino DUE

The SPI connector (no longer ICSP but the same format)
is right in the middle of the board.
The SdFat beta for the DUE posted by William Greiman in December 2012, now in general release makes it possible to control the SD card on the hardware SPI bus of the DUE. That should be way faster than emulating the SPI in software on some other digital pins. Unfortunately, that bus comes out on the SPI connector in the very middle of the board, overlapped by the standard shield footprint, and very few shields make provision for connecting to it. The Adafruit protoshield in the picture has been dremelled out to allow access, which cuts some of the traces, so think first. A six pin chunk has been dremelled off the end of a longer length of double wide female header. Some header pins have been pushed into some of the odd numbered digital pins at the bottom in the photo, primarily for structural support.

Half an Adafruit Permaproto Board fits right over the pins,
after dremelling away some unneeded surface and traces on
the bottom that shouldn't connect to each other.
The header slides on and matches to the top surface of
the protoshield, not the bottom. The protoshield is
mostly empty except for an X-Bee connection header.







I made sure to dremel off the traces on the protoboard that would be covered by the headers, then used the DUE as the template to hold all the pieces while soldering the pins. I soldered wires to the six header pins for connection to the Adafruit uSD breakout board, then used the breakout board as a legend to make sure they wound up in the appropriate holes in the protoboard. After the wiring was complete and tested, I soldered the breakout over top of the wiring. The result tips the breakout up, which actually turns out to be convenient for inserting and removing the card -- purely good luck ;-)



VCC (top right red on the header) goes to 5V on the breakout, GND (bottom right black) to GND,
SCK (middle left black) to CLK, MOSI (middle right yellow) to DI, and MISO (top left green) to DO. 
I wound up connecting the CS pin to digital pin 45 (yellow)
because it was handy. That was before I found out that
"the extended API can use pins 4, 10, and 52 for CS".

There's some freedom of choice on picking the CS pin connection, so I picked digital pin 45 because I already had a header pin stuck in that socket, and it had worked when I tested it with jumper wires. I found out after doing that soldering that there is an extended API for SPI on the DUE that can (only?) make use of digital pins 4, 10, and 52 for CS, but that doesn't seem to be interfering with the benchmark programs running sdFat. When I plugged in this configuration and ran the bench sketch it produced:

Free RAM: 62747
Type is FAT32
File size 20MB
Buffer size 32768 bytes
Starting write test.  Please wait up to a minute
Write 2074.35 KB/sec
Maximum latency: 452410 usec, Minimum Latency: 7830 usec, Avg Latency: 15788 usec

Starting read test.  Please wait up to a minute
Read 4381.52 KB/sec
Maximum latency: 12166 usec, Minimum Latency: 7413 usec, Avg Latency: 7476 usec

That was way better than the results I got from my first run with a small file and a small buffer size, although it ate up a third of the available memory.

Free RAM: 95415
Type is FAT32
File size 5MB
Buffer size 100 bytes
Starting write test.  Please wait up to a minute
Write 85.27 KB/sec
Maximum latency: 159470 usec, Minimum Latency: 15 usec, Avg Latency: 1171 usec

Starting read test.  Please wait up to a minute
Read 1208.31 KB/sec
Maximum latency: 865 usec, Minimum Latency: 15 usec, Avg Latency: 81 usec


The card also conveniently clears
the motor driver shield and wiring
that goes next in the stack.
Having the SD onboard the DUE will let me record all the performance parameters while running linear actuators from the motor shield and measuring the forces they generate on the load cells. This has the advantage that I won't need to have the SD running on the UNO with the TFT/TS combo that provides the user interface at the other end of the X-Bee link.






Thursday 17 January 2013

Programming the 2.8" TFTLCD / Touch Screen / uSD Shield

The Adafruit 2.8" TFT Touch Shield for Arduino provides some challenges in geometry, since the touchscreen coordinates don't match the TFT coordinates. The scale is different and the rotation frame is 180 degrees off. It also includes a microSD (uSD) slot hidden underneath, wired to the SPI pins 11,12,13 with the card select wired to pin 5. The TFT seems to use pins 4, 5, 6, 7, 8, 9, 10, 11, 13, A0, A1, A2, and A3. The touchscreen uses pins 6, 7, A1, and A2. Getting them to play nice while sharing all those pins looks like a challenge, but the sample software works fine for me right out of the box on an UNO.

Reading the touch screen and writing to the TFT are both slow processes with the supplied software libraries.

These are the coordinate systems for the shield with tft.setRotation(0), which is the default. It may make a little more sense if you rotate the entire picture 180 degrees so that the USB end of the Arduino is the top and the TFT coords will run left to right and top to bottom.

The square buttons can go anywhere on the screen,
with 16 bit colors and text overlays in color of your choice.  
The test application helps find the limits of the touch
screen for calibrating the library call to get the mapping right.
It would be nice to have some way of scrolling a section of the screen, e.g. for a scrolling chart recorder, but for now I can make do drawing lines and boxes. I'm working on a library that defines a single TFT/TS class and does some other cool stuff to draw buttons, graphs and otherwise make using the combination a little more seamless. Now posted on github, including an example program that follows the steps below to get all three features working together.

Getting the SD Card to work alongside the TFT and Touch Screen

Adafruit provides an example for copying bitmap files to the screen from a uSD card, that doesn't use the touch screen. They also provide an example for a really simple paint program that uses the TFT and the touch screen, but not the uSD. I wanted to be able to use the TFT and the touch screen and record data from the program to the uSD for a log of what happened. I found that really hard to accomplish because I wasn't paying enough attention to the hardware conflicts. In addition to getting the pin directions right, the status of the SPI control register has to be preserved separately for TFT operations and uSD operations. The sketch below is a copy of the Adafruit paint example that does that to record a log file to the uSD. I hope the insight saves you some time.

Testing also shows that you can open multiple files for output simultaneously by following the obvious steps in parallel. (1 SD object, two separate File objects.) Watch out because these three libraries together take a lot of code space, putting you up against the 32K barrier on the UNO.

// Paint example specifically for the TFTLCD Arduino shield.
// If using the breakout board, use the tftpaint.pde sketch instead!
//************************************************************
// Modified by RWS to record data to a log file on the SD card
// Crucial stuff about saving the SPI control register state
// marked with asterisks like these ones....
//************************************************************

#include <Adafruit_GFX.h>    // Core graphics library
#include <Adafruit_TFTLCD.h> // Hardware-specific library
#include <TouchScreen.h>
#include <SD.h>

#define SD_CS 5 // Card select for shield use

// These are the pins for the shield!
#define YP A1  // must be an analog pin, use "An" notation!
#define XM A2  // must be an analog pin, use "An" notation!
#define YM 7   // can be a digital pin
#define XP 6   // can be a digital pin

#define TS_MINX 150
#define TS_MINY 120
#define TS_MAXX 920
#define TS_MAXY 940

// For better pressure precision, we need to know the resistance
// between X+ and X- Use any multimeter to read it
// For the one we're using, its 300 ohms across the X plate
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);

//#define LCD_CS A3          // these defines don't seem to be needed in the sketch
//#define LCD_CD A2          // although they are a hint that the TFTLCD may use those pins
//#define LCD_WR A1
//#define LCD_RD A0

// Assign human-readable names to some common 16-bit color values:
#define BLACK   0x0000
#define BLUE    0x001F
#define RED     0xF800
#define GREEN   0x07E0
#define CYAN    0x07FF
#define MAGENTA 0xF81F
#define YELLOW  0xFFE0
#define WHITE   0xFFFF


Adafruit_TFTLCD tft;
uint8_t         spi_save_tft,spi_save_sd;

#define BOXSIZE   40
#define PENRADIUS  4
int oldcolor, currentcolor;

File logfile;

void setup(void) {
  Serial.begin(9600);
  progmemPrintln(PSTR("Paint!"));

  tft.reset();

  uint16_t identifier = tft.readID();

  if(identifier == 0x9325) {
    progmemPrintln(PSTR("Found ILI9325 LCD driver"));
  } else if(identifier == 0x9328) {
    progmemPrintln(PSTR("Found ILI9328 LCD driver"));
  } else if(identifier == 0x7575) {
    progmemPrintln(PSTR("Found HX8347G LCD driver"));
  } else {
    progmemPrint(PSTR("Unknown LCD driver chip: "));
    Serial.println(identifier, HEX);
    return;
  }

  tft.begin(identifier);

  tft.fillScreen(BLACK);

  tft.fillRect(0, 0, BOXSIZE, BOXSIZE, RED);
  tft.fillRect(BOXSIZE, 0, BOXSIZE, BOXSIZE, YELLOW);
  tft.fillRect(BOXSIZE*2, 0, BOXSIZE, BOXSIZE, GREEN);
  tft.fillRect(BOXSIZE*3, 0, BOXSIZE, BOXSIZE, CYAN);
  tft.fillRect(BOXSIZE*4, 0, BOXSIZE, BOXSIZE, BLUE);
  tft.fillRect(BOXSIZE*5, 0, BOXSIZE, BOXSIZE, MAGENTA);
  // tft.fillRect(BOXSIZE*6, 0, BOXSIZE, BOXSIZE, WHITE);
  tft.drawRect(0, 0, BOXSIZE, BOXSIZE, WHITE);
  currentcolor = RED;
                                //*******************************************************************
  spi_save_tft = SPCR;          // save the state of the SPI control register for TFT before SD stuff
                                //*******************************************************************
  progmemPrint(PSTR("Initializing SD card...")); //*****************************************************
  if (!SD.begin(SD_CS)) {                        // SD code from the Adafruit data logger shield example
    progmemPrintln(PSTR("failed!"));             //*****************************************************
    return;
  }
  progmemPrintln(PSTR("OK!"));
  // create a new file
  char filename[] = "TSTLOG00.TXT";
  for (uint8_t i = 0; i < 100; i++) {
    filename[6] = i/10 + '0';
    filename[7] = i%10 + '0';
    if (! SD.exists(filename)) {
      // only open a new file if it doesn't exist
      logfile = SD.open(filename, FILE_WRITE); 
      break;  // leave the loop!
    }
  }
  if (! logfile) {
    Serial.println("bad file");
  }
  else{
    Serial.print("Log to: ");
    Serial.println(filename);
  }
                               //**********************************************************
  spi_save_sd = SPCR;          // save the state of the SPI control register after SD stuff
                               //**********************************************************

  pinMode(13, OUTPUT);      // maybe this pin 13 stuff has something to do with the SD card on SPI?
                            // or maybe it's just about blinking
}

#define MINPRESSURE 10
#define MAXPRESSURE 1000

void loop()
{
  digitalWrite(13, HIGH);
  Point p = ts.getPoint();
  digitalWrite(13, LOW);

  // if sharing pins, you'll need to fix the directions of the touchscreen pins
  // the touchscreen swaps them back and forth between input and output multiple times
  pinMode(XP, OUTPUT);
  pinMode(XM, OUTPUT);
  pinMode(YP, OUTPUT);
  pinMode(YM, OUTPUT);
  
  // we have some minimum pressure we consider 'valid'
  // pressure of 0 means no pressing!

  if (p.z > MINPRESSURE && p.z < MAXPRESSURE) {

    /*
    Serial.print("X = "); Serial.print(p.x);
    Serial.print("\tY = "); Serial.print(p.y);
    Serial.print("\tPressure = "); Serial.println(p.z);
    */
                             //********************************************************
    SPCR = spi_save_sd;      // reset to the SD saved state of the SPI control register
                             //********************************************************
    logfile.print("X = "); logfile.print(p.x);
    logfile.print("\tY = "); logfile.print(p.y);
    logfile.print("\tPressure = "); logfile.println(p.z);
    logfile.flush();
                              //************************************************************
    SPCR = spi_save_tft;      // reset to the non-SD saved state of the SPI control register
                              //************************************************************
    if (p.y < (TS_MINY-5)) {
      Serial.println("erase");
      // press the bottom of the screen to erase 
      tft.fillRect(0, BOXSIZE, tft.width(), tft.height()-BOXSIZE, BLACK);
    }
    // scale from 0->1023 to tft.width
    p.x = map(p.x, TS_MINX, TS_MAXX, tft.width(), 0);
    p.y = map(p.y, TS_MINY, TS_MAXY, tft.height(), 0);
    
    /*
    Serial.print("("); Serial.print(p.x);
    Serial.print(", "); Serial.print(p.y);
    Serial.println(")");
    */
    if (p.y < BOXSIZE) {
       oldcolor = currentcolor;

       if (p.x < BOXSIZE) { 
         currentcolor = RED; 
         tft.drawRect(0, 0, BOXSIZE, BOXSIZE, WHITE);
       } else if (p.x < BOXSIZE*2) {
         currentcolor = YELLOW;
         tft.drawRect(BOXSIZE, 0, BOXSIZE, BOXSIZE, WHITE);
       } else if (p.x < BOXSIZE*3) {
         currentcolor = GREEN;
         tft.drawRect(BOXSIZE*2, 0, BOXSIZE, BOXSIZE, WHITE);
       } else if (p.x < BOXSIZE*4) {
         currentcolor = CYAN;
         tft.drawRect(BOXSIZE*3, 0, BOXSIZE, BOXSIZE, WHITE);
       } else if (p.x < BOXSIZE*5) {
         currentcolor = BLUE;
         tft.drawRect(BOXSIZE*4, 0, BOXSIZE, BOXSIZE, WHITE);
       } else if (p.x < BOXSIZE*6) {
         currentcolor = MAGENTA;
         tft.drawRect(BOXSIZE*5, 0, BOXSIZE, BOXSIZE, WHITE);
       }

       if (oldcolor != currentcolor) {
          if (oldcolor == RED) tft.fillRect(0, 0, BOXSIZE, BOXSIZE, RED);
          if (oldcolor == YELLOW) tft.fillRect(BOXSIZE, 0, BOXSIZE, BOXSIZE, YELLOW);
          if (oldcolor == GREEN) tft.fillRect(BOXSIZE*2, 0, BOXSIZE, BOXSIZE, GREEN);
          if (oldcolor == CYAN) tft.fillRect(BOXSIZE*3, 0, BOXSIZE, BOXSIZE, CYAN);
          if (oldcolor == BLUE) tft.fillRect(BOXSIZE*4, 0, BOXSIZE, BOXSIZE, BLUE);
          if (oldcolor == MAGENTA) tft.fillRect(BOXSIZE*5, 0, BOXSIZE, BOXSIZE, MAGENTA);
       }
    }
    if (((p.y-PENRADIUS) > BOXSIZE) && ((p.y+PENRADIUS) < tft.height())) {
      tft.fillCircle(p.x, p.y, PENRADIUS, currentcolor);
    }
  }
}

// Copy string from flash to serial port
// Source string MUST be inside a PSTR() declaration!
void progmemPrint(const char *str) {
  char c;
  while(c = pgm_read_byte(str++)) Serial.print(c);
}

// Same as above, with trailing newline
void progmemPrintln(const char *str) {
  progmemPrint(str);
  Serial.println();
}

Tuesday 8 January 2013

Amplifying Load Cells

Inside a small kitchen scale
A while ago I pulled pieces out of small electronic scales and found some full bridge load cells along with hidden circuitry. The circuits were applying a time varying excitation signal to the bridge, but without an oscilloscope, I couldn't see what it was. It is possible to operate a bridge in a simple DC circuit, although there can be issues with noise.

Resistive bridge circuits detect small changes in resistance by producing really small changes in output voltage, so the difference between the two measurement points on the bridge will be millivolts for an excitation of multiple volts. Getting this tiny, differential voltage up to something readable requires an instrumentation amplifier like the AD8221. Unfortunately, I could only find the chip in a surface mount package, less than a quarter inch on a side. This challenged my soldering skills getting it onto a DIP carrier so I could work with it.

The 8221 takes a really small difference in voltage between pins 1 and 4 and produces a larger voltage output on pin 7. If there is no differential voltage, the output will be equal to the reference voltage applied to pin 6. If there is a differential, the output will be above or below the reference voltage by the differential voltage, multiplied by the amplifier gain. The gain is determined by the resistance you attach between pins 2 and 3.

Tiny package, huge gain!
G = 1 + ( 49400 / R23)

Setting R23 = 124 ohms gives a gain of 399, so a 1.5 mV differential should give

3.0 + 399 * 0.0015 = 3.60 V

if the reference voltage applied to pin 6 is 3V. The reference voltage has to be somewhere in the middle between the supply (pin 8) and ground voltages (pin 5) and the expected output must be as well. The output should actually drive a small current through some load (like a 10K resistor), and some small capacitors will smooth the signals in and out. The direction the voltage swings about the reference voltage will depend on which direction you push the load cell and which side of the bridge you hooked to pin 1 vs pin 4, so swap until it increases in the right direction.

Hooked it up like this and it worked as advertised. As a mechanical engineer that always surprises me!

The response time of the circuit will be fast, but not infinitesimal. It will depend on the gain and the capacitors you select, and maybe which version of 8221. It will probably be way, way faster than the mechanical system you're measuring, but you will need to wait for it to settle if you are switching the excitation voltage rapidly, rather than sticking with this basic DC configuration.

Actuators
For my latest project, I want to measure the loads applied by a couple of small linear actuators. I've mounted them so their bases push on 20 kg load cells, and I've hooked up the load cells to AD8221 amplifiers, with nominal 82 ohm gain resistors for a nominal gain of 603. Vref is derived by dividing the supply voltage across a pair of 2K precision resistors. The output load resistors are also 2K each. For now, there are no filter capacitors on Vref or the outputs.

The circuit on the breadboard provides two channels of amplification, with separate inputs, but common connections for supply, ground and Vref. The excitation voltage for the bridges is common to both bridges, but could be separated from the amplifier power supply. That will allow the bridge excitation to be switched to eliminate low frequency noise, or increased to increase the magnitude of the outputs. Even a small push on the actuators generates an easily measurable voltage on the outputs.

When I first hooked them up the green wires went to pin 1 on the amps and the output voltage decreased with increasing load. I switched them around, and now the voltage increase goes with the applied load. If I knew which wires ran to which strain gauges under the white goop on the load cells, then I could predict the sense of the response, but it's easier to measure it!

The load cells are mounted with M5 socket head cap
screws on a block of hard red cherry wood. The bottom
screw doesn't contact the support, so the load cells are
free to flex under applied loads.  
Two realizations of the circuit are smaller
than the diagram describing them. There are
currently no capacitors on the outputs. Common
power comes from the 2.2 mm jack top left. 

Thursday 3 January 2013

Inertial Orientation Sensing

Phones all have gyros and compasses and accelerometers to provide orientation sensing for fairly pedestrian applications like mapping and gaming. There's lots of reasons to want to sense position and orientation for more exacting applications like UAVs, sailboat autopilots, and Human Mobility Research. I'm going to see what I can do with two sensors that were on special before Christmas at Pololu. The LSM303DLHC 3D Compass and Accelerometer Carrier with Voltage Regulator and L3GD20 3-Axis Gyro Carrier with Voltage Regulator are tiny, friendly to 3 and 5 volt logic levels and communicate over I2C to keep the pin requirements reasonable.

Pololu provides basic libraries for reading them with the Arduino. The results come back as integers for each of the 9 components, and they need to be translated into appropriate units before they can be used in further calculations.

The gyro chip starts on the 250 degree per second scale, returning +/-32768 for about 250 degrees per second and close to zero when sitting still.

The magnetometer starts off on the 1.3 Gauss scale, with 1100 LSB / Gauss on the X and Y axes and 980 LSB / Gauss on the Z axis, according to the datasheet. The Pololu library includes a calibration program to read the local maximum values for each axis to do a calibration, which presumably goes into its compass heading calculation.

The accelerometer reads integers in about cm/s^2, or thousandths of a g, so one gravity is about 981 or 1000 and the reading will be close to zero for the horizontal axes.

None of these sensors are exact to their ranges, so they will need to be calibrated for physical units.

It takes about 3 ms to read both sensors on an Arduino UNO, so the upper limit is probably about 300 Hz for an update rate. (Code below.)

Calibrating the Sensors

The sensors have to be assumed linear, but we can determine the zero points and scale factors by observation. The maxima should correspond to the limits and the zero values should be between them.

I'm near sea level in Kingston, Ontario, Canada so down should be 1 standard gravity. The #defined accelerometer calibration constants in the code below were determined by running the program for the particular sensors being used and finding the average reading in the six cardinal poses and assuming they were +/-g with 0 in the middle.

X from -1058 to 1044, 0 midpoint = -7, 1 g = 1051
Y from -1008 to 1012, 0 midpoint =  2, 1 g = 1010
Z from -979 to 1045, 0 midpoint = 33, 1 g = 1012

From wikipedia the local field strength is about 0.55 Gauss. The magnetometer maximum readings should correspond to +/- 0.55 Gauss, achieved when each of the axes is pointing north and down or south and up.

X from -564 to 633,  0 midpoint = 34.5, 1 Gauss = 1088
Y from -877 to 430,  0 midpoint = -223.5, 1 Gauss = 1188
Z from -517 to 593,  0 midpoint = 38, 1 Gauss = 1009

A full rotation about an axis should integrate to 360 degrees or 2 pi radians. The code below measures and outputs the raw values needed to complete the calibrations. The offset constants for the gyro were taken from stationary averages. The scale factors were taken from an integration of a complete 360 degree rotation about each axis.

The calibrations were all based on the assumption that the table top was flat and square and didn't move. They can all be better if taken under more controlled conditions. 


Compass redone:
X from -618 to 623,  0 midpoint = 2.5, 1 Gauss = 1128
Y from -896 to 511,  0 midpoint = -192.5, 1 Gauss = 1279
Z from -653 to 620,  0 midpoint = -33, 1 Gauss = 1157

Update 2013/01/08: Calibration is critical to get a good compass bearing even standing still. Once that is accomplished, gyro calibration needs to be accurate to avoid dynamic errors, and you really need to be sure the algorithm is getting updated often enough not to lose accuracy in the integration. The gyro integration assumes linearity. That works really well for short time steps, but fails if the time step gets longer due to e.g. attending to output and controlling other parts of the system. Ideally, it would run on a timer interrupt, but other software systems (PWM, etc.) are already overusing the 3 timers available on the UNO, so maybe an externally clocked interrupt would be best. For the moment I can get by with a static estimate....

Madgwick's Algorithm

Sebastian Madgwick's Attitude and Heading Reference System (AHRS) algorithm is written for units of g-force, radians / second, and Gauss, so the binary inputs need to be converted.

The C code supplied by Madgwick includes a routine for a fast inverse square root that relies on certain hardware specific elements of IEEE floating point and integer representations. That led me into trouble that Tobias Simon had found previously. To avoid potential platform dependent weirdness, I changed the Madgwick code to simply use the computationally expensive sqrt() function. It adds much less than 1 ms / update on the UNO, so I'm not going to get excited.

The sample frequency is also hard-coded in as a #define. It is used only to specify the integration time scale for advancing the estimate of the orientation, so this value could be taken directly from the system clock time elapsed since the last iteration. This code change does that (provided lastTime = micros() is updated at the end of each estimate):


//RWS use actual elapsed time instead of fixed sample frequency
#define sampleFreq    (1000000.0f / (micros() - lastTime))
unsigned long lastTime=0;

//#define sampleFreq 512.0f // sample frequency in Hz

The gain, beta, is set to 0.1 in the supplied code, but Madgwick's report suggests using a much larger value (2.5) initially to speed convergence.  The whole package winds up being almost two big to fit into the 32K available for code on an UNO, but it's running and spitting out values that make reasonable sense.

Code for Taking the Raw and Calibration Readings

This code is for the sketch Pololu_IMU_Bias. 

#include <Wire.h>
#include <math.h>
#include <MadgwickAHRS.h>
#include <LSM303.h>
#include <L3G.h>



#define AX0 -7      // accelerometer offset
#define AX1 1051    // divide by scale factor of about 100 to m/s^2
#define AY0 2       // LSM303DLHC returns integer values in about cm/s^2 or mg
#define AY1 1010    // divide by scale factor of about 1000 to g-force
#define AZ0 33
#define AZ1 1012

#define MX0 34.5    // magnetometer offset
#define MX1 1088    // divide by scale factor to Gauss
#define MY0 -223.5  // LSM303DLHC returns int values at 1100 LSB/Gauss on X and Y
#define MY1 1188
#define MZ0 38
#define MZ1 1009    // 980 LSB/Gauss on Z for default 1.3 Gauss scale

#define GX0 -109    // gyro offset from bias calibration
#define GX1 5919    // divide by 32768 / 250 deg/s = 131 scale factor to deg/s
#define GY0 0       // default for the L3DG20 is 250 deg/s
#define GY1 6411    // divide by 7510 scale factor to radians/s
#define GZ0 -242    // actual accumulation gives X 103.3 Y 111.9 Z 109.2
#define GZ1 6257    // translated for radians/s  X 5919  Y 6411  Z 6257




#define sp(x,y,z) Serial.print(x);Serial.print(" ");Serial.print(y);Serial.print(" ");Serial.print(z);

L3G gyro;
LSM303 compass;

char scratchy[200];

void setup() {
  Serial.begin(57600);
  Wire.begin();

  compass.init();
  compass.enableDefault();
  // Calibration values. Use the Calibrate example program to get the values for
  // your compass.
  compass.m_min.x = -639; compass.m_min.y = -834; compass.m_min.z = -597;
  compass.m_max.x = +536; compass.m_max.y = +414; compass.m_max.z = +599;

  
  if (!gyro.init())
  {
    Serial.println("Failed to autodetect gyro type!");
    while (1);
  }

  gyro.enableDefault();

}

void loop() {
  int aax,aay,aaz,amx,amy,amz,agx,agy,agz;
  int iax,iay,iaz,imx,imy,imz,igx,igy,igz;
  static long int n = 0,sax = 0,say = 0,saz = 0,smx = 0,smy = 0,smz = 0,sgx = 0,sgy = 0,sgz = 0;
  static double stgx = 0.0,stgy = 0.0,stgz = 0.0,ft = 0.0;
  static long lastTime = micros(),startTime,endTime;
  
  startTime = micros();
  compass.read();
  gyro.read();
  iax = compass.a.x; iay = compass.a.y; iaz = compass.a.z;
  imx = compass.m.x; imy = compass.m.y, imz = compass.m.z;
  igx = gyro.g.x; igy = gyro.g.y; igz = gyro.g.z;
    
  // Accumulate and calculate running averages on absolute inputs
  sax += (int) compass.a.x;
  say += (int) compass.a.y;
  saz += (int) compass.a.z;
  smx += (int) compass.m.x;
  smy += (int) compass.m.y;
  smz += (int) compass.m.z;
  sgx += (int) gyro.g.x;
  sgy += (int) gyro.g.y;
  sgz += (int) gyro.g.z;
  n++;
  aax = sax / n;
  aay = say / n;
  aaz = saz / n;
  amx = smx / n;
  amy = smy / n;
  amz = smz / n;
  agx = sgx / n;
  agy = sgy / n;
  agz = sgz / n;

  endTime = micros();
  ft = (double) (endTime-lastTime) / 1000000.0;    // floating point time in seconds
  lastTime = micros();

  // integrate the gyro performance in LSB*seconds
  stgx += ((double) gyro.g.x - GX0) * ft;
  stgy += ((double) gyro.g.y - GY0) * ft;
  stgz += ((double) gyro.g.z - GZ0) * ft;
    
    
  if(millis() % 1000 < 10){
    if(millis() %30000 < 10) stgx = stgy = stgz = 0.0;   // zero out the gyro drift now and then
    
    // Time in micro seconds
    sprintf(scratchy,"%6i us  ",endTime-startTime);
    Serial.print(scratchy);
    
    // Raw readings
    sprintf(scratchy," INST Acc X:%5i Y:%5i Z:%5i   Mag X:%5i Y:%5i Z:%5i   Gyro X:%5i Y:%5i Z:%5i  ",iax,iay,iaz,imx,imy,imz,igx,igy,igz);  
    Serial.print(scratchy);
    
    // Average raw readings for calibration constants 
    sprintf(scratchy,"    AVG Acc X:%5i Y:%5i Z:%5i   Mag X:%5i Y:%5i Z:%5i   Gyro X:%5i Y:%5i Z:%5i  ",aax,aay,aaz,amx,amy,amz,agx,agy,agz);
    Serial.print(scratchy);  

    // Accumulated zeroed gyro readings 
    Serial.print("    ACCUM  Gyro XYZ:  ");
    sp(stgx,stgy,stgz);
  
    Serial.println();  
  }
}