Inertia Sensor Network

Hi all,

I am totally new to Arduino as well as this forum, so please forgive me if I've posted this incorrectly.

I'm currently doing a project titled "Inertial Sensor Network for Gait Analysis", and I used the following hardware:

  1. Arduino Pro Mini ATmega328 8Mhz 3.3V
  2. Invensense MPU-6000 6-axis accelerometer/gyroscope
  3. $6 Bluetooth serial

Here's the thing:

  1. To do a good gait analysis, a minimum sampling rate of 100Hz is required.
  2. I used 3 of the acce/gyro, connected to Pro Mini using SPI interface.
  3. With the Pro Mini, I will:
    a) Read from all three sensors, use Serial.print() to push it to the Bluetooth serial com, and process the data with Processing, or....
    b) Do all the calculations on the Pro Mini, use Serial.print() to push it to the Bluetooth serial com, and plot the data with Processing.
  4. MPU-6000 gives acceleration and gyroscope reading. In order to acquire angle/orientation, calculations need to be done.
  5. Assume that I am setting the sensors correctly, the sensors should be giving out sampling rate of 1000Hz.

Questions:

  1. I used micros() to obtain the time delay with each loop, and found out that it is below 100Hz (I am expecting faster since it is a 8Mhz processor, and sensor is giving 1000Hz of readings). I need the Serial.print() cycle for every loop to be at least 100Hz. Is my method correct? Could it be that I am polling my sensors fast enough, but Serial.print() made it slow?
  2. How do I make it perform faster?
#include <SPI.h>
#include <math.h>

#define ToD(x) (x/131)
#define ToG(x) (x*9.80665/16384)

#define xAxis 0
#define yAxis 1
#define zAxis 2

#define Aoffset 0.8

//--- Settings /CS pins at pins 7, 8, and 9 ---//
const int ChipSelPin1 = 7;
const int ChipSelPin2 = 8;
const int ChipSelPin3 = 9;

int time=0;
int time_old=0;

float angle=0;
float angleX=0;
float angleY=0;
float angleZ=0;

//--- Setup for sensors ---//
void setup()
{
  Serial.begin(115200);  // start the serial communication at baud rate of 115200
  //Serial.println("MPU-6000 Data Acquisition");
    
  //--- SPI settings ---//
  //Serial.print("Initializing SPI Protocol...");
  SPI.begin();  // start the SPI library
  SPI.setClockDivider(SPI_CLOCK_DIV2);  // setting SPI at 4Mhz
  //Serial.print(".");
  SPI.setBitOrder(MSBFIRST);  // data delivered MSB first
  //Serial.print(".");
  SPI.setDataMode(SPI_MODE0);  // latched on rising edge, transitioned on falling edge, active low
  //Serial.println(".");
  //Serial.println("SPI Initialized");
  delay(100);
  
  //--- Configure the chip select pins as output ---//
  pinMode(ChipSelPin1, OUTPUT);
  pinMode(ChipSelPin2, OUTPUT);
  pinMode(ChipSelPin3, OUTPUT);
  
  ConfigureMPU6000();  // configure chip
}

void loop()
{
  time_old=time;

  Serial.println(AcceX(ChipSelPin1));
  Serial.print("           ");
  Serial.print(AcceY(ChipSelPin1));
  Serial.print("           ");
  Serial.print(AcceZ(ChipSelPin1));
  Serial.print("           ");
  
  Serial.print(AcceX(ChipSelPin2));
  Serial.print("           ");
  Serial.print(AcceY(ChipSelPin2));
  Serial.print("           ");
  Serial.print(AcceZ(ChipSelPin2));
  Serial.print("           ");
  
  Serial.print(AcceX(ChipSelPin3));
  Serial.print("           ");
  Serial.print(AcceY(ChipSelPin3));
  Serial.print("           ");
  Serial.print(AcceZ(ChipSelPin3));
  Serial.print("           ");
  
  time=micros();
  int dt=time-time_old;
  Serial.print(dt);
  Serial.println();
  
}

//***********************************************************//
//-----------------------------------------------------------//
//---------- Self-made functions for easier coding ----------//
//-----------------------------------------------------------//
//***********************************************************//

//------------------------------------------//
//--- Function for SPI writing to sensor ---//
//------------------------------------------//
void SPIwrite(byte reg, byte data, int ChipSelPin)
{
  uint8_t dump;
  digitalWrite(ChipSelPin,LOW);
  dump=SPI.transfer(reg);
  dump=SPI.transfer(data);
  digitalWrite(ChipSelPin,HIGH);
}

//--------------------------------------------//
//--- Function for SPI reading from sensor ---//
//--------------------------------------------//
uint8_t SPIread(byte reg,int ChipSelPin)
{
  uint8_t dump;
  uint8_t return_value;
  uint8_t addr=reg|0x80;
  digitalWrite(ChipSelPin,LOW);
  dump=SPI.transfer(addr);
  return_value=SPI.transfer(0x00);
  digitalWrite(ChipSelPin,HIGH);
  return(return_value);
}

//--- Functions for reading raw data ---//
int AcceX(int ChipSelPin)
{
  uint8_t AcceX_H=SPIread(0x3B,ChipSelPin);
  uint8_t AcceX_L=SPIread(0x3C,ChipSelPin);
  int16_t AcceX=AcceX_H<<8|AcceX_L;
  return(AcceX);
}

int AcceY(int ChipSelPin)
{
  uint8_t AcceY_H=SPIread(0x3D,ChipSelPin);
  uint8_t AcceY_L=SPIread(0x3E,ChipSelPin);
  int16_t AcceY=AcceY_H<<8|AcceY_L;
  return(AcceY);
}

int AcceZ(int ChipSelPin)
{
  uint8_t AcceZ_H=SPIread(0x3F,ChipSelPin);
  uint8_t AcceZ_L=SPIread(0x40,ChipSelPin);
  int16_t AcceZ=AcceZ_H<<8|AcceZ_L;
  return(AcceZ);
}

int GyroX(int ChipSelPin)
{
  uint8_t GyroX_H=SPIread(0x43,ChipSelPin);
  uint8_t GyroX_L=SPIread(0x44,ChipSelPin);
  int16_t GyroX=GyroX_H<<8|GyroX_L;
  return(GyroX);
}

int GyroY(int ChipSelPin)
{
  uint8_t GyroY_H=SPIread(0x45,ChipSelPin);
  uint8_t GyroY_L=SPIread(0x46,ChipSelPin);
  int16_t GyroY=GyroY_H<<8|GyroY_L;
  return(GyroY);
}

int GyroZ(int ChipSelPin)
{
  uint8_t GyroZ_H=SPIread(0x47,ChipSelPin);
  uint8_t GyroZ_L=SPIread(0x48,ChipSelPin);
  int16_t GyroZ=GyroZ_H<<8|GyroZ_L;
  return(GyroZ);
}

//--- Function to obtain angles based on accelerometer readings ---//
float AcceDeg(int ChipSelPin,int AxisSelect)
{
  float Ax=ToG(AcceX(ChipSelPin));
  float Ay=ToG(AcceY(ChipSelPin));
  float Az=ToG(AcceZ(ChipSelPin));
  float ADegX=((atan(Ax/(sqrt((Ay*Ay)+(Az*Az)))))/PI)*180;
  float ADegY=((atan(Ay/(sqrt((Ax*Ax)+(Az*Az)))))/PI)*180;
  float ADegZ=((atan((sqrt((Ax*Ax)+(Ay*Ay)))/Az))/PI)*180;
  switch (AxisSelect)
  {
    case 0:
    return ADegX;
    break;
    case 1:
    return ADegY;
    break;
    case 2:
    return ADegZ;
    break;
  }
}

//--- Function to obtain angles based on gyroscope readings ---//
float GyroDeg(int ChipSelPin, int AxisSelect)
{
  time_old=time;
  time=millis();
  float dt=time-time_old;
  if (dt>=1000)
  {
    dt=0;
  }
  float Gx=ToD(GyroX(ChipSelPin));
  if (Gx>0 && Gx<1.4)
  {
    Gx=0;
  }
  float Gy=ToD(GyroY(ChipSelPin));
  float Gz=ToD(GyroZ(ChipSelPin));
  angleX+=Gx*(dt/1000);
  angleY+=Gy*(dt/1000);
  angleZ+=Gz*(dt/1000);
  switch (AxisSelect)
  {
    case 0:
    return angleX;
    break;
    case 1:
    return angleY;
    break;
    case 2:
    return angleZ;
    break;
  }
}

//--- Function to initialize MPU6000 chip ---//
void ConfigureMPU6000()
{
  // DEVICE_RESET @ PWR_MGMT_1, reset device
  SPIwrite(0x6B,0x80,ChipSelPin1);
  delay(150);
  SPIwrite(0x6B,0x80,ChipSelPin2);
  delay(150);
  SPIwrite(0x6B,0x80,ChipSelPin3);
  delay(150);
  // TEMP_DIS @ PWR_MGMT_1, wake device and select GyroZ clock
  SPIwrite(0x6B,0x03,ChipSelPin1);
  delay(150);
  SPIwrite(0x6B,0x03,ChipSelPin2);
  delay(150);
  SPIwrite(0x6B,0x03,ChipSelPin3);
  delay(150);
  // I2C_IF_DIS @ USER_CTRL, disable I2C interface
  SPIwrite(0x6A,0x10,ChipSelPin1);
  delay(150);
  SPIwrite(0x6A,0x10,ChipSelPin2);
  delay(150);
  SPIwrite(0x6A,0x10,ChipSelPin3);
  delay(150);
  // SMPRT_DIV @ SMPRT_DIV, sample rate at 1000Hz
  SPIwrite(0x19,0x00,ChipSelPin1);
  delay(150);
  SPIwrite(0x19,0x00,ChipSelPin2);
  delay(150);
  SPIwrite(0x19,0x00,ChipSelPin3);
  delay(150);
  // DLPF_CFG @ CONFIG, digital low pass filter at 42Hz
  SPIwrite(0x1A,0x03,ChipSelPin1);
  delay(150);
  SPIwrite(0x1A,0x03,ChipSelPin2);
  delay(150);
  SPIwrite(0x1A,0x03,ChipSelPin3);
  delay(150);
  // FS_SEL @ GYRO_CONFIG, gyro scale at 250dps
  SPIwrite(0x1B,0x00,ChipSelPin1);
  delay(150);
  SPIwrite(0x1B,0x00,ChipSelPin2);
  delay(150);
  SPIwrite(0x1B,0x00,ChipSelPin3);
  delay(150);
  // AFS_SEL @ ACCEL_CONFIG, accel scale at 2g (1g=8192)
  SPIwrite(0x1C,0x00,ChipSelPin1);
  delay(150);
  SPIwrite(0x1C,0x00,ChipSelPin2);
  delay(150);
  SPIwrite(0x1C,0x00,ChipSelPin3);
  delay(150);
}
  1. I used micros() to obtain the time delay with each loop, and found out that it is below 100Hz (I am expecting faster since it is a 8Mhz processor, and sensor is giving 1000Hz of readings). I need the Serial.print() cycle for every loop to be at least 100Hz. Is my method correct?

You are wasting a lot of time converting the int values to strings, sending the strings, and (on the other end) converting the strings back to values.

Using Serial.write() to send the high byte and then the low byte will be faster. A lot faster.

Could it be that I am polling my sensors fast enough, but Serial.print() made it slow?

Exactly.

Hi PaulS,

Thank you very much for your help.
I understand what you meant, but I am unfamiliar with the environment.

Could you please show me an example of how should I modify the code, so I can move on from there?
Thanks!

  Serial.println(AcceX(ChipSelPin1));

becomes

int val = AcceX(ChipSelPin1);
Serial.write(hightByte(val));
Serial.write(lowByte(val));

You'll need some kind of separator between the pairs of bytes, so that if one gets lost, the receiver can know that and recover.

Or, you need some kind of separator between groups of bytes, and discard the whole group if the separator occurs again to soon, in the input stream.

Since you have no byte values that would not be part of the data stream, you have a couple of choices. One would be to split each byte into two nibbles, and send the 4 nibbles. Then, the separator can be any value over 127.

The other would be to send a series of (say 4) separator bytes (all the same value). That series of bytes is unlikely to appear in your data. That series of bytes could be 0s or 255s.

I see. But in such case, should I expect to see the same thing on serial monitor (as in, Serial.print shows the actual value, would Serial.write shows the same)?

Thank you.

yan5619:
Could it be that I am polling my sensors fast enough, but Serial.print() made it slow?

That's a good question. Why don't you remove the print statements and just call the Acce*() functions without printing the results, and see what effect that has on the speed?

PaulS:

  Serial.println(AcceX(ChipSelPin1));

becomes

int val = AcceX(ChipSelPin1);

Serial.write(hightByte(val));
Serial.write(lowByte(val));




You'll need some kind of separator between the pairs of bytes, so that if one gets lost, the receiver can know that and recover.

Or, you need some kind of separator between groups of bytes, and discard the whole group if the separator occurs again to soon, in the input stream.

Since you have no byte values that would not be part of the data stream, you have a couple of choices. One would be to split each byte into two nibbles, and send the 4 nibbles. Then, the separator can be any value over 127.

The other would be to send a series of (say 4) separator bytes (all the same value). That series of bytes is unlikely to appear in your data. That series of bytes could be 0s or 255s.

May I just use space (that is, Serial.write(040)) to be the seperator?
In such case, would it happen that my data from the sensor to be the same as some special ascii code, such as new line or carriage return?

The receiving side will be Processing running from my computer.

PeterH:

yan5619:
Could it be that I am polling my sensors fast enough, but Serial.print() made it slow?

That's a good question. Why don't you remove the print statements and just call the Acce*() functions without printing the results, and see what effect that has on the speed?

Thank you very much. I've tried it, and yes, Serial.print is exactly why it is that slow =)

May I just use space (that is, Serial.write(040)) to be the seperator?

As long as you can ensure that the high order byte and the low order byte of any integer will never be 32, 0x20, 040, you can.

Of course, I don't see how you can ensure that, given that the values come from accelerometer data.

Is there any reason that the data can't be logged locally, and displayed later? This would be far faster than trying to send data over the serial port.

PaulS:

May I just use space (that is, Serial.write(040)) to be the seperator?

As long as you can ensure that the high order byte and the low order byte of any integer will never be 32, 0x20, 040, you can.

Of course, I don't see how you can ensure that, given that the values come from accelerometer data.

Is there any reason that the data can't be logged locally, and displayed later? This would be far faster than trying to send data over the serial port.

Thank you for your swift reply.
I need a real time monitoring GUI, that is why I am streaming the data over the serial port (via Bluetooth) into Processing (and then plotted into a graph).

If that is the case, would Serial.write() become a less feasible method?

Thanks!

yan5619:
Thank you for your swift reply.
I need a real time monitoring GUI, that is why I am streaming the data over the serial port (via Bluetooth) into Processing (and then plotted into a graph).

The term 'real time' means different things to different people. What does it mean to you? What sort of update frequency and latency are you trying to achieve? How much data needs to be transferred at each update? If you are only displaying the values on a screen then you can probably afford to update them considerably slower than 100Hz. In that case you might want to separate the sensor handling code from the output code so that you can perform them at different frequencies - you probably want to do your inertial extrapolation as frequently as possible to minimise cumulative errors and you don't need to pass all that data through the bottleneck of your serial port.

PeterH:

yan5619:
Thank you for your swift reply.
I need a real time monitoring GUI, that is why I am streaming the data over the serial port (via Bluetooth) into Processing (and then plotted into a graph).

The term 'real time' means different things to different people. What does it mean to you? What sort of update frequency and latency are you trying to achieve? How much data needs to be transferred at each update? If you are only displaying the values on a screen then you can probably afford to update them considerably slower than 100Hz. In that case you might want to separate the sensor handling code from the output code so that you can perform them at different frequencies - you probably want to do your inertial extrapolation as frequently as possible to minimise cumulative errors and you don't need to pass all that data through the bottleneck of your serial port.

Please allow me to explain my project.

It is a wireless inertial sensor network as mentioned on the first post, with three acce/gyro sensors.
I need a data sampling rate of at least 100Hz, best if 200Hz can be achieved.
After collecting the data from the sensor, it should be streamed to my computer (which within my limited knowledge, serial port would be the way), and displayed on the screen by plotting a graph (by using Processing, the only program I am a little familiar with). Furthermore, for analysis purposes, the data that is streamed to my computer must be logged, therefore 100Hz-200Hz is still necessary.

The frequency will be fixed, so any frequency from 100Hz will be fine.

Finally, each sensor will output 6 sets of 2 bytes data (ax,ay,az,gx,gy,gz).

Thank you.

Finally, each sensor will output 6 sets of 2 bytes data (ax,ay,az,gx,gy,gz).

Sending 12 bytes of data 100 times per second is possible. Error detection/correction will require that you send more than 12 bytes at a time. How many more depends on how robust the error detection must be, how reliable the wireless transmitter and receiver are, and how much interference there will be.

I'm currently doing a project titled "Inertial Sensor Network for Gait Analysis",

I can't see where "wireless" is a necessary condition.

  1. $6 Bluetooth serial

You might want to rethink this.

PaulS:
Sending 12 bytes of data 100 times per second is possible. Error detection/correction will require that you send more than 12 bytes at a time. How many more depends on how robust the error detection must be, how reliable the wireless transmitter and receiver are, and how much interference there will be.

Well, 12 bytes per sensor, I'll have to send up to 36 bytes (since I used three), and furthermore I think I need more bytes for instructions such as spaces, CR and NL.
What I am concerned is that if I am going to use spaces, CR and NL (and then get the data sent to Processing and processed), it is possible for my sensors to produce such values as well, and "confuse" Processing...am I right?

I can't see where "wireless" is a necessary condition.

It's supposed to be a wearable device, which will be attached on the foot, and track the acceleration, angular velocity changes, as well as angles of the foot when walking. It'd be very troublesome if it is not wireless.

You might want to rethink this.

Surprisingly, even though it is cheap, it is very reliable, as reviewed by many users and tested by myself.

Thank you so much for your help.

What I am concerned is that if I am going to use spaces, CR and NL

The problem with any delimiter, when you are sending binary data, is that the delimiter can naturally be part of the data stream. So, seeing a 10 in the data stream does not necessarily mean that a CR was intended.

If can be assured that no data will ever be lost, then simply assume that 12 bytes are all the data for one sensor, and interpret the 12 bytes as 6 ints.

You can't, of course, assume that no data will ever be lost.

It'd be very troublesome if it is not wireless.

How is the wearer of the device supposed to see the output? Is the wearer supposed to cart a laptop around?

If the device is intended to be applied and the output studied in a clinical setting, wired is not necessarily out of the question.

Surprisingly, even though it is cheap, it is very reliable, as reviewed by many users and tested by myself.

It is only as good as the error detection and correction is applies. That is none. That is not what bluetooth is intended for.

XBees, on the other hand, do perform error detection and correction, and packet the data in a way that start and end markers (where a packet starts and ends) are present, so one can be assured that a packet does contain exactly 12 bytes in the correct order.

Of course, you can't get two XBees, a shield, and a USB explorer for $6, but you can't expect to get something for nothing. TANSTAAFL.

Hi Yan,

A long shot as this was quite some time ago, but very interested in how you got on with accessing your values from the MPU6000?

Did you have to amend your sketch much from the initial one posted?

Many thanks.

Again, this is a a fairly old thread, but Sebastian Madgwick's work on gait tracking with an IMU is certainly worth a look: Gait tracking with x-IMU – x-io Technologies

Hello everyone

I am working with mpu6000, feather m0 ada ble (CPU). I applied the code for taking data as Yan posted I above and put data to the code was takenc here: Gait tracking with x-IMU – x-io Technologies. However the result is not the same. I don’t have much experience. I would like to ask you all if you have examples to share : reading data from IMU6000, analyzing Euler angle, filter, and connection with Bluetooth. Many thanks