February 9th, 2011 | 
Posted by Osman Eralp | 
Comments are closed
Tags: The Chumby is a great brain for a home-built robot: it has an ARM processor running Linux, a 2 GB micro SD card which I upgraded to 16 GB, a touch screen, WiFi, an accelerometer, and many other useful features. I’m using a Chumby as the brain for my latest bot, Blinky. Blinky is a balancing robot. Balancing robots require a gyro and an accelerometer to balance, so I was particularly interested in using the MMA7455 accelerometer in the Chumby.
My first attempts at using the accelerometer were met with frustration: The time between accelerometer updates was too long. Fortunately all of the code for the Chumby is open source. I began digging into the code for the accelerometer daemon. I quickly discovered that the daemon code is not optimal for an application that needs rapid and precise updates:
- The code contained sleep statements that would sometimes sleep for seconds.
- The range and scale of the 8-bit acceleration data was changed from its usual -128 to +127 range to an unusual +1024 to +3071 range.
- The accelerometer filter bandwidth was set to the slower 125 Hz output data rate instead of the faster 250 Hz rate.
For my balancing robot application, I needed to squeeze maximum performance out of the accelerometer. Instead of using the accelerometer daemon, acceld, I decided to access the accelerometer registers directly from my robot program. The code I created incorporates the following changes from the original daemon code:
- Raw data is reported; no scaling or transforming is performed. The range of readings on each axis is -128 to + 127.
- No data averaging is performed; only instantaneous data is reported. The original daemon returned a moving average in addition to the instantaneous data.
- The shared memory functions are removed. My code is intended to be called directly from a robot program and not run as a daemon, so there is no need to use shared memory to exchange data.
- Procedures are added to make it easier to set and clear individual bits of the accelerometer registers.
- The accelerometer filter bandwidth is set to the faster 250 Hz data output rate.
My code consists of three parts: procedures to read and write to the I2C bus, procedures that are wrappers to provide a simple interface to the I2C procedures, and commands that can be used in the main program.
All of the code can be downloaded from the links at the bottom of this post. The code is also reproduced in the following sections.
Note: If you use these procedures, you should kill the accelerometer daemon, acceld. If you are using the Chumby in robot mode as described in this article, you can prevent acceld from starting by commenting out line 70 of /mnt/storage/bin/rcS.robot:
68:
69: # Start the accelerometer daemon.
70: # acceld &
71:
Part One
The first part of the revised accelerometer code is the two general purpose procedures that read and write to any device on the I2C bus. These procedures are essentially the same as the procedures in the original daemon code in acceld_i2c.c. There was a lot of other code in acceld_i2c.c that I stripped out, keeping only these two core procedures.
// file: i2c.c
#include <stdio.h>
#include <stdlib.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <sys/un.h>
/*************************************************************************/
int I2C_Read_nbyte(
int i2c_file, // A file descriptor pointing to the device node
unsigned char address, // The address on the I2C bus
unsigned char reg, // The register on the device
unsigned char *inbuf, // Pointer to a buffer where the data read is put
int length // Number of characters to read
) {
unsigned char outbuf[1]; // A buffer containing data written to the bus
struct i2c_rdwr_ioctl_data packets;
struct i2c_msg messages[2];
outbuf[0] = reg;
// This first message is for data written to the I2C device. It specifies
// the address on the I2C bus and the register to read.
messages[0].addr = address;
messages[0].flags = 0;
messages[0].len = sizeof(outbuf);
messages[0].buf = outbuf;
// This second message is for data read from the I2C device. It specifies
// how many bytes to read and the pointer to the buffer to store the
// data that was read.
messages[1].addr = address;
messages[1].flags = I2C_M_RD;
messages[1].len = length;
messages[1].buf = inbuf;
packets.msgs = messages;
packets.nmsgs = 2;
if(ioctl(i2c_file, I2C_RDWR, &packets) < 0) {
perror("Unable to write/read data");
return 1;
}
return 0;
}
/*************************************************************************/
int I2C_Write_nbyte(
int i2c_file, // A file descriptor pointing to the device node
unsigned char address, // The address on the I2C bus
unsigned char reg, // The register on the device
unsigned char *outbuf, // Pointer to a buffer containg data to write
int length // Number of characters to write
) {
char buf[length+1];
if (ioctl(i2c_file, I2C_SLAVE, address) < 0) {
perror("Unable to assign slave address");
return 1;
}
// Create buffer containing both the register and the data to write
// to the register.
buf[0] = reg;
memcpy(&(buf[1]), outbuf, length);
if(write(i2c_file, buf, length+1) != length+1) {
perror("Unable to write value");
return 1;
}
return 0;
}
Part Two
The second part of the revised accelerometer code includes the wrapper procedures that read and write to the accelerometer.
// file: acc.c
#include <stdio.h>
#include <fcntl.h>
#include <stdlib.h>
#define I2C_FILE_NAME "/dev/i2c-0"
#include "acc.h"
#define NUMREGS 32
static int debug = 0;
static int acc_fd;
/*
The following four short functions read and write accelerometer registers.
They are wrappers around the I2C read and write functions.
*/
/*****************************************************************************/
/* */
/* name: read_acc_reg */
/* This function reads a single register from the accelerometer. */
/* */
/*****************************************************************************/
int read_acc_reg(unsigned char reg) {
unsigned char buf[1];
I2C_Read_nbyte(acc_fd, ACC_I2C_ADDR, reg, buf, 1);
return buf[0];
}
/*****************************************************************************/
/* */
/* name: write_acc_reg */
/* This function writes a single register to the accelerometer. */
/* */
/*****************************************************************************/
int write_acc_reg(unsigned char reg, unsigned char value) {
return I2C_Write_nbyte(acc_fd, ACC_I2C_ADDR, reg, &value, 1);
}
/*****************************************************************************/
/* */
/* name: set_acc_bits */
/* This function provides a way to set a single bit in a register. In the */
/* value passed to this procedure, the bit to be set is 1; all other */
/* bits must be 0. */
/* */
/* example: The following example sets to 1 the LSB of CTRL1. */
/* clear_acc_bits( CTRL1, 0x01 ); */
/* */
/*****************************************************************************/
int set_acc_bits(unsigned char reg, unsigned char value) {
unsigned char temp;
temp = read_acc_reg(reg) | value;
return I2C_Write_nbyte(acc_fd, ACC_I2C_ADDR, reg, &temp, 1);
}
/*****************************************************************************/
/* */
/* name: clear_acc_bits */
/* This function provides a way to clear a single bit in a register. In the */
/* value passed to this procedure, the bit to be cleared is 0; all other */
/* bits must be 1. */
/* */
/* example: The following example clears (ie, sets to 0) the LSB of CTRL1. */
/* clear_acc_bits( CTRL1, 0xfe ); */
/* */
/*****************************************************************************/
int clear_acc_bits(unsigned char reg, unsigned char value) {
unsigned char temp;
temp = read_acc_reg(reg) & value;
return I2C_Write_nbyte(acc_fd, ACC_I2C_ADDR, reg, &temp, 1);
}
/*****************************************************************************/
/* */
/* name: read_acc_xyz */
/* The normal way to read an acc reg is to use read_acc_reg. However, the */
/* three axis regs are read frequently, so this procedure was created as a */
/* shortcut to read all three registers at the same time. */
/* */
/*****************************************************************************/
int read_acc_xyz(signed char xyz[3]) {
signed char buf[3];
// Read all three register values at once. Convert from unsigned to signed.
if (I2C_Read_nbyte(acc_fd, ACC_I2C_ADDR, XOUT8_REG,
(unsigned char *)buf, sizeof(buf))) {
perror("Unable to read acceleromter data");
return 1;
}
xyz[0] = buf[0];
xyz[1] = buf[1];
xyz[2] = buf[2];
return 0;
}
/*****************************************************************************/
/* */
/* name: dump_registers */
/* */
/*****************************************************************************/
int dump_registers() {
unsigned char tmp_regs[NUMREGS];
int reg;
printf("Register dump:\n");
for(reg=0; reg < sizeof(tmp_regs); reg++) {
printf("%02x: 0x%02x\n", reg, read_acc_reg(reg) );
}
return 0;
}
/*****************************************************************************/
/* */
/* name: init_acc */
/* */
/*****************************************************************************/
int init_acc() {
// Open a connection to the I2C userspace control file.
if ((acc_fd = open(I2C_FILE_NAME, O_RDWR)) < 0) {
perror("Unable to open i2c control file");
exit(1);
}
// disable accelerometer, set 2g range
if(write_acc_reg(MCTL_REG, 0x04)) {
fprintf(stderr, "Unable to set accelerometer to standby\n");
return 1;
}
// set digital filter bandwidth to 125 Hz
write_acc_reg(CTL1_REG, 0xb8);
// set drive strength to standard
write_acc_reg(CTL2_REG, 0x00);
// reenable the accelerometer by setting measurement mode.
if(set_acc_bits(MCTL_REG, 0x01)) {
fprintf(stderr, "Unable to enable the accelerometer\n");
return 1;
}
return 0;
}
Part Three
The third part of the revised accelerometer code is the commands that you use in your main program.
// file: main.c
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
/*****************************************************************************/
/* */
/* name: main */
/* */
/*****************************************************************************/
int main(int argc, char **argv) {
int status;
signed char xyz[3]; // This variable stores the 3 accelerometer axis readings
unsigned char debug=0, reg;
if (argc>1) debug=1;
// Initialize the accelerometer.
if(init_acc()) {
printf("Error: Unable to initialize accelerometer.\n");
return 1;
}
// Print out current value of all accelerometer registers.
if (debug) dump_registers();
// Some examples of how to set and clear bits in register 0x10.
/*
reg = 0x10;
printf("%02x: 0x%02x\n", reg, read_acc_reg(reg) );
set_acc_bits( reg, 0xff );
printf("%02x: 0x%02x\n", reg, read_acc_reg(reg) );
clear_acc_bits( reg, 0xf0 );
printf("%02x: 0x%02x\n", reg, read_acc_reg(reg) );
set_acc_bits( reg, 0x05 );
printf("%02x: 0x%02x\n", reg, read_acc_reg(reg) );
clear_acc_bits( reg, 0x9e );
printf("%02x: 0x%02x\n", reg, read_acc_reg(reg) );
set_acc_bits( reg, 0x20 );
printf("%02x: 0x%02x\n", reg, read_acc_reg(reg) );
*/
write_acc_reg( 0x10, 0x00 );
if(debug) printf("Start\n");
// This loop prints the values of the acc x, y, and z axis regs.
while(1) {
status = read_acc_xyz(xyz);
printf("%d\t%d\t%d\n", xyz[0], xyz[1], xyz[2]);
usleep(1000000);
}
return 0;
}
Further Reading
- The datasheet for the accelerometer in the Chumby is located here: http://www.freescale.com/files/sensors/doc/data_sheet/MMA7455L.pdf. The most useful part of the datasheet is Table 9 which shows the registers of the accelerometer.
- This article (http://demandperipherals.com/docs/chumbot_chumby_intro.pdf) is a good introduction to using the Chumby as a robot controller.