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();  
  }
}

Getting X-Bee going on my Mac / Arduino

I got a couple of X-Bees and adapters from Adafruit. I sharpied one of the Xs black so I could tell the difference between them. I got some hints from Ashley Hughes and from the Adafruit tutorial. Both valuable!

Note the black X, and how the I2C pins line up in this orientation. Power is coming in the back

I installed Coolterm from http://freeware.the-meiers.org/ so I could run a serial terminal outside the Arduino IDE.   

I installed the FTDI drivers from http://www.ftdichip.com/Drivers/VCP.htm so that the FTDI cable would interface with my USB and behave like a serial port.

I started coolterm, picked the right port under options, connected and could get recognition of +++ [No Enter] with an OK. I started with a series of queries and responses on the black X-Bee. (With the X blacked out, same for the one with the B blacked out)

Things I Type [normally followed by Enter] that don't show up on the terminal
        Things The black X-Bee Sends Back

+++[No Enter]
        OK
AT
        OK       So the X-Bee is responding, but it will time out and need a +++ again soon
ATID
        3332      is what the PANID was set to before (out of the box default)
ATID2711
        OK        Sets PANID to 2711, could be any 4 digits, but must be the same for all units on network
ATMY
        0            This X-Bee is unit 0
ATDL
        0            The destination X-Bee is unit 0
ATDL1
        OK        Set the destination X-Bee to unit 1
ATBD
        3           The baud rate is set to 9600 and I'll leave it there
ATWR
        OK       Writes all the changes to permanent, otherwise they would disappear at power off

Disconnected, then unplugged USB and changed over to the white X-Bee. Did all the same things, except ATMY1 and ATDL0 to reverse the roles.

Then I connected the black X-Bee RX and TX to RX and TX on an Arduino (turns out they should be TX-RX, RX-TX). Something was happening, but not the right things. http://www.ladyada.net/make/xbee/arduino.html suggests the programming communications won't work to upload to an UNO (just with a Duemilanove bootloader), or that the baud rate should be higher (ATBD6 - 57600 for a newer Arduino with a '328). One problem at a time...

void setup() {              
  Serial.begin(9600);
  delay(1000);
  Serial.println("Hello World! Now to Echo");
}
void loop() {
    if(Serial.available()){
      char c = Serial.read();
      Serial.print("Another new character: ");
      delay(1000);
      Serial.println(c);
    }
}

The code above will echo characters back 1 second after they're sent. Loaded and tested over USB, then disconnected and powered the Arduino by batteries, connected coolterm to the white X-Bee on the FTDI and it didn't work. Switched the connections so that black X-Bee RX went to Arduino TX and black X-Bee TX went to Arduino RX, and things started working.

So now that I have wireless serial at 9600 baud, do I want to get wireless programming working, or just work on wireless links between Arduinos? Start with upping the speed on both to ATBD6 - 57600. And then remember to up the sketch baud rate too... and presto, it works. Up to ATBD7 - 115200 and that works too, but gets flaky, so I'll slow it down to ATBD6 - 57600. (Yes, there's a reason why this might read like a lab notebook ;-) )


Wireless programming seems like a lot more effort with the extra pins to worry about, so I'll tackle that when I need it.

Modifying a Pololu Shield for the Due

The Arduino Due is a 3 volt board all through, except for providing a 5 volt line on the same pin as the Uno. The Pololu Dual VNH5019 Motor Driver Shield for Arduino is compatible with either 3 volt or 5 volt logic, however it takes its VDD from the 5 volt pin on the Arduino headers. To make it safe for the Due I tied the 3 volt pin to VDD on the end of the card (red wire) and cut the trace from the 5 volt pin (lower right near the mounting hole). As a result the logic returns come at 3V rather than 5V, safe for the Due and still works with the Uno.

My first thought (and test) was to jump the 3 volt and 5 volt pins and cut off the 5 volt pin so the board gets all its feed that way. Problem: boards stacked on top of the driver shield may still be expecting 5 volts to drive displays, etc.

I have a feeling there are going to be a bunch of 3/5 volt challenges ahead, but on the plus side, the Due has capabilities we spent about $20K to get in our lab in 1984!

From Pololu's User Manual on 3V systems:

"Logic power at the same level as your controlling device should be supplied to the VDD pin. This will typically be between 2.5 and 5 V, but the VNH5019 motor drivers are guaranteed to treat any logic input voltage over 2.1 V as high. The only purpose of the VDD pin is to power the pull-up resistors on the EN/DIAG lines."

and

"Current sense output. The pin voltage is roughly 140 mV per amp of output current when the CS_DIS pin is low or disconnected. The current sense reading is more accurate at higher currents. (Note that while the CS voltage can potentially exceed 3 V at high currents, the current sense circuit is safe for use with 3V analog inputs. The MCU’s analog input voltage will be clamped to a safe value by its protection diode, and only a few hundred microamps at most will flow through that diode.)"

Too much on one board?

5 breakout boards very tightly packed 

I jammed a bunch of breakout boards all onto the same screwshield and managed to trash the BMP085 along the way, from either heat or static. The boards overlap to make everything fit, and the necessary wiring also made the clearances tight.

The end product has a 3-axis accelerometer, feeding 3 of a total of 8 channels of 12 bit ADC, plus one channel of 12 bit DAC. It would also have pressure and temperature if I hadn't fried the board along the way.

The DAC output, as well as the three accelerometer readouts are available on the WXYZ screw terminals.

Cuts down on the wiring mess

TFTLCD 2.8 on top of it all

The ADC are configured for I2C 0x48 and 0x49 and the DAC on 0x62. The green boxes are spring loaded quick release sockets for ground and 5 of the ADC channels. They're a little tall, but it's still possible to fit a display on top to show how the orientation of the board is changing with time.



Of course all of this may be entirely redundant as my long awaited Arduino Due walked in the door late this afternoon, with 12 ADC and 2 DAC and it will run AnalogRead() at 40us / call and 12 bits.

Using a Teensy 3.0 with Adafruit bits

I2C update: Apparently there are issues with the internal pull-up resistors and the Teensy 3.0 requires external resistors connecting both the SDA and SCL lines to logic high. I added 2K pull-ups and things went from flakey to functional.

I got a Teensy 3.0 because it seemed like a good idea, especially with the high resolution ADC. Besides, it's teensy, and that's cool, always cool!

Fired it up with the iMac, downloaded the beta 6 Arduino IDE, and things ran right away.

Attached the 2x16 LCD shield on I2C and the example ran.

Attached the MCP4725 DAC breakout on I2C and the example wouldn't compile because TWBR is not defined. It seems to have something to do with the I2C clock speed. It was only mentioned because the code wanted to change it, so I commented out the change lines in the library cpp and the triangle wave code ran fine.


void Adafruit_MCP4725::setVoltage( uint16_t output, bool writeEEPROM )
{
  //uint8_t twbrback = TWBR;
  //TWBR = 12; // 400 khz
  Wire.beginTransmission(_i2caddr);
  if (writeEEPROM)
  {
    Wire.write(MCP4726_CMD_WRITEDACEEPROM);
  }
  else
  {
    Wire.write(MCP4726_CMD_WRITEDAC);
  }
  Wire.write(output / 16);                   // Upper data bits          (D11.D10.D9.D8.D7.D6.D5.D4)
  Wire.write(output % 16) << 4;              // Lower data bits          (D3.D2.D1.D0.x.x.x.x)
  Wire.endTransmission();
  //TWBR = twbrback;
}

Attached the MPL115A2 Pressure and Temperature breakout on I2C and everything compiles fine, but the answers are way wrong, and constant. I'm not sure why, but the temperature return value is 1023, all bits set, suggesting some sort of handshake problem. The same result shows up on the Uno if you disconnect the breakout board.

It looks like there are different libraries in the internal parts of the Teensy version of the ide, so maybe the variations in the wire.h library are to blame. The adafruit code has tests on the value of ARDUINO >= 100 to determine which wire.xxx calls to use and it may be using the wrong ones. Figuring what's right would require a look at the internal wire library built into the package. (Right click view package contents) I think I'll wait for the beta software to advance a little farther.

Pressure and Temperature over I2C

I picked up the Adafruit MPL115A2 - I2C Barometric Pressure/Temperature Sensor and connected it up using I2C and ran their sample program. First thing I learned is that I really should read the data sheets. The resolution is 0.15 kPa (=1.5 hPa a unit I seldom encounter) and the absolute accuracy is 1 kPa. Thus, values that vary a lot and differ from Environment Canada shouldn't be a surprise. Here's some sample output:

Pressure (kPa): -2929.0664 kPa
Temp (*C): -12035.9 *C
Pressure (kPa): 102.2696 kPa
Temp (*C): 22.6 *C

Note that the large negative numbers are ridiculous, and I don't know where they're coming from. The reasonable numbers seem to be within spec.

So I went to the adafruit forum and looked up the product. Others were having the same problem. That made me think it was software. Following some suggestions, I started messing with the library and found the bug. I fixed it, then posted about the fix, then posted new source code (that actually got accepted) to the code repository, which may make me a geek.

Bottom line: Spend a little more and get the BMP085 Breakout that provides accuracy high enough to detect raising and lowering the board by a metre. Unfortunately it also seem to be fragile enough that I busted one soldering it into a protoboard, so be careful with heat and static.

Driving Bigger Motors

All of the docs on the Adafruit motor shield warn that it's just for low current motors. Motors on 12 V linear actuators need something more like 6 amps at 150 lbs, which is way beyond the motor shield. I'm using this Progressive Automations actuator. The red and black lines are the drive current. The other three are a position potentiometer with blue connected to the wiper.





Enter the Polulu 18v15 motor driver: http://www.pololu.com/catalog/product/755. It comes with the screw terminals and capacitor loose. I added the capacitor a little offset to make sure there's still good airflow to the chips for heat dissipation. Polulu says it's good for 15 amps without a heat sink, so it may be overkill.

I connected it up like this and ignored the fault flags - they never went on.

It works about the way I expected, except that the +5 volt line is an OUTPUT that can provide a few milliamps for additional circuitry. Initial tests set PWMH high for full speed and changed direction with 5v and no connection other than the LED.