# Reading and then applying serial data from 9DOF Razor IMU AHRS

Hi,

I’ve got a 9DOF Razor IMU AHRS hooked up to an Arduino (ATmega 328), and I’m able to read it’s serial data in the serial monitor. It looks like this -

``````!ANG:-11.35,-8.51,14.78
!ANG:-13.21,-9.75,14.29
!ANG:-14.73,-11.15,13.33
!ANG:-15.83,-12.74,11.84
!ANG:-16.71,-14.07,10.29
!ANG:-17.37,-14.93,8.96
!ANG:-18.29,-15.52,8.11
!ANG:-19.28,-16.12,7.46
!ANG:-19.40,-15.98,6.56
!ANG:-18.80,-15.27,5.64
``````

I’m using the AHRS code from here - http://code.google.com/p/sf9domahrs/

More specifically, this -

``````#include <Wire.h>

// ADXL345 Sensitivity(from datasheet) => 4mg/LSB   1G => 1000mg/4mg = 256 steps
// Tested value : 248
#define GRAVITY 248  //this equivalent to 1G in the raw data coming from the accelerometer
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square

#define ToDeg(x) (x*57.2957795131)  // *180/pi

// LPR530 & LY530 Sensitivity (from datasheet) => (3.3mv at 3v)at 3.3v: 3mV/º/s, 3.22mV/ADC step => 0.93
// Tested values : 0.92
#define Gyro_Gain_X 0.92 //X axis Gyro gain
#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
#define Gyro_Gain_Z 0.92 //Z axis Gyro gain

#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002

/*For debugging purposes*/
//OUTPUTMODE=1 will print the corrected data,
//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
#define OUTPUTMODE 1

//#define PRINT_DCM 0     //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw

#define STATUS_LED 13

int8_t sensors = {1,2,0};  // Map the ADC channels gyro_x, gyro_y, gyro_z
int SENSOR_SIGN = {-1,1,-1,1,1,1,-1,-1,-1};  //Correct directions x,y,z - gyros, accels, magnetormeter

float G_Dt=0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible

long timer=0;   //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print values
int AN; //array that store the 3 ADC filtered data (gyros)
int AN_OFFSET={0,0,0,0,0,0}; //Array that stores the Offset of the sensors
int ACC;          //array that store the accelerometers data

int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;

float Accel_Vector= {0,0,0}; //Store the acceleration in a vector
float Gyro_Vector= {0,0,0};//Store the gyros turn rate in a vector
float Omega_Vector= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P= {0,0,0};//Omega Proportional correction
float Omega_I= {0,0,0};//Omega Integrator
float Omega= {0,0,0};

// Euler angles
float roll;
float pitch;
float yaw;

float errorRollPitch= {0,0,0};
float errorYaw= {0,0,0};

unsigned int counter=0;
byte gyro_sat=0;

float DCM_Matrix= {
{
1,0,0  }
,{
0,1,0  }
,{
0,0,1  }
};
float Update_Matrix={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here

float Temporary_Matrix={
{
0,0,0  }
,{
0,0,0  }
,{
0,0,0  }
};

volatile uint8_t MuxSel=0;
volatile uint8_t analog_reference;
volatile uint16_t analog_buffer;
volatile uint8_t analog_count;

void setup()
{
Serial.begin(57600);
pinMode (STATUS_LED,OUTPUT);  // Status LED

Analog_Reference(DEFAULT);
Analog_Init();
I2C_Init();
Accel_Init();

Serial.println("Sparkfun 9DOF Razor AHRS");

digitalWrite(STATUS_LED,LOW);
delay(1500);

// Magnetometer initialization
Compass_Init();

delay(20);

for(int i=0;i<32;i++)    // We take some readings...
{
for(int y=0; y<6; y++)   // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
}

for(int y=0; y<6; y++)
AN_OFFSET[y] = AN_OFFSET[y]/32;

AN_OFFSET-=GRAVITY*SENSOR_SIGN;

//Serial.println("Offset:");
for(int y=0; y<6; y++)
Serial.println(AN_OFFSET[y]);

delay(2000);
digitalWrite(STATUS_LED,HIGH);

timer=millis();
delay(20);
counter=0;
}

void loop() //Main Loop
{
if((millis()-timer)>=20)  // Main loop runs at 50Hz
{
counter++;
timer_old = timer;
timer=millis();
if (timer>timer_old)
G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0;

// *** DCM algorithm

if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
{
counter=0;
}

// Calculations...
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// ***

printdata();

//Turn off the LED when you saturate any of the gyros.
{
if (gyro_sat<50)
gyro_sat+=10;
}
else
{
if (gyro_sat>0)
gyro_sat--;
}

if (gyro_sat>0)
digitalWrite(STATUS_LED,LOW);
else
digitalWrite(STATUS_LED,HIGH);

}

}
``````

What I need to do is insert an ‘if’ statement into this code (I want to be able to trigger a Speakjet speech synth based on the movements of a dancer wearing the accelerometer - more or less random phenomes. A human machine cyborg hybrid generating a hybrid language). For example, at the very basic level, I’m starting with this -

`````` {
if (accel_x < 40) // accel x data

{pinMode(txPin, OUTPUT);

SpeakJetSerial.begin(9600); //setup software serial port to connect to speakjet

char SayIt[] = {189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006}; // makes speak jet say "Shit! Shit! Shit!

SpeakJetSerial.println(SayIT); // send to speakjet
}

}
``````

But the problem is, whenever I alter the 9DOF code, I end up with errors like these -

“Speak___9DOF:144: error: ‘Analog_Reference’ was not declared in this scope
Speak___9DOF:145: error: ‘Analog_Init’ was not declared in this scope
Speak___9DOF:146: error: ‘I2C_Init’ was not declared in this scope”

For these parts of the original code (which worked fine before)-

Analog_Reference(DEFAULT);
Analog_Init();
I2C_Init();

I’m still very new to writing code, and I don’t quite understand what a lot of the AHRS code is for, or how to interpret it. How come the code works fine before I add anything, and then when I add something without any flaws, parts of the original code suddenly have error messages?

And, does anyone have general suggestions for harvesting the data coming in from the 9DOF and using it to trigger functions in the Speakjet (for example)?

Thanks for any suggestions. I hope this post is clear enough.

Post your entire modified code - perhaps its something simple like you just have things put in the wrong section,or are missing a ) or } somewhere. Can't tell from you snippet.

Here it is - just trying to start real simple.

``````// Sparkfveun 9DOF Razor IMU AHRS
// 9 Degree of Measurement Attitude and Heading Reference System
// Firmware v1.0
//
// Released under Creative Commons License
// Code by Doug Weibel and Jose Julio
// Based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel

// Axis definition:
// X axis pointing forward (to the FTDI connector)
// Y axis pointing to the right
// and Z axis pointing down.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise

/* Hardware version - v13

ATMega328@3.3V w/ external 8MHz resonator
High Fuse DA
Low Fuse FF

HMC5843: Magnetometer
LY530:	Yaw Gyro
LPR530:	Pitch and Roll Gyro

Programmer : 3.3v FTDI
Arduino IDE : Select board  "Arduino Duemilanove w/ATmega328"
*/
// This code works also on ATMega168 Hardware

#include <Wire.h>

// ADXL345 Sensitivity(from datasheet) => 4mg/LSB   1G => 1000mg/4mg = 256 steps
// Tested value : 248
#define GRAVITY 248  //this equivalent to 1G in the raw data coming from the accelerometer
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square

#define ToDeg(x) (x*57.2957795131)  // *180/pi

// LPR530 & LY530 Sensitivity (from datasheet) => (3.3mv at 3v)at 3.3v: 3mV/º/s, 3.22mV/ADC step => 0.93
// Tested values : 0.92
#define Gyro_Gain_X 0.92 //X axis Gyro gain
#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
#define Gyro_Gain_Z 0.92 //Z axis Gyro gain

#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002

/*For debugging purposes*/
//OUTPUTMODE=1 will print the corrected data,
//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
#define OUTPUTMODE 1

//#define PRINT_DCM 0     //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 1   //Will print the Euler angles Roll, Pitch and Yaw

#define STATUS_LED 13

#define txPin 2
#define rxPin 3
#include <SoftwareSerial.h>

SoftwareSerial SpeakJetSerial =  SoftwareSerial(rxPin, txPin);

int8_t sensors = {1,2,0};  // Map the ADC channels gyro_x, gyro_y, gyro_z
int SENSOR_SIGN = {-1,1,-1,1,1,1,-1,-1,-1};  //Correct directions x,y,z - gyros, accels, magnetormeter

float G_Dt=0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible

long timer=0;   //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print values
int AN; //array that store the 3 ADC filtered data (gyros)
int AN_OFFSET={0,0,0,0,0,0}; //Array that stores the Offset of the sensors
int ACC;          //array that store the accelerometers data

int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;

float Accel_Vector= {0,0,0}; //Store the acceleration in a vector
float Gyro_Vector= {0,0,0};//Store the gyros turn rate in a vector
float Omega_Vector= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P= {0,0,0};//Omega Proportional correction
float Omega_I= {0,0,0};//Omega Integrator
float Omega= {0,0,0};

// Euler angles
float roll;
float pitch;
float yaw;

float errorRollPitch= {0,0,0};
float errorYaw= {0,0,0};

unsigned int counter=0;
byte gyro_sat=0;

float DCM_Matrix= {
{
1,0,0  }
,{
0,1,0  }
,{
0,0,1  }
};
float Update_Matrix={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here

float Temporary_Matrix={
{
0,0,0  }
,{
0,0,0  }
,{
0,0,0  }
};

volatile uint8_t MuxSel=0;
volatile uint8_t analog_reference;
volatile uint16_t analog_buffer;
volatile uint8_t analog_count;

void setup()
{
Serial.begin(57600);
pinMode (STATUS_LED,OUTPUT);  // Status LED

Analog_Reference(DEFAULT);
Analog_Init();
I2C_Init();
Accel_Init();

Serial.println("Sparkfun 9DOF Razor AHRS");

digitalWrite(STATUS_LED,LOW);
delay(1500);

// Magnetometer initialization
Compass_Init();

delay(20);

for(int i=0;i<32;i++)    // We take some readings...
{
for(int y=0; y<6; y++)   // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
}

for(int y=0; y<6; y++)
AN_OFFSET[y] = AN_OFFSET[y]/32;

AN_OFFSET-=GRAVITY*SENSOR_SIGN;

//Serial.println("Offset:");
for(int y=0; y<6; y++)
Serial.println(AN_OFFSET[y]);

delay(2000);
digitalWrite(STATUS_LED,HIGH);

timer=millis();
delay(20);
counter=0;
}

void loop() //Main Loop
{
if((millis()-timer)>=20)  // Main loop runs at 50Hz
{
counter++;
timer_old = timer;
timer=millis();
if (timer>timer_old)
G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0;

// *** DCM algorithm

if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
{
counter=0;
}

// Calculations...
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// ***

printdata();

//Turn off the LED when you saturate any of the gyros.
{
if (gyro_sat<50)
gyro_sat+=10;
}
else
{
if (gyro_sat>0)
gyro_sat--;
}

if (gyro_sat>0)
digitalWrite(STATUS_LED,LOW);
else
digitalWrite(STATUS_LED,HIGH);

}

{
if (accel_x < 40) // accel x data

{pinMode(txPin, OUTPUT);

SpeakJetSerial.begin(9600); //setup software serial port to connect to speakjet

char Shit[] = {189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006};
SpeakJetSerial.println(Shit);
}

}
``````
``````  {
if (accel_x < 40) // accel x data

{pinMode(txPin, OUTPUT);

SpeakJetSerial.begin(9600); //setup software serial port to connect to speakjet

char Shit[] = {189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006, 189, 129, 191, 006};
SpeakJetSerial.println(Shit);
}
``````

What is the first { for? The { defines a block of code, and is typically used after an if, switch, for, or do statement, not placed randomly in the code.

This is your new code. Count how many { and how many } there are.

The SpeakJetSerial.begin() statement does not belong here. Why would you want to initialize the communications channel every time you want to say something. Move that to setup() so it happens once.

Ok, thanks. That makes perfect sense. I made those changes, but I still get these errors -

Speak_9DOF:146: error: 'Analog_Reference' was not declared in this scope Speak_9DOF:147: error: 'Analog_Init' was not declared in this scope

• many more

from the ORIGINAL accelorometer code. Wondering why this only happens when I change some code somewhere else?

If you select Tools: Autoformat, it will tell you have too many left curly brace. That's a good test there. If you go thru your code and click the cursor next to ) and }, then you can use the vertical scroll bar to see where the mating ( & { are. Make these match up and are defining what you think is being defined.

Ok, thanks. The error messages I’m getting are all in relation to these portions of the orginal code -

Analog_Reference(DEFAULT);
Analog_Init();
I2C_Init();
Accel_Init();